interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add logging

This commit is contained in:
interfisch 2021-03-20 21:20:17 +01:00
parent 3b8ea2bd3a
commit 27e7863d5e
1 changed files with 90 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
controller_teensy/src/main.cpp
View File

@ -15,9 +15,13 @@ Tennsy Pin, Pin Name, Connected to
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
#define SERIAL_LOG_BAUD 115200 // baud rate for logging output
bool log_update=true;
unsigned long last_log_send=0;
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
#define RECEIVEPERIOD 50 //ms
bool controllers_connected=false;
#define PIN_THROTTLE A7
const uint16_t calib_throttle_min = 400; //better a bit too high than too low
@ -46,8 +50,7 @@ unsigned long last_adcread=0;
unsigned long last_send = 0;
unsigned long last_receive = 0;
float avg_currentL=0;
float avg_currentR=0;
float filtered_currentAll=0;
int16_t cmd_send=0;
int16_t last_cmd_send=0;
@ -102,6 +105,9 @@ typedef struct{
uint8_t cur_pos=0;
int16_t cmdL=0;
int16_t cmdR=0;
float filtered_curL=0;
float filtered_curR=0;
unsigned long millis=0; //time when last message received
} MotorParameter;
MotorParameter motorparamsFront;
MotorParameter motorparamsRear;
@ -113,6 +119,9 @@ bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
int sort_desc(const void *cmp1, const void *cmp2);
float filterMedian(int16_t* values);
void writeLogHeader(HardwareSerial &SerialRef);
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake);
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
{
// Create command
@ -194,10 +203,12 @@ bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
void setup()
{
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial.begin(SERIAL_BAUD); //Debug and Program
Serial2.begin(SERIAL_CONTROL_BAUD); //control
Serial3.begin(SERIAL_CONTROL_BAUD); //control
Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0
Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7
pinMode(PIN_THROTTLE, INPUT);
pinMode(PIN_BRAKE, INPUT);
@ -242,14 +253,24 @@ void loop() {
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
throttle_pos/=2;
digitalWrite(PIN_MODE_LEDG,HIGH); //Green
digitalWrite(PIN_MODE_LEDR,LOW);
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
digitalWrite(PIN_MODE_LEDR,HIGH);
}else{ //button not pushed in
digitalWrite(PIN_MODE_LEDG,LOW);
digitalWrite(PIN_MODE_LEDR,HIGH); //Red
digitalWrite(PIN_MODE_LEDG,HIGH);
digitalWrite(PIN_MODE_LEDR,LOW); //Red
}
}
#define FEEDBACKRECEIVETIMEOUT 500
if ( (loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT) | (loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT) ) { //timeout of at least one controller
throttle_pos=0;
brake_pos=0;
controllers_connected=false;
}else if(!controllers_connected) { //not timeouted but was before
controllers_connected=true;
writeLogHeader(Serial1); //connection recovered, write log header
}
if (newData2) {
@ -257,12 +278,16 @@ void loop() {
motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
motorparamsFront.millis=loopmillis;
log_update=true;
}
if (newData3) {
motorparamsRear.cur_pos++;
motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
motorparamsRear.curL_DC[motorparamsRear.cur_pos] = FeedbackRear.curL_DC;
motorparamsRear.curR_DC[motorparamsRear.cur_pos] = FeedbackRear.curR_DC;
motorparamsRear.millis=loopmillis;
log_update=true;
}
if (loopmillis - last_send > SENDPERIOD) {
@ -274,16 +299,16 @@ void loop() {
float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
float freewheel_current=0.1+brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
float filtered_curRL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
float filtered_curRR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
float filtered_currentFront=min(filtered_curFL,filtered_curFR);
float filtered_currentRear=min(filtered_curRL,filtered_curRR);
float filtered_currentFront=min(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
float filtered_currentRear=min(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
float filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
if (throttle_pos>=last_cmd_send) { //accelerating
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
@ -306,9 +331,17 @@ void loop() {
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000); Serial.println();
log_update=true;
//Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000); Serial.println();
}
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
if (log_update && loopmillis>last_log_send+LOGMININTERVAL) {
last_log_send=loopmillis;
log_update=false;
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
}
}
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
@ -333,4 +366,42 @@ float filterMedian(int16_t* values) {
mean/=(1+CURRENT_MEANVALUECOUNT*2);
return mean;
}
void writeLogHeader(HardwareSerial &SerialRef) {
SerialRef.print("time, cmd_FrontL, cmd_FrontR, cmd_RearL, cmd_RearR, ");
SerialRef.print("current_FrontL, current_FrontR, current_RearL, current_RearR, ");
SerialRef.print("temp_Front, temp_Rear, vbat_Front, vbat_Rear, ");
SerialRef.print("speed_FrontL, speed_FrontR, speed_RearL, speed_RearR, ");
SerialRef.print("currentAll, throttle, brake"); Serial.println();
}
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
{
SerialRef.print(time/1000.0); SerialRef.print(", "); //time in seconds
SerialRef.print(mpfront.cmdL); SerialRef.print(", ");
SerialRef.print(mpfront.cmdR); SerialRef.print(", ");
SerialRef.print(mprear.cmdL); SerialRef.print(", ");
SerialRef.print(mprear.cmdR); SerialRef.print(", ");
SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(", ");
SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(", ");
SerialRef.print(mprear.filtered_curL,3); SerialRef.print(", ");
SerialRef.print(mprear.filtered_curR,3); SerialRef.print(", ");
SerialRef.print(fbfront.speedL_meas); SerialRef.print(", ");
SerialRef.print(fbfront.speedR_meas); SerialRef.print(", ");
SerialRef.print(fbrear.speedL_meas); SerialRef.print(", ");
SerialRef.print(fbrear.speedR_meas); SerialRef.print(", ");
SerialRef.print(fbfront.boardTemp); SerialRef.print(", ");
SerialRef.print(fbrear.boardTemp); SerialRef.print(", ");
SerialRef.print(fbfront.batVoltage); SerialRef.print(", ");
SerialRef.print(fbrear.batVoltage); SerialRef.print(", ");
SerialRef.print(currentAll,3); SerialRef.print(", ");
SerialRef.print(throttle); SerialRef.print(", ");
SerialRef.print(brake); SerialRef.println();
}