add logging
This commit is contained in:
parent
3b8ea2bd3a
commit
27e7863d5e
|
@ -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();
|
||||
}
|
Loading…
Reference in New Issue