diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index f490f7f..69b41b7 100644 --- a/controller_teensy/include/definitions.h +++ b/controller_teensy/include/definitions.h @@ -74,6 +74,7 @@ uint16_t brake_raw=failsafe_brake_min; //start at min so that failsafe is not t #define ADC_DIFFHIGH_TIME 500 //for failsafe_throttle_maxDiff. how long values need to stay bad to trigger failsafe bool error_throttle_outofrange=false; +bool error_throttle_difftoohigh=false; bool error_brake_outofrange=false; bool error_ads_max_read_interval=false; bool error_sdfile_unavailable=false; diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index 94be73e..bb3222b 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -51,7 +51,7 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){ display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage()); */ - if ( (error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval || error_sdfile_unavailable) && ((loopmillis/2000)%2==0)) { + if ( (error_brake_outofrange || error_throttle_outofrange || error_throttle_difftoohigh || error_ads_max_read_interval || error_sdfile_unavailable) && ((loopmillis/2000)%2==0)) { //Error Messages display.setFont(); @@ -68,6 +68,9 @@ void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){ if (error_throttle_outofrange) { errorstring+="throttle_outofrange\n"; } + if (error_throttle_difftoohigh) { + errorstring+="throttle_difftoohigh\n"; + } if (error_ads_max_read_interval) { errorstring+="ads_max_read_interval\n"; }if (error_sdfile_unavailable) { diff --git a/controller_teensy/include/led.h b/controller_teensy/include/led.h index 71c4c90..37abd4a 100644 --- a/controller_teensy/include/led.h +++ b/controller_teensy/include/led.h @@ -103,7 +103,7 @@ void led_update(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& - if (error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval) { + if (error_brake_outofrange || error_throttle_outofrange || error_throttle_difftoohigh || error_ads_max_read_interval) { //Error }else{ diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 11d889b..f5aee5b 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -430,7 +430,6 @@ void readADC() { //Serial.print(throttle_posA); Serial.print('\t'); //Serial.print(throttle_posB); Serial.print('\t'); - int16_t _throttlediff = (int)throttle_posA-(int)throttle_posB; int16_t throttle_posMean = (throttle_posA+throttle_posB)/2.0; @@ -529,8 +528,8 @@ void failChecks() { throttlediff_ok_time=loopmillis; } if (loopmillis>throttlediff_ok_time+ADC_DIFFHIGH_TIME) { //not ok for too long - if (!error_throttle_outofrange) { - error_throttle_outofrange=true; + if (!error_throttle_difftoohigh) { + error_throttle_difftoohigh=true; writeLogComment(loopmillis, "Error Throttle Diff too High. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw); } //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw); @@ -563,7 +562,7 @@ void failChecks() { writeLogComment(loopmillis, "Error SDFile Unavailable"); } - if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval) { //any errors? + if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_throttle_difftoohigh || error_ads_max_read_interval) { //any errors? armed=false; //disarm throttle_pos=0; brake_pos=0; @@ -589,9 +588,7 @@ void calculateSetSpeed(unsigned long timediff){ filtered_currentAll=filtered_currentFront+filtered_currentRear; //positive value is current Drawn from battery. negative value is braking current - if (throttle_pos>=last_cmd_send) { //accelerating - cmd_send += constrain(throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly - }else{ //freewheeling or braking + if(throttle_posfreewheel_current) { //drive current too high cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(timediff/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value } @@ -599,6 +596,9 @@ void calculateSetSpeed(unsigned long timediff){ } + //acceleration + cmd_send += constrain(throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly + cmd_send=constrain(cmd_send,0,1000); last_cmd_send=cmd_send;