From 82fbc8234e8cdddc280f33a6ac4eddafbc43a79c Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Wed, 31 Mar 2021 20:46:24 +0200
Subject: [PATCH] invert current

---
 controller_teensy/src/main.cpp | 28 ++++++++++++++--------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index 4d8e29a..9be85f4 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -92,7 +92,7 @@ typedef struct{
    int16_t  speedR_meas; //right speed is positive when driving forward
    int16_t  batVoltage;
    int16_t  boardTemp;
-   int16_t  curL_DC; //negative values are current drawn. positive values mean generated current
+   int16_t  curL_DC; //negative values are current consumed. positive values mean generated current
    int16_t  curR_DC;
    uint16_t cmdLed;
    uint16_t  checksum;
@@ -105,7 +105,7 @@ SerialFeedback NewFeedbackRear;
 #define CURRENT_FILTER_SIZE 100 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
 #define CURRENT_MEANVALUECOUNT 10 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used
 typedef struct{
-  int16_t curL_DC[CURRENT_FILTER_SIZE] = {0};
+  int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
   int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
   uint8_t cur_pos=0;
   int16_t cmdL=0;
@@ -283,16 +283,16 @@ void loop() {
   if (newData2) {
     motorparamsFront.cur_pos++;
     motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
-    motorparamsFront.curL_DC[motorparamsFront.cur_pos] = FeedbackFront.curL_DC;
-    motorparamsFront.curR_DC[motorparamsFront.cur_pos] = FeedbackFront.curR_DC;
+    motorparamsFront.curL_DC[motorparamsFront.cur_pos] = -FeedbackFront.curL_DC; //invert so positive current is consumed current. negative then means regenerated
+    motorparamsFront.curR_DC[motorparamsFront.cur_pos] = -FeedbackFront.curR_DC;
     motorparamsFront.millis=loopmillis;
     log_update=true;
   }
   if (newData3) {
     motorparamsRear.cur_pos++;
     motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
-    motorparamsRear.curL_DC[motorparamsRear.cur_pos] = FeedbackRear.curL_DC;
-    motorparamsRear.curR_DC[motorparamsRear.cur_pos] = FeedbackRear.curR_DC;
+    motorparamsRear.curL_DC[motorparamsRear.cur_pos] = -FeedbackRear.curL_DC;
+    motorparamsRear.curR_DC[motorparamsRear.cur_pos] = -FeedbackRear.curR_DC;
     motorparamsRear.millis=loopmillis;
     log_update=true;
   }
@@ -312,10 +312,10 @@ void loop() {
     motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
     motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
 
-    float filtered_currentFront=min(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
-    float filtered_currentRear=min(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
+    float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
+    float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
 
-    filtered_currentAll=min(filtered_currentFront,filtered_currentRear);
+    filtered_currentAll=max(filtered_currentFront,filtered_currentRear);
 
     if (throttle_pos>=last_cmd_send) { //accelerating
       cmd_send = throttle_pos; //if throttle higher than apply throttle directly
@@ -406,10 +406,10 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
   SerialRef.print(mprear.cmdL); SerialRef.print(",");
   SerialRef.print(mprear.cmdR); SerialRef.print(",");
 
-  SerialRef.print(-mpfront.filtered_curL,3); SerialRef.print(","); //invert. positive current is drive current
-  SerialRef.print(-mpfront.filtered_curR,3); SerialRef.print(",");
-  SerialRef.print(-mprear.filtered_curL,3); SerialRef.print(",");
-  SerialRef.print(-mprear.filtered_curR,3); SerialRef.print(",");
+  SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(","); //invert. positive current is drive current
+  SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(",");
+  SerialRef.print(mprear.filtered_curL,3); SerialRef.print(",");
+  SerialRef.print(mprear.filtered_curR,3); SerialRef.print(",");
 
   SerialRef.print(-fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
   SerialRef.print(fbfront.speedR_meas); SerialRef.print(",");
@@ -421,7 +421,7 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
   SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
   SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
 
-  SerialRef.print(-currentAll,3); SerialRef.print(","); //invert. positive current is drive current
+  SerialRef.print(currentAll,3); SerialRef.print(","); //invert. positive current is drive current
   SerialRef.print(throttle); SerialRef.print(",");
   SerialRef.print(brake); SerialRef.println();
 }
\ No newline at end of file