interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

working freewheeling with breaking, values untested

This commit is contained in:
interfisch 2021-03-20 17:56:44 +01:00
parent 0580cab66a
commit ecb5fff511
1 changed files with 27 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
controller_teensy/src/main.cpp
View File

@ -198,8 +198,11 @@ void setup()
pinMode(PIN_START, INPUT_PULLUP);
pinMode(PIN_LED_START, OUTPUT);
digitalWrite(PIN_LED_START,HIGH);
pinMode(PIN_MODE_LEDG, OUTPUT);
digitalWrite(PIN_MODE_LEDG,LOW);
pinMode(PIN_MODE_LEDR, OUTPUT);
digitalWrite(PIN_MODE_LEDR,LOW);
pinMode(PIN_LATCH_ENABLE, OUTPUT);
digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
@ -223,15 +226,21 @@ void loop() {
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
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_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
throttle_pos/=2;
digitalWrite(PIN_MODE_LEDG,HIGH); //Green
digitalWrite(PIN_MODE_LEDR,LOW);
}else{ //button not pushed in
digitalWrite(PIN_MODE_LEDG,LOW);
digitalWrite(PIN_MODE_LEDR,HIGH); //Red
}
}
//Serial.print("fo="); Serial.println(count);
//count++;
if (newData2) {
motorparamsFront.cur_pos++;
motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
@ -243,20 +252,24 @@ void loop() {
//Calculate motor stuff and send to motor controllers
last_send=loopmillis;
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;
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
#define MAXBREAKCURRENT 10
float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
float filtered_currentAll=(filtered_curFL+filtered_curFR)/2.0; //mean current of all motors
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 filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
float filtered_currentAll=min(filtered_curFL,filtered_curFR);
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
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-=1; //reduce slowly anyways
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
cmd_send=constrain(cmd_send,0,1000);
}
@ -266,8 +279,7 @@ void loop() {
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();
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();
}
}