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

add speed and trip distance log

This commit is contained in:
interfisch 2021-05-28 18:24:14 +02:00
parent da09098a14
commit cc74c5ac63
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
controller_teensy/src/main.cpp
View File

@ -57,6 +57,14 @@ unsigned long brake_ok_time=0;
bool error_throttle_outofrange=false;
bool error_brake_outofrange=false;
unsigned long last_feedback=0; //time millis at last feedback receive
float meanSpeedms=0;
float trip=0; //trip distance in meters
float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi
#define PIN_START A9
#define PIN_LED_START 2 //Enginge start led
@ -285,6 +293,13 @@ void loop() {
updateMotorparams(motorparamsRear,FeedbackRear);
}
if (newData2 || newData3) { //when new data arrived from one controller
float _meanRPM=FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas;
float meanSpeedms=_meanRPM*wheelcircumference/60.0; // Units: 1/min * m / 60s
trip+=abs(meanSpeedms)* (millis()-last_feedback) / 1000.0;
last_feedback=millis();
}
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
last_adcread=loopmillis;
@ -341,9 +356,9 @@ float filterMedian(int16_t* values) {
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("speed_FrontL,speed_FrontR,speed_RearL,speed_RearR,");
SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
SerialRef.println("currentAll,throttle,brake");
SerialRef.println("currentAll,throttle,brake,speed,trip");
}
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
@ -371,7 +386,9 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
SerialRef.print(currentAll,3); SerialRef.print(","); //invert. positive current is drive current
SerialRef.print(throttle); SerialRef.print(",");
SerialRef.print(brake); SerialRef.println();
SerialRef.print(brake); SerialRef.print(",");
SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
SerialRef.print(trip); SerialRef.println(); //in m
}
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)