simple proportional braking for freewheeling
This commit is contained in:
parent
3becfe30b1
commit
e83de79916
|
|
@ -13,16 +13,15 @@
|
|||
const uint16_t calib_throttle_min = 350;
|
||||
const uint16_t calib_throttle_max = 810;
|
||||
|
||||
int16_t out_speedFL=0;
|
||||
int16_t out_speedFR=0;
|
||||
int16_t out_speedRL=0;
|
||||
int16_t out_speedRR=0;
|
||||
|
||||
unsigned long last_send = 0;
|
||||
unsigned long last_receive = 0;
|
||||
|
||||
float avg_currentL=0;
|
||||
float avg_currentR=0;
|
||||
|
||||
int16_t cmd_send=0;
|
||||
|
||||
|
||||
// Global variables for serial communication
|
||||
typedef struct{
|
||||
|
|
@ -44,6 +43,7 @@ typedef struct{
|
|||
} SerialCommand;
|
||||
SerialCommand CommandFront;
|
||||
|
||||
|
||||
typedef struct{
|
||||
uint16_t start;
|
||||
int16_t cmd1;
|
||||
|
|
@ -52,7 +52,7 @@ typedef struct{
|
|||
int16_t speedR_meas;
|
||||
int16_t batVoltage;
|
||||
int16_t boardTemp;
|
||||
int16_t curL_DC;
|
||||
int16_t curL_DC; //negative values are current drawn. positive values mean generated current
|
||||
int16_t curR_DC;
|
||||
uint16_t cmdLed;
|
||||
uint16_t checksum;
|
||||
|
|
@ -60,10 +60,24 @@ typedef struct{
|
|||
SerialFeedback FeedbackFront;
|
||||
SerialFeedback NewFeedbackFront;
|
||||
|
||||
#define CURRENT_FILTER_SIZE 100 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
|
||||
#define CURRENT_MEANVALUECOUNT 10 //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};
|
||||
int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
|
||||
uint8_t cur_pos=0;
|
||||
int16_t cmdL=0;
|
||||
int16_t cmdR=0;
|
||||
} MotorParameter;
|
||||
MotorParameter motorparamsFront;
|
||||
|
||||
|
||||
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
|
||||
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
|
||||
|
||||
int sort_desc(const void *cmp1, const void *cmp2);
|
||||
float filterMedian(int16_t* values);
|
||||
|
||||
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
|
||||
{
|
||||
// Create command
|
||||
|
|
@ -139,7 +153,6 @@ bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
|
|||
sread.incomingBytePrev = sread.incomingByte;
|
||||
|
||||
return _result; //new data was available
|
||||
|
||||
}
|
||||
|
||||
// ########################## SETUP ##########################
|
||||
|
|
@ -161,43 +174,74 @@ unsigned long loopmillis;
|
|||
void loop() {
|
||||
loopmillis=millis(); //read millis for this cycle
|
||||
|
||||
bool newData=false;
|
||||
newData=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
|
||||
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
|
||||
|
||||
|
||||
//Serial.print("fo="); Serial.println(count);
|
||||
//count++;
|
||||
|
||||
if (newData) {
|
||||
float _current = (FeedbackFront.curL_DC+FeedbackFront.curR_DC)/2.0 / 50;
|
||||
|
||||
/*
|
||||
Serial.print(FeedbackFront.curL_DC); Serial.print(", ");
|
||||
Serial.print(FeedbackFront.curR_DC); Serial.print(", ");
|
||||
|
||||
Serial.print(", mean="); Serial.print(_current);
|
||||
Serial.print(", ");
|
||||
Serial.print(FeedbackFront.cmd1); Serial.print(", ");
|
||||
Serial.print(FeedbackFront.cmd2); Serial.print(", ");
|
||||
Serial.println();
|
||||
*/
|
||||
|
||||
#define CURRENTFILTER 0.95
|
||||
avg_currentL=avg_currentL*CURRENTFILTER+FeedbackFront.curL_DC*(1-CURRENTFILTER);
|
||||
avg_currentR=avg_currentR*CURRENTFILTER+FeedbackFront.curR_DC*(1-CURRENTFILTER);
|
||||
if (newData2) {
|
||||
motorparamsFront.cur_pos++;
|
||||
motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
|
||||
motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
|
||||
motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
|
||||
}
|
||||
|
||||
if (loopmillis - last_send > SENDPERIOD) {
|
||||
last_send=loopmillis;
|
||||
|
||||
uint16_t throttle_raw = analogRead(PIN_THROTTLE);
|
||||
//Serial.print("Analog: "); Serial.print(throttle_raw);
|
||||
out_speedFR=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000)));
|
||||
|
||||
//Serial.print(", Send: "); Serial.println(out_speedFL);
|
||||
SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2);
|
||||
int16_t throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
|
||||
|
||||
|
||||
Serial.print(out_speedFL); Serial.print(", "); Serial.print(out_speedFR); Serial.print(", "); Serial.print(avg_currentR); Serial.print(", "); Serial.println(avg_currentL);
|
||||
float freewheel_current=0.1;
|
||||
float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
|
||||
float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0;
|
||||
float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0;
|
||||
|
||||
float filtered_currentAll=(filtered_curFL+filtered_curFR)/2.0; //mean current of all motors
|
||||
|
||||
if (throttle_pos>=motorparamsFront.cmdR) { //accelerating
|
||||
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
|
||||
}else{ //freewheeling or braking
|
||||
if (-filtered_currentAll>freewheel_current) { //breaking current high enough
|
||||
cmd_send-= max(0, (-filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor
|
||||
}
|
||||
cmd_send-=1; //reduce slowly anyways
|
||||
cmd_send=constrain(cmd_send,0,1000);
|
||||
}
|
||||
|
||||
//apply throttle command to all motors
|
||||
motorparamsFront.cmdL=cmd_send;
|
||||
motorparamsFront.cmdR=cmd_send;
|
||||
|
||||
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
||||
|
||||
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();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
Loading…
Reference in New Issue