fix freewheel current direction

This commit is contained in:
interfisch 2021-05-15 18:56:42 +02:00
parent bd01ea3f78
commit 467d454547
1 changed files with 64 additions and 19 deletions

View File

@ -81,6 +81,14 @@ uint8_t speedmode=0;
#define SPEEDMODE_NORMAL 0 #define SPEEDMODE_NORMAL 0
unsigned long button_start_lastchange=0;
bool button_start_state=false;
#define LONG_PRESS_ARMING_TIME 2000
#define DEBOUNCE_TIME 50
bool armed = false; //cmd output values forced to 0 if false
// Global variables for serial communication // Global variables for serial communication
typedef struct{ typedef struct{
uint8_t idx = 0; // Index for new data pointer uint8_t idx = 0; // Index for new data pointer
@ -122,8 +130,8 @@ SerialFeedback NewFeedbackFront;
SerialFeedback FeedbackRear; SerialFeedback FeedbackRear;
SerialFeedback NewFeedbackRear; 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 20 //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 4 //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{ typedef struct{
int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
int16_t curR_DC[CURRENT_FILTER_SIZE] = {0}; int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
@ -155,6 +163,8 @@ void checkLog();
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb); void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
void leds(); void leds();
void readButtons();
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
{ {
// Create command // Create command
@ -245,7 +255,7 @@ void setup()
pinMode(PIN_THROTTLE, INPUT); pinMode(PIN_THROTTLE, INPUT);
pinMode(PIN_BRAKE, INPUT); pinMode(PIN_BRAKE, INPUT);
pinMode(PIN_START, INPUT_PULLUP); pinMode(PIN_START, INPUT_PULLUP); //Pressed=High
pinMode(PIN_LED_START, OUTPUT); //Active High pinMode(PIN_LED_START, OUTPUT); //Active High
@ -279,6 +289,8 @@ void loop() {
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
last_adcread=loopmillis; last_adcread=loopmillis;
readADC(); readADC();
readButtons();
} }
@ -424,7 +436,7 @@ void failChecks() {
controllers_connected=controllerFront_connected & controllerRear_connected; controllers_connected=controllerFront_connected & controllerRear_connected;
//ADC Range Check //ADC Range Check
if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //outside safe range. maybe wire got disconnected if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
throttle_ok_time=loopmillis; throttle_ok_time=loopmillis;
} }
if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
@ -445,20 +457,21 @@ void failChecks() {
if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange) { //controllers not available if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange) { //any errors?
throttle_pos=0; throttle_pos=0;
brake_pos=0; brake_pos=0;
} }
} }
void sendCMD() { void sendCMD() {
#define BREAK_CMDREDUCE_CONSTANT 500 //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=BREAK_CMDREDUCE_CONSTANT / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling) #define MINIMUM_CONSTANT_CMD_REDUCE 1 //reduce cmd every loop by this constant amount when freewheeling/braking
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,(int16_t)(BREAK_CMDREDUCE_CONSTANT*SENDPERIOD/1000)); //reduce cmd value every cycle #define BREAK_CMDREDUCE_PROPORTIONAL 500 //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=BREAK_CMDREDUCE_PROPORTIONAL / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
#define STARTBREAKCURRENT 5 //Ampere. at what point to start apply brake, proportional to brake_pos. int16_t cmdreduce_constant=map(brake_pos,0,1000,0,(int16_t)(BREAK_CMDREDUCE_PROPORTIONAL*SENDPERIOD/1000)); //reduce cmd value every cycle
#define STARTBREAKCURRENT_OFFSET 0.1 //offset start point for breaking, because of reading fluctuations around 0A #define STARTBREAKCURRENT 5 //Ampere. at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
#define STARTBREAKCURRENT_OFFSET 0.1 //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
float brakepedal_current_multiplier=STARTBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000) float brakepedal_current_multiplier=STARTBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
float freewheel_current=STARTBREAKCURRENT_OFFSET+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=STARTBREAKCURRENT_OFFSET-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=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average) float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average)
motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
@ -468,19 +481,19 @@ void sendCMD() {
float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR); float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR); float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
filtered_currentAll=max(filtered_currentFront,filtered_currentRear); filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current
if (throttle_pos>=last_cmd_send) { //accelerating if (throttle_pos>=last_cmd_send) { //accelerating
cmd_send = throttle_pos; //if throttle higher than last applied value, apply throttle directly cmd_send = throttle_pos; //if throttle higher than last applied value, 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(MINIMUM_CONSTANT_CMD_REDUCE,cmdreduce_constant); //reduce slowly anyways
cmd_send=constrain(cmd_send,0,1000); cmd_send=constrain(cmd_send,0,1000);
} }
if (!controllers_connected) { //controllers not connected if (!controllers_connected || !armed) { //controllers not connected or not armed
cmd_send=0; //safety off cmd_send=0; //safety off
} }
@ -503,12 +516,12 @@ void sendCMD() {
void checkLog() { void checkLog() {
if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
writeLogHeader(Serial1); //connection recovered, write log header writeLogHeader(Serial1);
log_header_written=true; log_header_written=true;
} }
if ((log_header_written && log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) { if (log_header_written && ( (log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) ) {
last_log_send=loopmillis; last_log_send=loopmillis;
log_update=false; log_update=false;
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos); writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
@ -525,7 +538,13 @@ void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
} }
void leds() { void leds() {
digitalWrite(PIN_LED_START,(loopmillis/1000)%2 == 0); //high is on for LED_START //Start LED
if (!armed) { //disarmed
digitalWrite(PIN_LED_START,((loopmillis/1000)%2 == 0)); //high is on for LED_START. blink every second. loopmillis 0 - 1000 led is on.
}else{ //armed
digitalWrite(PIN_LED_START,HIGH); //LED On
}
if (speedmode==SPEEDMODE_SLOW) { if (speedmode==SPEEDMODE_SLOW) {
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
@ -535,3 +554,29 @@ void leds() {
digitalWrite(PIN_MODE_LEDR,LOW); //Red digitalWrite(PIN_MODE_LEDR,LOW); //Red
} }
} }
void readButtons() {
if (loopmillis > button_start_lastchange+DEBOUNCE_TIME) { //wait some time after last change
if (digitalRead(PIN_START) && !button_start_state) { //start engine button pressed and was not pressed before
button_start_state=true; //pressed
button_start_lastchange=loopmillis; //save time for debouncing
}else if (!digitalRead(PIN_START) && button_start_state) { //released an was pressed before
button_start_state=false; // not pressed
button_start_lastchange=loopmillis; //save time for debouncing
}
}
if (button_start_state) { //pressed
if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
if (throttle_pos<=0 && brake_pos<=0 && !armed) { //brake or thottle not pressed
armed=true; //arm if button pressed long enough
writeLogComment(Serial1,loopmillis, "Armed by button");
}
}else if (armed){ //not pressed long enough and is armed
armed=false; //disarm
writeLogComment(Serial1,loopmillis, "Disarmed by button");
}
}
}