fix tank steering in standstill

controller_teensy/include/display.h

@@ -344,7 +344,7 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc
     display.setCursor(6*10,0);
     dtostrf((escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0,1,1,buf);
     display.print((String)buf);  
-    display.print(F('V'));
+    display.print(F("V"));
     display.println();
 
 

controller_teensy/src/main.cpp

@@ -607,27 +607,38 @@ void calculateSetSpeed(unsigned long timediff){
   
   float steeringdifferential_speed=0.3; //Speed for turning the steering wheel by differtially driving the front wheels
 
-  static bool reverse_enabled2=true; //pohibit instant reverse driving when releasing steering wheel buttons when brake is still pressed. wait for throttle and brake to release once
-  if (!reverse_enabled2 && throttle_pos<=0 && brake_pos<=0){
-    reverse_enabled2=true;
-  }
-  
   if (reverse_enabled) {  //backwards driving not prohibited
+    static bool tanksteering_prohibit_left=false;
+    static bool tanksteering_prohibit_right=false;
+    static bool prohibit_reverse=false;
+
     if (control_buttonA && !control_buttonB) { //Right button (A) only
+      tanksteering_prohibit_left=true;
+      prohibit_reverse=true;
+    }else if(control_buttonB && !control_buttonA) {//Left button (B) only
+      tanksteering_prohibit_right=true;
+      prohibit_reverse=true;
+    }else if(!control_buttonA && !control_buttonB){ //no button pressed
+      tanksteering_prohibit_left=true;
+      tanksteering_prohibit_right=true;
+      if (throttle_pos<=0 && brake_pos<=0) {//levers released
+        tanksteering_prohibit_left=false;
+        tanksteering_prohibit_right=false;
+        prohibit_reverse=false;
+      }
+    }
+
+    if (!tanksteering_prohibit_right && tanksteering_prohibit_left && prohibit_reverse) { //Right Tanksteering
       cmd_send_toMotor_FL+=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_FR-=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_RL+=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_RR-=brake_pos*steeringdifferential_speed;
-      reverse_enabled2=false;
+    }else if(tanksteering_prohibit_right && !tanksteering_prohibit_left && prohibit_reverse) { //Left Tanksteering
-
-    }else if(control_buttonB && !control_buttonA) { //Left button (B) only
       cmd_send_toMotor_FL-=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_FR+=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_RL-=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_RR+=brake_pos*steeringdifferential_speed;
-      reverse_enabled2=false;
+    }else if(tanksteering_prohibit_right && tanksteering_prohibit_left && !prohibit_reverse){ //no button on steering wheel pressed, drive backwards
-
-    }else if(reverse_enabled2){ //no button on steering wheel pressed, drive backwards
       _cmd_send_toMotor-=brake_pos*reverse_speed;
 
       cmd_send_toMotor_FL=_cmd_send_toMotor;