From f64f910f50e36780952a49b1ba522fa163e95317 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Mon, 1 Jun 2020 18:06:30 +0200
Subject: [PATCH] change back to voltage mode

---
 controller/controller.ino               | 43 ++++++++++++++++++++++---
 hoverboard-firmware-hack-foc-serial-esc |  2 +-
 2 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/controller/controller.ino b/controller/controller.ino
index a01f048..70244e2 100644
--- a/controller/controller.ino
+++ b/controller/controller.ino
@@ -146,6 +146,9 @@ SerialFeedback Feedback2;
 SerialFeedback NewFeedback2;
 
 
+#define A2BIT_CONV 50  //divide curL_DC value by A2BIT_CONV to get current in amperes. Take this value from hoverboard firmware config.h
+
+
 enum mode{booting, idle, on, error, off};
 /*
  * idle: controller is on, hoverboards are off
@@ -163,7 +166,7 @@ mode last_currentmode=off; //for printout
 void setup() 
 {
 
-  Serial.begin(115200); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)
+  Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)
 
   Serial1.begin(SERIAL_CONTROL_BAUD); //control.  A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear)
   Serial2.begin(SERIAL_CONTROL_BAUD); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)
@@ -181,7 +184,9 @@ void setup()
   pinMode(PIN_THROTTLE, INPUT);
   pinMode(PIN_BRAKE, INPUT);
 
-  Serial.println("Initialized"); 
+  Serial.println("Initialized Serial"); 
+  Serial1.println("Initialized Serial1"); 
+  Serial2.println("Initialized Serial2"); 
 
   currentmode = booting; //start in idle mode
   requestmode = currentmode;
@@ -462,17 +467,47 @@ void loop_idle() {
 }
 
 void loop_on() {
+  
   int _maxspeed=1000;
   int _maxbrake=400;
   if (MODESWITCH_DOWN) {
     _maxspeed=200;
     _maxbrake=200;
-  }
-  int16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);
+  }  
+
+  /*
+  uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);
 
   int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking
 
   int16_t speedvalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque
+  */
+
+  int16_t speedvalue = (out_speedFL+out_speedFR+out_speedRL+out_speedRR)/4; //generate last speedvalue from individual motor speeds
+
+  uint16_t throttlevalue=constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, _maxspeed ) ,0, _maxspeed);
+  int16_t brakevalue=constrain( map(adc_brake, ADC_CALIB_BRAKE_MIN, ADC_CALIB_BRAKE_MAX, 0, _maxbrake ) ,0, _maxbrake); //positive value for braking
+
+  int16_t combthrottlevalue=throttlevalue*(1- (((float)brakevalue)/_maxbrake)) - (brakevalue*(1- (((float)throttlevalue)/_maxspeed)) ); //brake reduces throttle and adds negative torque
+  int16_t combthrottlevalue_positive = max(0,combthrottlevalue); //only positive
+
+  #define CURRENTBRAKE_P 2.0 //proportional brake when throttle is lower than current speed. Depends on LOOPTIME
+  #define BRAKE_P 0.1 //speed-=brakevalue*brake_p . depends on LOOPTIME
+
+  //serial2 is Front. serial1 is Rear
+  float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / A2BIT_CONV; 
+
+  if (combthrottlevalue_positive>=speedvalue) { //if throttle higher then apply immediately
+    speedvalue = combthrottlevalue_positive;
+  }else{ //throttle lever is lower than current set speedvalue
+    if (_current > 0) { //is consuming current when it shouldnt
+      speedvalue = max( speedvalue-_current*CURRENTBRAKE_P ,combthrottlevalue_positive); //not lower than throttlevalue
+    }
+  }
+
+  if (combthrottlevalue<0){ //throttle off and brake pressed
+    speedvalue= max(speedvalue + combthrottlevalue*BRAKE_P,0); //not negative = not backwards
+  }
 
   out_speedFL=speedvalue;
   out_speedFR=speedvalue;
diff --git a/hoverboard-firmware-hack-foc-serial-esc b/hoverboard-firmware-hack-foc-serial-esc
index b9bf849..3a71408 160000
--- a/hoverboard-firmware-hack-foc-serial-esc
+++ b/hoverboard-firmware-hack-foc-serial-esc
@@ -1 +1 @@
-Subproject commit b9bf849330f8e7355d0881300ebdc59607845e2d
+Subproject commit 3a71408329ea0776ffba80c50bf8607b2010dbe2