From 1e9eda245ffa614f120f473e2a583a71d5bdedf6 Mon Sep 17 00:00:00 2001 From: Fisch Date: Wed, 31 Mar 2021 21:16:48 +0200 Subject: [PATCH] move functions in loop to own functions --- controller_teensy/src/main.cpp | 230 ++++++++++++++++++++------------- 1 file changed, 139 insertions(+), 91 deletions(-) diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 9be85f4..cf76e94 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -26,6 +26,11 @@ unsigned long last_log_send=0; #define WRITE_HEADER_TIME 1000 bool log_header_written = false; + +#define FEEDBACKRECEIVETIMEOUT 500 + +bool controllerFront_connected=false; +bool controllerRear_connected=false; bool controllers_connected=false; #define PIN_THROTTLE A7 @@ -126,6 +131,12 @@ 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 writeLogComment(HardwareSerial &SerialRef, String msg); + +void readADC(); +void failChecks(); +void sendCMD(); +void checkLog(); void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) { @@ -241,42 +252,14 @@ void loop() { bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3); - if (loopmillis - last_adcread > ADCREADPERIOD) { - //read teensy adc and filter + if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter last_adcread=loopmillis; - uint16_t throttle_raw = analogRead(PIN_THROTTLE); - throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain - uint16_t brake_raw = analogRead(PIN_BRAKE); - brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain - - int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake - throttle_pos=constrain(throttlebreak_pos,0,1000); - brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos - - //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", "); - //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println(); - - if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected - throttle_pos/=2; - digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on - digitalWrite(PIN_MODE_LEDR,HIGH); - }else{ //button not pushed in - digitalWrite(PIN_MODE_LEDG,HIGH); - digitalWrite(PIN_MODE_LEDR,LOW); //Red - } - + readADC(); } - #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; - } - + failChecks(); + @@ -297,71 +280,15 @@ void loop() { log_update=true; } - if (loopmillis - last_send > SENDPERIOD) { - //Calculate motor stuff and send to motor controllers + if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers last_send=loopmillis; - - int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle - #define MAXBREAKCURRENT 5 - 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=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=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR); - float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR); - - filtered_currentAll=max(filtered_currentFront,filtered_currentRear); - - if (throttle_pos>=last_cmd_send) { //accelerating - cmd_send = throttle_pos; //if throttle higher than apply throttle directly - }else{ //freewheeling or braking - if (filtered_currentAll=WRITE_HEADER_TIME){ - writeLogHeader(Serial1); //connection recovered, write log header - log_header_written=true; - } - - if ((log_header_written && log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) { - last_log_send=loopmillis; - log_update=false; - writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos); - } + checkLog(); } int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort @@ -424,4 +351,125 @@ 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(); +} + +void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) +{ + SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println(); +} + + + +void readADC() { + uint16_t throttle_raw = analogRead(PIN_THROTTLE); + throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain + uint16_t brake_raw = analogRead(PIN_BRAKE); + brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain + + int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake + throttle_pos=constrain(throttlebreak_pos,0,1000); + brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos + + //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", "); + //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println(); + + if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected + throttle_pos/=2; + digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on + digitalWrite(PIN_MODE_LEDR,HIGH); + }else{ //button not pushed in + digitalWrite(PIN_MODE_LEDG,HIGH); + digitalWrite(PIN_MODE_LEDR,LOW); //Red + } +} + +void failChecks() { + if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected + if (controllerFront_connected) { //just got disconnected + controllerFront_connected=false; + writeLogComment(Serial1,loopmillis, "Controller Front feedback timeout"); + } + }else if(!controllerFront_connected) { //not timeouted but was before + controllerFront_connected=true; + writeLogComment(Serial1,loopmillis, "Controller Front connected"); + } + + if ( loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected + if (controllerRear_connected) { //just got disconnected + controllerRear_connected=false; + writeLogComment(Serial1,loopmillis, "Controller Rear feedback timeout"); + } + }else if(!controllerRear_connected) { //not timeouted but was before + controllerRear_connected=true; + writeLogComment(Serial1,loopmillis, "Controller Rear connected"); + } + + controllers_connected=controllerFront_connected & controllerRear_connected; + + if (!controllers_connected) { //controllers not available + throttle_pos=0; + brake_pos=0; + } +} + +void sendCMD() { + int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle + #define MAXBREAKCURRENT 5 + 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=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=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR); + float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR); + + filtered_currentAll=max(filtered_currentFront,filtered_currentRear); + + if (throttle_pos>=last_cmd_send) { //accelerating + cmd_send = throttle_pos; //if throttle higher than apply throttle directly + }else{ //freewheeling or braking + if (filtered_currentAll=WRITE_HEADER_TIME){ + writeLogHeader(Serial1); //connection recovered, write log header + log_header_written=true; + } + + if ((log_header_written && log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) { + last_log_send=loopmillis; + log_update=false; + writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos); + } } \ No newline at end of file