From 58b8a80f58ce215d08bc39ae197a070e458c1b87 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 30 May 2021 00:07:43 +0200
Subject: [PATCH] add reverse

---
 controller_teensy/src/main.cpp | 25 +++++++++++++++++++++----
 1 file changed, 21 insertions(+), 4 deletions(-)

diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 735c756..e368439 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -75,7 +75,8 @@ int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount propor
 float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
 float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
 
-
+bool reverse_enabled=false;
+unsigned long last_notidle=0; //not rolling to fast, no pedal pressed
 
 #define PIN_START A9
 #define PIN_LED_START 2 //Enginge start led
@@ -310,6 +311,8 @@ void loop() {
     readADC();
 
     readButtons();
+
+    
   }
 
 
@@ -422,6 +425,15 @@ void readADC() {
   brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
   //brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000);
 
+  if (throttle_pos>0 || meanSpeedms>0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
+    last_notidle=loopmillis;
+    reverse_enabled=false;
+  }
+  #define REVERSE_ENABLE_TIME 1000
+  if (loopmillis-last_notidle > REVERSE_ENABLE_TIME) {
+    reverse_enabled=true;
+  }
+
   int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
   throttle_pos=constrain(throttlebreak_pos,0,1000);
   brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos
@@ -542,9 +554,7 @@ void sendCMD() {
     
   }
 
-  if (!controllers_connected || !armed) { //controllers not connected or not armed
-    cmd_send=0; //safety off
-  }
+  
 
   cmd_send=constrain(cmd_send,0,1000);
 
@@ -552,7 +562,14 @@ void sendCMD() {
 
   int16_t cmd_send_toMotor=constrain(cmd_send*  (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking"
 
+  if (reverse_enabled) {
+    cmd_send_toMotor-=brake_pos*0.1;
+  }
 
+  if (!controllers_connected || !armed) { //controllers not connected or not armed
+    cmd_send=0;
+    cmd_send_toMotor=0; //safety off
+  }
 
   //apply throttle command to all motors
   motorparamsFront.cmdL=cmd_send_toMotor;