From 3f78a5b8f24504b9bf7435ecd9b24e5df9246b7c Mon Sep 17 00:00:00 2001 From: Fisch Date: Thu, 2 Mar 2023 21:18:48 +0100 Subject: [PATCH] separate files --- controller_teensy/include/comms.h | 162 ++++++++++ controller_teensy/include/definitions.h | 124 ++++++++ controller_teensy/include/helpfunctions.h | 34 +++ controller_teensy/include/structs.h | 60 ++++ controller_teensy/src/main.cpp | 349 +--------------------- 5 files changed, 391 insertions(+), 338 deletions(-) create mode 100644 controller_teensy/include/comms.h create mode 100644 controller_teensy/include/definitions.h create mode 100644 controller_teensy/include/helpfunctions.h create mode 100644 controller_teensy/include/structs.h diff --git a/controller_teensy/include/comms.h b/controller_teensy/include/comms.h new file mode 100644 index 0000000..ebb890e --- /dev/null +++ b/controller_teensy/include/comms.h @@ -0,0 +1,162 @@ +#ifndef _COMMS_H_ +#define _COMMS_H_ + + +#include "definitions.h" +#include "structs.h" + +void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef); +bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef); + +void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis); + +void writeLogInfo(HardwareSerial &SerialRef); +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, unsigned long time, String msg); + + +void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) +{ + // Create command + scom.start = (uint16_t)START_FRAME; + scom.speedLeft = (int16_t)uSpeedLeft; + scom.speedRight = (int16_t)uSpeedRight; + scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight); + + SerialRef.write((uint8_t *) &scom, sizeof(scom)); + +} + +bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef) +{ + bool _result=false; //return true if new full data frame received + // Check for new data availability in the Serial buffer + if ( SerialRef.available() ) { + sread.incomingByte = SerialRef.read(); // Read the incoming byte + sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame + } + else { + return false; //nothing new + } + + // If DEBUG_RX is defined print all incoming bytes + #ifdef DEBUG_RX + Serial.print(sread.incomingByte); + #endif + + // Copy received data + if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected + sread.p = (byte *)&NewFeedback; + *sread.p++ = sread.incomingBytePrev; + *sread.p++ = sread.incomingByte; + sread.idx = 2; + } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data + *sread.p++ = sread.incomingByte; + sread.idx++; + } + + // Check if we reached the end of the package + if (sread.idx == sizeof(SerialFeedback)) { + uint16_t checksum; + + checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 + ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed); + + // Check validity of the new data + if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { + // Copy the new data + memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); + sread.lastValidDataSerial_time = millis(); + _result=true; + } else { + _result=false; + } + sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) + } + /* + // Print data to built-in Serial + Serial.print("1: "); Serial.print(Feedback.cmd1); + Serial.print(" 2: "); Serial.print(Feedback.cmd2); + Serial.print(" 3: "); Serial.print(Feedback.speedR); + Serial.print(" 4: "); Serial.print(Feedback.speedL); + Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); + Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); + Serial.print(" 7: "); Serial.print(Feedback.batVoltage); + Serial.print(" 8: "); Serial.println(Feedback.boardTemp); + } else { + Serial.println("Non-valid data skipped"); + }*/ + + // Update previous states + sread.incomingBytePrev = sread.incomingByte; + + return _result; //new data was available +} + + + +void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis) { + mp.cur_pos++; + mp.cur_pos%=CURRENT_FILTER_SIZE; + mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated + mp.curR_DC[mp.cur_pos] = -fb.curR_DC; + mp.millis=_loopmillis; + log_update=true; +} + + + +void writeLogInfo(HardwareSerial &SerialRef) { //first line of file + SerialRef.print("#TIMESTAMP:"); + SerialRef.println(now()); +} + +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("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,"); + SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,"); + SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed"); +} + +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,3); 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(","); //invert speed, because left wheels are negated + SerialRef.print(-fbfront.speedR_meas); SerialRef.print(","); + SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated + SerialRef.print(-fbrear.speedR_meas); SerialRef.print(","); + + SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC + SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC + SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V + SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V + + SerialRef.print(currentAll,3); SerialRef.print(","); + SerialRef.print(throttle); SerialRef.print(","); + SerialRef.print(brake); SerialRef.print(","); + SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s + SerialRef.print(trip); SerialRef.print(","); //in m + SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours) + +} + +void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) +{ + SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println(); +} + + + +#endif \ No newline at end of file diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h new file mode 100644 index 0000000..5008c37 --- /dev/null +++ b/controller_teensy/include/definitions.h @@ -0,0 +1,124 @@ +#ifndef _DEFINITIONS_H +#define _DEFINITIONS_H + + +// ########################## DEFINES ########################## +#define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) +#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 20 //ms. delay for sending speed and steer data to motor controller via serial +#define LOGMININTERVAL 20 //minimum interval (ms) to send logs +#define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send + +#define WRITE_HEADER_TIME 400 //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments +bool log_header_written = false; + + +#define FEEDBACKRECEIVETIMEOUT 500 + +bool controllerFront_connected=false; +bool controllerRear_connected=false; +bool controllers_connected=false; + +#define PIN_THROTTLE A7 +//const uint16_t calib_throttle_min = 420; //better a bit too high than too low +//const uint16_t calib_throttle_max = 790; +const uint16_t failsafe_throttle_min = 4900; //if adc value falls below this failsafe is triggered. old 20 +const uint16_t failsafe_throttle_max = 14000; //if adc value goes above this failsafe is triggered. old 1000 +//const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel +const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel +#define PIN_BRAKE A8 +const uint16_t calib_brake_min = 2000;//better a bit too high than too low +const uint16_t calib_brake_max = 11000; +const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered +const uint16_t failsafe_brake_max = 13000; //if adc value goes above this failsafe is triggered + +uint16_t ads_throttle_A_raw=0; +uint16_t ads_throttle_B_raw=0; +uint16_t ads_brake_raw=failsafe_brake_min; +uint16_t ads_control_raw=0; + +int16_t throttle_pos=0; +int16_t brake_pos=0; + + +#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query +#define ADCREADPERIOD 10 +#define BUTTONREADPERIOD 20 +unsigned long last_adsread=0; //needed for failcheck +uint16_t throttle_raw=failsafe_throttle_min; //start at min so that failsafe is not triggered +#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response +uint16_t brake_raw=failsafe_brake_min; //start at min so that failsafe is not triggered +#define ADC_OUTOFRANGE_TIME 100 +unsigned long throttle_ok_time=0; +unsigned long brake_ok_time=0; +bool error_throttle_outofrange=false; +bool error_brake_outofrange=false; +bool error_ads_max_read_interval=false; + +#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward +#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1 + +#define NORMAL_MAX_ACCELERATION_RATE 10000 +#define SLOW_MAX_ACCELERATION_RATE 500 +int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second + + +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 + +float currentConsumed=0; //Ah + + +//Driving parameters +int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking +int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling) +float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor +float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading + +bool reverse_enabled=false; +unsigned long last_notidle=0; //not rolling to fast, no pedal pressed + +#define PIN_START A9 +#define PIN_LED_START 2 //Enginge start led + +#define PIN_LATCH_ENABLE A6 + +#define PIN_MODE_SWITCH 3 +#define PIN_MODE_LEDG 4 +#define PIN_MODE_LEDR 5 + + + +unsigned long last_send = 0; +unsigned long last_receive = 0; + +float filtered_currentAll=0; + +int16_t cmd_send=0; +int16_t last_cmd_send=0; + +uint8_t speedmode=0; +#define SPEEDMODE_SLOW 1 +#define SPEEDMODE_NORMAL 0 + + +unsigned long button_start_lastchange=0; +bool button_start_state=false; +#define LONG_PRESS_ARMING_TIME 2000 +#define DEBOUNCE_TIME 50 + +bool armed = false; //cmd output values forced to 0 if false + + + +#define CURRENT_FILTER_SIZE 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller) +#define CURRENT_MEANVALUECOUNT 20 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used + +#endif \ No newline at end of file diff --git a/controller_teensy/include/helpfunctions.h b/controller_teensy/include/helpfunctions.h new file mode 100644 index 0000000..337b90f --- /dev/null +++ b/controller_teensy/include/helpfunctions.h @@ -0,0 +1,34 @@ +#ifndef _HELPFUNCTIONS_H_ +#define _HELPFUNCTIONS_H_ + +#include "definitions.h" + +int sort_desc(const void *cmp1, const void *cmp2); +float filterMedian(int16_t* values); + +int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort +{ + float a = *((float *)cmp1); + float b = *((float *)cmp2); + return a > b ? -1 : (a < b ? 1 : 0); +} + + +float filterMedian(int16_t* values) { + float copied_values[CURRENT_FILTER_SIZE]; + for(int i=0;i +#include "definitions.h" +#include "structs.h" +#include "helpfunctions.h" +#include //for teensy rtc +#include "comms.h" + #include "ADS1X15.h" ADS1115 ADS(0x48); @@ -14,280 +20,29 @@ Tennsy Pin, Pin Name, Connected to 7, Rx3, Hoverboard TX(Blue) */ -// ########################## DEFINES ########################## -#define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) -#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 20 //ms. delay for sending speed and steer data to motor controller via serial -#define LOGMININTERVAL 20 //minimum interval (ms) to send logs -#define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send - -#define WRITE_HEADER_TIME 400 //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments -bool log_header_written = false; - - -#define FEEDBACKRECEIVETIMEOUT 500 - -bool controllerFront_connected=false; -bool controllerRear_connected=false; -bool controllers_connected=false; - -#define PIN_THROTTLE A7 -//const uint16_t calib_throttle_min = 420; //better a bit too high than too low -//const uint16_t calib_throttle_max = 790; -const uint16_t failsafe_throttle_min = 4900; //if adc value falls below this failsafe is triggered. old 20 -const uint16_t failsafe_throttle_max = 14000; //if adc value goes above this failsafe is triggered. old 1000 -//const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel -const uint16_t throttleCurvePerMM[] = {8485,8904,9177,9368,9513,9623,9705,9768,9823,9877,9932,9978,10032,10087,10169,10278,10451,10697,11061,11579,11898,11952}; //adc values for every unit (mm) of linear travel -#define PIN_BRAKE A8 -const uint16_t calib_brake_min = 2000;//better a bit too high than too low -const uint16_t calib_brake_max = 11000; -const uint16_t failsafe_brake_min = 700; //if adc value falls below this failsafe is triggered -const uint16_t failsafe_brake_max = 13000; //if adc value goes above this failsafe is triggered - -uint16_t ads_throttle_A_raw=0; -uint16_t ads_throttle_B_raw=0; -uint16_t ads_brake_raw=failsafe_brake_min; -uint16_t ads_control_raw=0; - -int16_t throttle_pos=0; -int16_t brake_pos=0; - - -#define ADSREADPERIOD 3 //set slightly higher as actual read time to avoid unnecessary register query -#define ADCREADPERIOD 10 -#define BUTTONREADPERIOD 20 -unsigned long last_adsread=0; //needed for failcheck -uint16_t throttle_raw=failsafe_throttle_min; //start at min so that failsafe is not triggered -#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response -uint16_t brake_raw=failsafe_brake_min; //start at min so that failsafe is not triggered -#define ADC_OUTOFRANGE_TIME 100 -unsigned long throttle_ok_time=0; -unsigned long brake_ok_time=0; -bool error_throttle_outofrange=false; -bool error_brake_outofrange=false; -bool error_ads_max_read_interval=false; - -#define REVERSE_ENABLE_TIME 1000 //ms. how long standstill to be able to drive backward -#define REVERSE_SPEED 0.25 //reverse driving speed //0 to 1 - -#define NORMAL_MAX_ACCELERATION_RATE 10000 -#define SLOW_MAX_ACCELERATION_RATE 500 -int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second - - -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 - -float currentConsumed=0; //Ah - - -//Driving parameters -int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking -int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling) -float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor -float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading - -bool reverse_enabled=false; -unsigned long last_notidle=0; //not rolling to fast, no pedal pressed - -#define PIN_START A9 -#define PIN_LED_START 2 //Enginge start led - -#define PIN_LATCH_ENABLE A6 - -#define PIN_MODE_SWITCH 3 -#define PIN_MODE_LEDG 4 -#define PIN_MODE_LEDR 5 -unsigned long last_send = 0; -unsigned long last_receive = 0; - -float filtered_currentAll=0; - -int16_t cmd_send=0; -int16_t last_cmd_send=0; - -uint8_t speedmode=0; -#define SPEEDMODE_SLOW 1 -#define SPEEDMODE_NORMAL 0 -unsigned long button_start_lastchange=0; -bool button_start_state=false; -#define LONG_PRESS_ARMING_TIME 2000 -#define DEBOUNCE_TIME 50 - -bool armed = false; //cmd output values forced to 0 if false -// Global variables for serial communication -typedef struct{ - uint8_t idx = 0; // Index for new data pointer - uint16_t bufStartFrame; // Buffer Start Frame - byte *p; // Pointer declaration for the new received data - byte incomingByte; - byte incomingBytePrev; - long lastValidDataSerial_time; -} SerialRead; -SerialRead SerialcomFront; -SerialRead SerialcomRear; - - -typedef struct{ - uint16_t start; - int16_t speedLeft; - int16_t speedRight; - uint16_t checksum; -} SerialCommand; -SerialCommand CommandFront; -SerialCommand CommandRear; - - -typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c - uint16_t start; - int16_t cmd1; - int16_t cmd2; - int16_t speedL_meas; //left speed is positive when driving forward - int16_t speedR_meas; //right speed is negatie when driving forward - int16_t batVoltage; - int16_t boardTemp; - int16_t curL_DC; //negative values are current consumed. positive values mean generated current - int16_t curR_DC; - uint16_t cmdLed; - uint16_t checksum; -} SerialFeedback; -SerialFeedback FeedbackFront; -SerialFeedback NewFeedbackFront; -SerialFeedback FeedbackRear; -SerialFeedback NewFeedbackRear; - -#define CURRENT_FILTER_SIZE 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller) -#define CURRENT_MEANVALUECOUNT 20 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used -typedef struct{ - int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current - int16_t curR_DC[CURRENT_FILTER_SIZE] = {0}; - 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; - - -void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef); -bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef); - -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 writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg); void readADS(); void readADC(); void failChecks(); void sendCMD(); void checkLog(); -void updateMotorparams( MotorParameter &mp, SerialFeedback &fb); + void leds(); void readButtons(); uint16_t linearizeThrottle(uint16_t v); -#include //for teensy rtc + time_t getTeensy3Time(); -void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) -{ - // Create command - scom.start = (uint16_t)START_FRAME; - scom.speedLeft = (int16_t)uSpeedLeft; - scom.speedRight = (int16_t)uSpeedRight; - scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight); - - SerialRef.write((uint8_t *) &scom, sizeof(scom)); - -} - -bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef) -{ - bool _result=false; //return true if new full data frame received - // Check for new data availability in the Serial buffer - if ( SerialRef.available() ) { - sread.incomingByte = SerialRef.read(); // Read the incoming byte - sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame - } - else { - return false; //nothing new - } - - // If DEBUG_RX is defined print all incoming bytes - #ifdef DEBUG_RX - Serial.print(sread.incomingByte); - #endif - - // Copy received data - if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected - sread.p = (byte *)&NewFeedback; - *sread.p++ = sread.incomingBytePrev; - *sread.p++ = sread.incomingByte; - sread.idx = 2; - } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data - *sread.p++ = sread.incomingByte; - sread.idx++; - } - - // Check if we reached the end of the package - if (sread.idx == sizeof(SerialFeedback)) { - uint16_t checksum; - - checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 - ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed); - - // Check validity of the new data - if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { - // Copy the new data - memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); - sread.lastValidDataSerial_time = millis(); - _result=true; - } else { - _result=false; - } - sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) - } - /* - // Print data to built-in Serial - Serial.print("1: "); Serial.print(Feedback.cmd1); - Serial.print(" 2: "); Serial.print(Feedback.cmd2); - Serial.print(" 3: "); Serial.print(Feedback.speedR); - Serial.print(" 4: "); Serial.print(Feedback.speedL); - Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); - Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); - Serial.print(" 7: "); Serial.print(Feedback.batVoltage); - Serial.print(" 8: "); Serial.println(Feedback.boardTemp); - } else { - Serial.println("Non-valid data skipped"); - }*/ - - // Update previous states - sread.incomingBytePrev = sread.incomingByte; - - return _result; //new data was available -} // ########################## SETUP ########################## void setup() @@ -376,10 +131,10 @@ void loop() { //Max (40) or 22 available/pending bytes if (newData2) { - updateMotorparams(motorparamsFront,FeedbackFront); + updateMotorparams(motorparamsFront,FeedbackFront,loopmillis); } if (newData3) { - updateMotorparams(motorparamsRear,FeedbackRear); + updateMotorparams(motorparamsRear,FeedbackRear,loopmillis); } @@ -433,88 +188,13 @@ void loop() { } -// ##### HELPFUNCTIONS - time_t getTeensy3Time() { return Teensy3Clock.get(); } -int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort -{ - float a = *((float *)cmp1); - float b = *((float *)cmp2); - return a > b ? -1 : (a < b ? 1 : 0); -} -float filterMedian(int16_t* values) { - float copied_values[CURRENT_FILTER_SIZE]; - for(int i=0;i