copy functions from bobbycar code

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<CURRENT_FILTER_SIZE;i++) {
+    copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
+  }
+  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
+  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
+
+  float mean=copied_values[CURRENT_FILTER_SIZE/2];
+  for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
+    mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
+  }
+  mean/=(1+CURRENT_MEANVALUECOUNT*2);
+
+  return mean;
 }

src/hoverboard-esc-serial-comm.h

@@ -3,17 +3,115 @@
 
 #include <Arduino.h>
 
+
+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
+    ESCSerialComm(HardwareSerial& _SerialRef); //constructor
-    void update(long millis, boolean state); //declare example function
+    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