change back to voltage mode

This commit is contained in:
interfisch 2020-06-01 18:06:30 +02:00
parent 3a78ff3ac1
commit f64f910f50
2 changed files with 40 additions and 5 deletions

View File

@ -146,6 +146,9 @@ SerialFeedback Feedback2;
SerialFeedback NewFeedback2;
#define A2BIT_CONV 50 //divide curL_DC value by A2BIT_CONV to get current in amperes. Take this value from hoverboard firmware config.h
enum mode{booting, idle, on, error, off};
/*
* idle: controller is on, hoverboards are off
@ -163,7 +166,7 @@ mode last_currentmode=off; //for printout
void setup()
{
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear)
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)
@ -181,7 +184,9 @@ void setup()
pinMode(PIN_THROTTLE, INPUT);
pinMode(PIN_BRAKE, INPUT);
Serial.println("Initialized");
Serial.println("Initialized Serial");
Serial1.println("Initialized Serial1");
Serial2.println("Initialized Serial2");
currentmode = booting; //start in idle mode
requestmode = currentmode;
@ -462,17 +467,47 @@ void loop_idle() {
}
void loop_on() {
int _maxspeed=1000;
int _maxbrake=400;
if (MODESWITCH_DOWN) {
_maxspeed=200;
_maxbrake=200;
}
int16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);
}
/*
uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);
int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking
int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque
*/
int16_t speedvalue = (out_speedFL+out_speedFR+out_speedRL+out_speedRR)/4; //generate last speedvalue from individual motor speeds
uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);
int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking
int16_t combthrottlevalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque
int16_t combthrottlevalue_positive = max(0,combthrottlevalue); //only positive
#define CURRENTBRAKE_P 2.0 //proportional brake when throttle is lower than current speed. Depends on LOOPTIME
#define BRAKE_P 0.1 //speed-=brakevalue*brake_p . depends on LOOPTIME
//serial2 is Front. serial1 is Rear
float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / A2BIT_CONV;
if (combthrottlevalue_positive>=speedvalue) { //if throttle higher then apply immediately
speedvalue = combthrottlevalue_positive;
}else{ //throttle lever is lower than current set speedvalue
if (_current > 0) { //is consuming current when it shouldnt
speedvalue = max( speedvalue-_current*CURRENTBRAKE_P ,combthrottlevalue_positive); //not lower than throttlevalue
}
}
if (combthrottlevalue<0){ //throttle off and brake pressed
speedvalue= max(speedvalue + combthrottlevalue*BRAKE_P,0); //not negative = not backwards
}
out_speedFL=speedvalue;
out_speedFR=speedvalue;

@ -1 +1 @@
Subproject commit b9bf849330f8e7355d0881300ebdc59607845e2d
Subproject commit 3a71408329ea0776ffba80c50bf8607b2010dbe2