working throttle and brake with slight freewheeling
This commit is contained in:
parent
ecb5fff511
commit
3b8ea2bd3a
|
@ -30,7 +30,7 @@ int16_t throttle_pos=0;
|
||||||
int16_t brake_pos=0;
|
int16_t brake_pos=0;
|
||||||
|
|
||||||
unsigned long last_adcread=0;
|
unsigned long last_adcread=0;
|
||||||
#define ADCREADPERIOD 100
|
#define ADCREADPERIOD 25
|
||||||
|
|
||||||
#define PIN_START A9
|
#define PIN_START A9
|
||||||
#define PIN_LED_START 2 //Enginge start led
|
#define PIN_LED_START 2 //Enginge start led
|
||||||
|
@ -50,6 +50,7 @@ float avg_currentL=0;
|
||||||
float avg_currentR=0;
|
float avg_currentR=0;
|
||||||
|
|
||||||
int16_t cmd_send=0;
|
int16_t cmd_send=0;
|
||||||
|
int16_t last_cmd_send=0;
|
||||||
|
|
||||||
|
|
||||||
// Global variables for serial communication
|
// Global variables for serial communication
|
||||||
|
@ -62,6 +63,7 @@ typedef struct{
|
||||||
long lastValidDataSerial_time;
|
long lastValidDataSerial_time;
|
||||||
} SerialRead;
|
} SerialRead;
|
||||||
SerialRead SerialcomFront;
|
SerialRead SerialcomFront;
|
||||||
|
SerialRead SerialcomRear;
|
||||||
|
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
|
@ -71,6 +73,7 @@ typedef struct{
|
||||||
uint16_t checksum;
|
uint16_t checksum;
|
||||||
} SerialCommand;
|
} SerialCommand;
|
||||||
SerialCommand CommandFront;
|
SerialCommand CommandFront;
|
||||||
|
SerialCommand CommandRear;
|
||||||
|
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
|
@ -88,6 +91,8 @@ typedef struct{
|
||||||
} SerialFeedback;
|
} SerialFeedback;
|
||||||
SerialFeedback FeedbackFront;
|
SerialFeedback FeedbackFront;
|
||||||
SerialFeedback NewFeedbackFront;
|
SerialFeedback NewFeedbackFront;
|
||||||
|
SerialFeedback FeedbackRear;
|
||||||
|
SerialFeedback NewFeedbackRear;
|
||||||
|
|
||||||
#define CURRENT_FILTER_SIZE 100 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
|
#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
|
#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
|
||||||
|
@ -99,6 +104,7 @@ typedef struct{
|
||||||
int16_t cmdR=0;
|
int16_t cmdR=0;
|
||||||
} MotorParameter;
|
} MotorParameter;
|
||||||
MotorParameter motorparamsFront;
|
MotorParameter motorparamsFront;
|
||||||
|
MotorParameter motorparamsRear;
|
||||||
|
|
||||||
|
|
||||||
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
|
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
|
||||||
|
@ -216,6 +222,7 @@ void loop() {
|
||||||
loopmillis=millis(); //read millis for this cycle
|
loopmillis=millis(); //read millis for this cycle
|
||||||
|
|
||||||
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
|
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
|
||||||
|
bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
|
||||||
|
|
||||||
|
|
||||||
if (loopmillis - last_adcread > ADCREADPERIOD) {
|
if (loopmillis - last_adcread > ADCREADPERIOD) {
|
||||||
|
@ -226,6 +233,10 @@ void loop() {
|
||||||
uint16_t brake_raw = analogRead(PIN_BRAKE);
|
uint16_t brake_raw = analogRead(PIN_BRAKE);
|
||||||
brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
|
brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
|
||||||
|
|
||||||
|
if (brake_pos>0) { //pressed brake disables throttle
|
||||||
|
throttle_pos=0;
|
||||||
|
}
|
||||||
|
|
||||||
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
|
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
|
||||||
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
|
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
|
||||||
|
|
||||||
|
@ -247,37 +258,53 @@ void loop() {
|
||||||
motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
|
motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
|
||||||
motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
|
motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
|
||||||
}
|
}
|
||||||
|
if (newData3) {
|
||||||
|
motorparamsRear.cur_pos++;
|
||||||
|
motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
|
||||||
|
motorparamsRear.curL_DC[motorparamsRear.cur_pos] = FeedbackRear.curL_DC;
|
||||||
|
motorparamsRear.curR_DC[motorparamsRear.cur_pos] = FeedbackRear.curR_DC;
|
||||||
|
}
|
||||||
|
|
||||||
if (loopmillis - last_send > SENDPERIOD) {
|
if (loopmillis - last_send > SENDPERIOD) {
|
||||||
//Calculate motor stuff and send to motor controllers
|
//Calculate motor stuff and send to motor controllers
|
||||||
last_send=loopmillis;
|
last_send=loopmillis;
|
||||||
|
|
||||||
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
|
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
|
||||||
#define MAXBREAKCURRENT 10
|
#define MAXBREAKCURRENT 20
|
||||||
float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
|
float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
|
||||||
|
|
||||||
float freewheel_current=0.1-brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
|
float freewheel_current=0.1+brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
|
||||||
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 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; //in Amps
|
float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
|
||||||
float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
|
float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
|
||||||
|
float filtered_curRL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
|
||||||
|
float filtered_curRR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
|
||||||
|
|
||||||
float filtered_currentAll=min(filtered_curFL,filtered_curFR);
|
float filtered_currentFront=min(filtered_curFL,filtered_curFR);
|
||||||
|
float filtered_currentRear=min(filtered_curRL,filtered_curRR);
|
||||||
|
|
||||||
if (throttle_pos>=motorparamsFront.cmdR) { //accelerating
|
float filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
|
||||||
|
|
||||||
|
if (throttle_pos>=last_cmd_send) { //accelerating
|
||||||
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
|
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
|
||||||
}else{ //freewheeling or braking
|
}else{ //freewheeling or braking
|
||||||
if (-filtered_currentAll>freewheel_current) { //drive current too high
|
if (filtered_currentAll<freewheel_current) { //drive current too high
|
||||||
cmd_send-= max(0, (-filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
|
cmd_send-= max(0, (-filtered_currentAll+freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
|
||||||
}
|
}
|
||||||
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
|
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
|
||||||
cmd_send=constrain(cmd_send,0,1000);
|
cmd_send=constrain(cmd_send,0,1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
last_cmd_send=cmd_send;
|
||||||
|
|
||||||
//apply throttle command to all motors
|
//apply throttle command to all motors
|
||||||
motorparamsFront.cmdL=cmd_send;
|
motorparamsFront.cmdL=cmd_send;
|
||||||
motorparamsFront.cmdR=cmd_send;
|
motorparamsFront.cmdR=cmd_send;
|
||||||
|
motorparamsRear.cmdL=cmd_send;
|
||||||
|
motorparamsRear.cmdR=cmd_send;
|
||||||
|
|
||||||
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
||||||
|
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
|
||||||
|
|
||||||
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();
|
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();
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue