add functions for hoverbrett
This commit is contained in:
parent
c2eba898ac
commit
dc40db8e74
|
|
@ -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;
|
||||
|
||||
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()
|
||||
|
||||
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 (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;
|
||||
}
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue