interfisch
/
hoverboard-esc-serial-comm
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

copy functions from bobbycar code

This commit is contained in:
interfisch 2022-03-28 21:03:00 +02:00
parent ba0b19560f
commit 97630cfcff
2 changed files with 256 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

159
src/hoverboard-esc-serial-comm.cpp
View File

@ -1,10 +1,163 @@
#include "hoverboard-esc-serial-comm.h" #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;
} }

102
src/hoverboard-esc-serial-comm.h
View File

@ -3,17 +3,115 @@
#include <Arduino.h> #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_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 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 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 class ESCSerialComm
{ {
public: 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: private:
unsigned long loopmillis;
long _millis_lastinput; //declare private variable 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 #endif