From 97630cfcff7fa813ace7d1cfb7d4268a07e98750 Mon Sep 17 00:00:00 2001 From: Fisch Date: Mon, 28 Mar 2022 21:03:00 +0200 Subject: [PATCH] copy functions from bobbycar code --- src/hoverboard-esc-serial-comm.cpp | 159 ++++++++++++++++++++++++++++- src/hoverboard-esc-serial-comm.h | 102 +++++++++++++++++- 2 files changed, 256 insertions(+), 5 deletions(-) diff --git a/src/hoverboard-esc-serial-comm.cpp b/src/hoverboard-esc-serial-comm.cpp index 5d9ed73..60caf07 100644 --- a/src/hoverboard-esc-serial-comm.cpp +++ b/src/hoverboard-esc-serial-comm.cpp @@ -1,10 +1,163 @@ #include "hoverboard-esc-serial-comm.h" -ESCSerialComm::ESCSerialComm() { //constructor + +ESCSerialComm::ESCSerialComm(HardwareSerial &_serialRef) { //constructor + serialRef=&_serialRef; + wheelcircumference=0.5278; //8.4cm radius -> 0.084m*2*Pi } -void ESCSerialComm::update(long millis, boolean state) //example function +void ESCSerialComm::setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight) { - //Do stuff + Motorparams.cmdL=uSpeedLeft; + Motorparams.cmdR=uSpeedRight; +} + +bool ESCSerialComm::update(long millis) //returns true if something was sent or received +{ + loopmillis=millis; + bool flag_sent=false; + bool flag_received=ReceiveSerial(); + if (flag_received) { + updateMotorparams(); + } + if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers + last_send=loopmillis; + + if (controller_connected) { + SendSerial(Motorparams.cmdL,Motorparams.cmdR); + + + flag_sent=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() + + } + + //Update speed and trip + float _meanRPM=(Feedback.speedL_meas-Feedback.speedR_meas)/2.0; + meanSpeedms=_meanRPM*wheelcircumference/60.0; // Units: 1/min * m / 60s + trip+=abs(meanSpeedms)* (SENDPERIOD/1000.0); + + //mah consumed + currentConsumed += (Motorparams.filtered_curL+Motorparams.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours + } + return flag_received || flag_sent; +} + +bool ESCSerialComm::feedbackAvailable() +{ + return flag_received; +} + + + +void ESCSerialComm::SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight) +{ + // Create command + Command.start = (uint16_t)START_FRAME; + Command.speedLeft = (int16_t)uSpeedLeft; + Command.speedRight = (int16_t)uSpeedRight; + Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight); + + serialRef->write((uint8_t *) &Command, sizeof(Command)); + +} + +bool ESCSerialComm::ReceiveSerial() +{ + + bool _result=1; + // 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 0; + } + + // 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(); + } else { + _result=0; + } + 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 ESCSerialComm::updateMotorparams() { + Motorparams.cur_pos++; + Motorparams.cur_pos%=CURRENT_FILTER_SIZE; + Motorparams.curL_DC[Motorparams.cur_pos] = -Feedback.curL_DC; //invert so positive current is consumed current. negative then means regenerated + Motorparams.curR_DC[Motorparams.cur_pos] = -Feedback.curR_DC; + Motorparams.millis=loopmillis; +} + +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 + +int sort_desc(const void *cmp1, const void *cmp2); +float filterMedian(int16_t* values); + #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 SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial + + +#define FEEDBACKRECEIVETIMEOUT 500 + + + +// 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; + + + +typedef struct{ + uint16_t start; + int16_t speedLeft; + int16_t speedRight; + uint16_t checksum; +} SerialCommand; + + +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; + +#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; + + + + class ESCSerialComm { public: - ESCSerialComm(); //constructor - void update(long millis, boolean state); //declare example function + ESCSerialComm(HardwareSerial& _SerialRef); //constructor + bool update(long millis); + bool feedbackAvailable(); + void setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight); + private: + unsigned long loopmillis; + long _millis_lastinput; //declare private variable + + + HardwareSerial *serialRef; + + + bool controller_connected=false; + + float meanSpeedms; + float trip; //trip distance in meters + float wheelcircumference; //wheel diameter in m. + + float currentConsumed; //Ah + + + unsigned long last_send; + unsigned long last_receive; + + + SerialCommand Command; + SerialRead SRead; + SerialFeedback Feedback; + SerialFeedback NewFeedback; + MotorParameter Motorparams; + + bool flag_received=false; + + + void updateMotorparams(); + + void SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight); + bool ReceiveSerial(); }; #endif \ No newline at end of file