318 lines
9.4 KiB
C++
318 lines
9.4 KiB
C++
#include "hoverboard-esc-serial-comm.h"
|
|
|
|
|
|
ESCSerialComm::ESCSerialComm(HardwareSerial &_serialRef) { //constructor
|
|
serialRef=&_serialRef;
|
|
wheelcircumference=0.5278; //8.4cm radius -> 0.084m*2*Pi
|
|
}
|
|
|
|
void ESCSerialComm::init() {
|
|
serialRef->begin(SERIAL_CONTROL_BAUD);
|
|
}
|
|
|
|
void ESCSerialComm::setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight)
|
|
{
|
|
Motorparams.cmdL=uSpeedLeft;
|
|
Motorparams.cmdR=uSpeedRight;
|
|
}
|
|
int16_t ESCSerialComm::getCmdL() {
|
|
return Motorparams.cmdL;
|
|
}
|
|
int16_t ESCSerialComm::getCmdR() {
|
|
return Motorparams.cmdR;
|
|
}
|
|
|
|
bool ESCSerialComm::sendPending(unsigned long loopmillis) {
|
|
return (loopmillis - last_send > SENDPERIOD);
|
|
}
|
|
|
|
bool ESCSerialComm::update(unsigned long loopmillis) //returns true if something was sent or received
|
|
{
|
|
bool flag_sent=false;
|
|
bool flag_received=ReceiveSerial();
|
|
if (flag_received) {
|
|
updateMotorparams(loopmillis);
|
|
}
|
|
|
|
#define TRIP_UPDATE_INTERVAL 100
|
|
if ( loopmillis > last_update_trip+TRIP_UPDATE_INTERVAL) {
|
|
unsigned long trip_update_interval_real=loopmillis-last_update_trip;
|
|
last_update_trip=loopmillis;
|
|
|
|
//double _meanRPM=(-Feedback.speedL_meas+Feedback.speedR_meas)/2.0;
|
|
double _meanRPM=(getFeedback_speedL_meas()+getFeedback_speedR_meas())/2.0;
|
|
meanSpeedms=_meanRPM*wheelcircumference/60.0; // Units: 1/min * m / 60s
|
|
trip+=abs(meanSpeedms)* ((trip_update_interval_real)/1000.0);
|
|
|
|
//mah consumed
|
|
currentConsumed += (Motorparams.filtered_curL+Motorparams.filtered_curR)* (trip_update_interval_real/1000.0)/3600.0; //amp hours
|
|
}
|
|
|
|
|
|
if ( loopmillis > Motorparams.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
|
|
if (controller_connected) { //just got disconnected
|
|
controller_connected=false;
|
|
Serial.println("Controller Front feedback timeout");
|
|
}
|
|
}else if(!controller_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
|
|
controller_connected=true;
|
|
Serial.println("Controller Front connected");
|
|
}
|
|
|
|
if (sendPending(loopmillis)) { //Calculate motor stuff and send to motor controllers. timing seems to be not too exact
|
|
last_send=loopmillis;
|
|
|
|
Motorparams.filtered_curL=filterMedian(Motorparams.curL_DC)/50.0; //in Amps
|
|
Motorparams.filtered_curR=filterMedian(Motorparams.curR_DC)/50.0; //in Amps
|
|
|
|
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()
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
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(unsigned long loopmillis) {
|
|
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;
|
|
|
|
|
|
if (loopmillis>20000) { //wait until voltage is reliable from esc
|
|
minBatVoltage=min(minBatVoltage,getFeedback_batVoltage());
|
|
maxBoardTemp=max(maxBoardTemp,getFeedback_boardTemp());
|
|
}
|
|
|
|
mincurL=min(mincurL,getFiltered_curL());
|
|
mincurR=min(mincurR,getFiltered_curR());
|
|
maxcurL=max(maxcurL,getFiltered_curL());
|
|
maxcurR=max(maxcurR,getFiltered_curR());
|
|
|
|
|
|
//Update speed and trip
|
|
static unsigned long last_motorparams_received;
|
|
feedback_interval_timed=loopmillis-last_motorparams_received;
|
|
last_motorparams_received=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 ESCSerialComm::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;
|
|
}
|
|
|
|
|
|
int16_t ESCSerialComm::getFeedback_cmd1() {
|
|
return Feedback.cmd1;
|
|
}
|
|
int16_t ESCSerialComm::getFeedback_cmd2() {
|
|
return Feedback.cmd2;
|
|
}
|
|
int16_t ESCSerialComm::getFeedback_speedL_meas() {
|
|
return Feedback.speedL_meas;
|
|
}
|
|
int16_t ESCSerialComm::getFeedback_speedR_meas() {
|
|
return -Feedback.speedR_meas; //negate rpm, so that positive rpm means driving forward
|
|
}
|
|
float ESCSerialComm::getFeedback_batVoltage() {
|
|
return Feedback.batVoltage/100.0;
|
|
}
|
|
float ESCSerialComm::getFeedback_boardTemp() {
|
|
return Feedback.boardTemp/10.0;
|
|
}
|
|
float ESCSerialComm::getFeedback_curL_DC() {
|
|
return Feedback.curL_DC/50.0;
|
|
}
|
|
float ESCSerialComm::getFeedback_curR_DC() {
|
|
return Feedback.curR_DC/50.0;
|
|
}
|
|
|
|
float ESCSerialComm::getFiltered_curL() {
|
|
return Motorparams.filtered_curL;
|
|
}
|
|
float ESCSerialComm::getFiltered_curR() {
|
|
return Motorparams.filtered_curR;
|
|
}
|
|
|
|
|
|
|
|
double ESCSerialComm::getCurrentConsumed() {
|
|
return currentConsumed;
|
|
}
|
|
|
|
double ESCSerialComm::getTrip() {
|
|
return trip;
|
|
}
|
|
|
|
double ESCSerialComm::getMeanSpeed() {
|
|
return meanSpeedms;
|
|
}
|
|
|
|
float ESCSerialComm::getMinBatVoltage() {
|
|
return minBatVoltage;
|
|
}
|
|
|
|
float ESCSerialComm::getMaxBoardTemp() {
|
|
return maxBoardTemp;
|
|
}
|
|
|
|
void ESCSerialComm::resetStatistics() {
|
|
minBatVoltage=1000;
|
|
maxBoardTemp=0;
|
|
mincurL=0;
|
|
mincurR=0;
|
|
maxcurL=0;
|
|
maxcurR=0;
|
|
last_reset_time=millis();
|
|
|
|
}
|
|
|
|
unsigned long ESCSerialComm::getTripTime(unsigned long loopmillis) { //time since last reset in ms
|
|
return loopmillis-last_reset_time;
|
|
}
|
|
|
|
float ESCSerialComm::getMincurL() {
|
|
return mincurL;
|
|
}
|
|
float ESCSerialComm::getMincurR() {
|
|
return mincurR;
|
|
}
|
|
float ESCSerialComm::getMaxcurL() {
|
|
return maxcurL;
|
|
}
|
|
float ESCSerialComm::getMaxcurR() {
|
|
return maxcurR;
|
|
}
|
|
|
|
unsigned long ESCSerialComm::getFeedbackInterval() {
|
|
return feedback_interval_timed;
|
|
}
|
|
|
|
bool ESCSerialComm::getControllerConnected() {
|
|
return controller_connected;
|
|
}
|
|
|
|
float ESCSerialComm::getWheelspeed_L() {
|
|
return getFeedback_speedL_meas()*wheelcircumference/60.0; // Units: 1/min * m / 60s
|
|
}
|
|
float ESCSerialComm::getWheelspeed_R() {
|
|
return getFeedback_speedR_meas()*wheelcircumference/60.0; // Units: 1/min * m / 60s
|
|
} |