From 5f48b6ef4af3158e39beaf0152bc39a85363926e Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sat, 9 Aug 2025 14:24:47 +0200
Subject: [PATCH] add rc yaw channel

---
 controller_teensy/include/rc.h | 14 ++++++++++++++
 controller_teensy/src/main.cpp | 17 ++++++++++++++++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/controller_teensy/include/rc.h b/controller_teensy/include/rc.h
index 1c3ee20..8c2daad 100644
--- a/controller_teensy/include/rc.h
+++ b/controller_teensy/include/rc.h
@@ -21,6 +21,7 @@ static int count=0;
 #define PPM_DEADBAND 5
 #define PPM_CHANNEL_THROTTLE 1
 #define PPM_CHANNEL_STEER 2
+#define PPM_CHANNEL_YAW 3
 #define PPM_CHANNEL_MODE 5
 float ppm[PPM_CHANNELS];
 
@@ -33,6 +34,7 @@ void printPPM();
 
 int16_t getRCThrottle();
 int16_t getRCSteer();
+int16_t getRCYaw();
 uint8_t getRCMode(uint8_t modes);
 
 bool rc_fullcontrol_enabled=false; //full remote control. can be cancelled by button press
@@ -98,6 +100,18 @@ int16_t getRCSteer() {
     
 }
 
+//Return value -1000 - 1000
+int16_t getRCYaw() { //tank steering
+    if (ppm[PPM_CHANNEL_YAW-1]>PPM_MID+PPM_DEADBAND){
+        return constrain(map(ppm[PPM_CHANNEL_YAW-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000);
+    }else if (ppm[PPM_CHANNEL_YAW-1]<PPM_MID-PPM_DEADBAND){
+        return constrain(map(ppm[PPM_CHANNEL_YAW-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,-1000), -1000,0);
+    }else{
+        return 0;
+    }
+    
+}
+
 //Return descrete value for equidistant ranges. modes = number of ranges.
 uint8_t getRCMode(uint8_t modes) {
     float v=constrain(map(ppm[PPM_CHANNEL_MODE-1],PPM_MIN,PPM_MAX,0.0,1.0), 0.0,0.999);
diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 6b7abee..73132c3 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -584,6 +584,7 @@ void calculateSetSpeed(unsigned long timediff){
 
 #ifdef RC
   int16_t differential_steering_max=200;
+  int16_t differential_yaw_max=500; //tank steering
 
   #define RC_STEER_ENABLE_WAITTIME 1000/SENDPERIOD*2; //in cycles. every SENDPERIOD
   static uint16_t rc_steer_enable_wait=0; //give some time to release buttons after arming with rc mode to not cancel immediately
@@ -596,8 +597,22 @@ void calculateSetSpeed(unsigned long timediff){
         rc_steer_enabled=false;
       }
       float rcsteer=getRCSteer()/1000.0;
+      
       cmd_send_toMotor_FL+=rcsteer*differential_steering_max;
       cmd_send_toMotor_FR-=rcsteer*differential_steering_max;
+
+      float rcyaw=getRCYaw()/1000.0;
+      cmd_send_toMotor_FL+=rcyaw*differential_yaw_max;
+      cmd_send_toMotor_FR-=rcyaw*differential_yaw_max;
+      cmd_send_toMotor_RL+=rcyaw*differential_yaw_max;
+      cmd_send_toMotor_RR-=rcyaw*differential_yaw_max;
+
+      //limit to max values
+      cmd_send_toMotor_FL=constrain(cmd_send_toMotor_FL,-1000,1000);
+      cmd_send_toMotor_FR=constrain(cmd_send_toMotor_FR,-1000,1000);
+      cmd_send_toMotor_RL=constrain(cmd_send_toMotor_RL,-1000,1000);
+      cmd_send_toMotor_RR=constrain(cmd_send_toMotor_RR,-1000,1000);
+      
     }
   }else{
     rc_steer_enable_wait=RC_STEER_ENABLE_WAITTIME;
@@ -779,7 +794,7 @@ void readButtons() {
       rc_steer_enabled=false;
       if (control_buttonA && control_buttonB) { //button A and B is held down during start button press
         uint8_t rcmode=getRCMode(6);
-        if (getRCThrottle()==0 && getRCBrake()==0) {
+        if (getRCThrottle()==0 && getRCBrake()==0 && getRCYaw()==0) { //safe to enable rc (controls at zero)
           switch (rcmode) {
             case 0:
               throttle_max=250;