From 64bd6f79f5d0900145f31d759073c9427752c581 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Tue, 16 Jul 2024 22:34:54 +0200
Subject: [PATCH] change tanksteering and add standstill tanksteering

---
 controller_teensy/src/main.cpp | 23 +++++++++++++----------
 1 file changed, 13 insertions(+), 10 deletions(-)

diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 695752f..07aac79 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -558,8 +558,8 @@ void calculateSetSpeed(unsigned long timediff){
   static float tanksteering_differential=0; //to ramp up slowly. value between -1.0 and 1.0
   //Parameters:
   int16_t tanksteering_max_speed=200;
-  float tanksteering_rate_increase=1.0; //increase units per second
-  float tanksteering_rate_decrease=1.5; //decrease units per second
+  float tanksteering_rate_increase=2.0; //increase units per second
+  float tanksteering_rate_decrease=3.0; //decrease units per second
 
 
 
@@ -568,7 +568,7 @@ void calculateSetSpeed(unsigned long timediff){
     tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1
   }else if(control_buttonB && !control_buttonA && !reverse_enabled && throttle_pos>0) { //Left button (B) only. and throttle touched
     tanksteering_differential-=tanksteering_rate_increase*(timediff/1000.0);
-    tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1    
+    tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1
   }else{ //buttons released
     if(tanksteering_differential>0) {
       tanksteering_differential-=tanksteering_rate_decrease*(timediff/1000.0);
@@ -586,6 +586,7 @@ void calculateSetSpeed(unsigned long timediff){
 
 
   // ## Braking, Reversing and Standstill movements below here ##
+  /*
   if (reverse_enabled) {  //backwards driving not prohibited
     _cmd_send_toMotor-=brake_pos*reverse_speed;
 
@@ -595,9 +596,10 @@ void calculateSetSpeed(unsigned long timediff){
     cmd_send_toMotor_RR=_cmd_send_toMotor;
 
   }
+  */
   
-  /*
-  float steeringdifferential_speed=0.1; //Speed for turning the steering wheel by differtially driving the front wheels
+  
+  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){
@@ -608,15 +610,15 @@ void calculateSetSpeed(unsigned long timediff){
     if (control_buttonA && !control_buttonB) { //Right button (A) only
       cmd_send_toMotor_FL+=brake_pos*steeringdifferential_speed;
       cmd_send_toMotor_FR-=brake_pos*steeringdifferential_speed;
-      //cmd_send_toMotor_RL-=0;
-      //cmd_send_toMotor_RR-=0;
+      cmd_send_toMotor_RL+=brake_pos*steeringdifferential_speed;
+      cmd_send_toMotor_RR-=brake_pos*steeringdifferential_speed;
       reverse_enabled2=false;
 
     }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-=0;
-      //cmd_send_toMotor_RR-=0;
+      cmd_send_toMotor_RL-=brake_pos*steeringdifferential_speed;
+      cmd_send_toMotor_RR+=brake_pos*steeringdifferential_speed;
       reverse_enabled2=false;
 
     }else if(reverse_enabled2){ //no button on steering wheel pressed, drive backwards
@@ -628,7 +630,8 @@ void calculateSetSpeed(unsigned long timediff){
       cmd_send_toMotor_RR=_cmd_send_toMotor;
     }
   }
-  */
+  
+
 
   if (!controllers_connected || !armed) { //controllers not connected or not armed
     cmd_send=0;