From dc40db8e74366a779578c9fd3c9edbfc43e79972 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 22 May 2022 21:00:26 +0200 Subject: [PATCH] add functions for hoverbrett --- src/hoverboard-esc-serial-comm.cpp | 151 ++++++++++++++++++++--------- src/hoverboard-esc-serial-comm.h | 47 ++++++--- 2 files changed, 140 insertions(+), 58 deletions(-) diff --git a/src/hoverboard-esc-serial-comm.cpp b/src/hoverboard-esc-serial-comm.cpp index 35607d6..40f371f 100644 --- a/src/hoverboard-esc-serial-comm.cpp +++ b/src/hoverboard-esc-serial-comm.cpp @@ -23,59 +23,62 @@ int16_t ESCSerialComm::getCmdR() { return Motorparams.cmdR; } -bool ESCSerialComm::sendPending(long millis) { - return (millis - last_send > SENDPERIOD); +bool ESCSerialComm::sendPending(unsigned long loopmillis) { + return (loopmillis - last_send > SENDPERIOD); } -bool ESCSerialComm::update(long millis) //returns true if something was sent or received +bool ESCSerialComm::update(unsigned long loopmillis) //returns true if something was sent or received { - loopmillis=millis; - bool flag_sent=false; - bool flag_received=ReceiveSerial(); - if (flag_received) { - updateMotorparams(); + bool flag_sent=false; + bool flag_received=ReceiveSerial(); + if (flag_received) { + updateMotorparams(loopmillis); + } + + #define TRIP_UPDATE_INTERVAL 100 + static unsigned long last_update_trip; + if ( loopmillis > last_update_trip+TRIP_UPDATE_INTERVAL) { + last_update_trip=loopmillis; + + double _meanRPM=(-Feedback.speedL_meas+Feedback.speedR_meas)/2.0; + meanSpeedms=_meanRPM*wheelcircumference/60.0; // Units: 1/min * m / 60s + trip+=abs(meanSpeedms)* ((TRIP_UPDATE_INTERVAL)/1000.0); + + //mah consumed + currentConsumed += (Motorparams.filtered_curL+Motorparams.filtered_curR)* (TRIP_UPDATE_INTERVAL/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; - if ( loopmillis > Motorparams.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected - if (controller_connected) { //just got disconnected - controller_connected=false; - Serial.println("Controller Front feedback timeout"); + 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() + } - }else if(!controller_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before - controller_connected=true; - Serial.println("Controller Front connected"); - } + + + } - if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers - 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() - - } - - - - //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; + return flag_received || flag_sent; } bool ESCSerialComm::feedbackAvailable() @@ -164,12 +167,29 @@ bool ESCSerialComm::ReceiveSerial() } -void ESCSerialComm::updateMotorparams() { +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>5000) { //wait until voltage is reliable from esc + minBatVoltage=min(minBatVoltage,getFeedback_batVoltage()); + } + + 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 @@ -231,10 +251,49 @@ float ESCSerialComm::getFiltered_curR() { -float ESCSerialComm::getCurrentConsumed() { +double ESCSerialComm::getCurrentConsumed() { return currentConsumed; } -float ESCSerialComm::getTrip() { +double ESCSerialComm::getTrip() { return trip; +} + +double ESCSerialComm::getMeanSpeed() { + return meanSpeedms; +} + +float ESCSerialComm::getMinBatVoltage() { + return minBatVoltage; +} + +void ESCSerialComm::resetStatistics() { + minBatVoltage=1000; + 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; } \ No newline at end of file diff --git a/src/hoverboard-esc-serial-comm.h b/src/hoverboard-esc-serial-comm.h index dd7ddd5..410e7e0 100644 --- a/src/hoverboard-esc-serial-comm.h +++ b/src/hoverboard-esc-serial-comm.h @@ -42,8 +42,8 @@ typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct 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 speedL_meas; //left speed is positive when driving forward (bobbycar?) + int16_t speedR_meas; //right speed is negatie when driving forward (bobbycar?) int16_t batVoltage; int16_t boardTemp; int16_t curL_DC; //negative values are current consumed. positive values mean generated current @@ -75,12 +75,12 @@ class ESCSerialComm public: ESCSerialComm(HardwareSerial& _SerialRef); //constructor void init(); - bool update(long millis); + bool update(unsigned long loopmillis); bool feedbackAvailable(); void setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight); int16_t getCmdL(); int16_t getCmdR(); - bool sendPending(long millis); + bool sendPending(unsigned long loopmillis); int16_t getFeedback_cmd1(); int16_t getFeedback_cmd2(); @@ -93,12 +93,22 @@ class ESCSerialComm float getFiltered_curL(); float getFiltered_curR(); - float getCurrentConsumed(); - float getTrip(); + float getMinBatVoltage(); + double getCurrentConsumed(); + double getMeanSpeed(); + double getTrip(); + + float getMincurL(); + float getMincurR(); + float getMaxcurL(); + float getMaxcurR(); + + unsigned long getFeedbackInterval(); //get time from last received feedback + unsigned long getTripTime(unsigned long loopmillis) ; + + void resetStatistics(); private: - unsigned long loopmillis; - long _millis_lastinput; //declare private variable @@ -107,15 +117,28 @@ class ESCSerialComm bool controller_connected=false; - float meanSpeedms; - float trip; //trip distance in meters + double meanSpeedms; + double trip; //trip distance in meters float wheelcircumference; //wheel diameter in m. - float currentConsumed; //Ah + double currentConsumed; //Ah + + float minBatVoltage=1000; + + + float mincurL=0; + float mincurR=0; + + float maxcurL=0; + float maxcurR=0; + unsigned long last_send; unsigned long last_receive; + unsigned long last_reset_time; + + unsigned long feedback_interval_timed; SerialCommand Command; @@ -127,7 +150,7 @@ class ESCSerialComm bool flag_received=false; - void updateMotorparams(); + void updateMotorparams(unsigned long loopmillis); void SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight); bool ReceiveSerial();