From ecb5fff511e440279eace1cc8619f16e196f7edd Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sat, 20 Mar 2021 17:56:44 +0100
Subject: [PATCH] working freewheeling with breaking, values untested

---
 controller_teensy/src/main.cpp | 42 ++++++++++++++++++++++------------
 1 file changed, 27 insertions(+), 15 deletions(-)

diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index fd4563f..3696814 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -198,8 +198,11 @@ void setup()
   pinMode(PIN_START, INPUT_PULLUP);
 
   pinMode(PIN_LED_START, OUTPUT);
+  digitalWrite(PIN_LED_START,HIGH);
   pinMode(PIN_MODE_LEDG, OUTPUT);
+  digitalWrite(PIN_MODE_LEDG,LOW);
   pinMode(PIN_MODE_LEDR, OUTPUT);
+  digitalWrite(PIN_MODE_LEDR,LOW);
 
   pinMode(PIN_LATCH_ENABLE, OUTPUT);
   digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
@@ -223,14 +226,20 @@ void loop() {
     uint16_t brake_raw = analogRead(PIN_BRAKE);
     brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
 
-    Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
-    Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
+    //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
+    //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
+
+    if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
+      throttle_pos/=2;
+      digitalWrite(PIN_MODE_LEDG,HIGH); //Green
+      digitalWrite(PIN_MODE_LEDR,LOW);
+    }else{ //button not pushed in
+      digitalWrite(PIN_MODE_LEDG,LOW);
+      digitalWrite(PIN_MODE_LEDR,HIGH); //Red
+    }
     
   }
   
-  
-  //Serial.print("fo="); Serial.println(count);
-  //count++;
 
   if (newData2) {
     motorparamsFront.cur_pos++;
@@ -243,20 +252,24 @@ void loop() {
     //Calculate motor stuff and send to motor controllers
     last_send=loopmillis;
 
-    float freewheel_current=0.1;
-    float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
-    float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0;
-    float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0;
+    int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
+    #define MAXBREAKCURRENT 10
+    float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
 
-    float filtered_currentAll=(filtered_curFL+filtered_curFR)/2.0; //mean current of all motors
+    float freewheel_current=0.1-brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
+    float freewheel_break_factor=1000.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
+    float filtered_curFL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
+    float filtered_curFR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
+
+    float filtered_currentAll=min(filtered_curFL,filtered_curFR);
 
     if (throttle_pos>=motorparamsFront.cmdR) { //accelerating
       cmd_send = throttle_pos; //if throttle higher than apply throttle directly
     }else{ //freewheeling or braking
-      if (-filtered_currentAll>freewheel_current) { //breaking current high enough
-        cmd_send-= max(0, (-filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor
+      if (-filtered_currentAll>freewheel_current) { //drive current too high
+        cmd_send-= max(0, (-filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
       }
-      cmd_send-=1; //reduce slowly anyways
+      cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
       cmd_send=constrain(cmd_send,0,1000);
     }
 
@@ -266,8 +279,7 @@ void loop() {
 
     SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
 
-    //Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
-    
+    Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println();
     
   }
 }