Diff: brushless_motor.cpp

Revision:

13:ef7a06fa11de

Parent:

12:d1d21a2941ef

Child:

14:acaa1add097b

--- a/brushless_motor.cpp	Mon Mar 04 17:51:08 2019 +0000
+++ b/brushless_motor.cpp	Sun Sep 29 16:34:37 2019 +0000
@@ -1,8 +1,9 @@
 //  Cloned from 'DualBLS2018_06' on 23 November 2018
 #include "mbed.h"
 //#include    "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
-#include    "stm32f401xe.h"
-#include "DualBLS.h"
+//#include    "stm32f401xe.h"
+//#include    "stm32f411xe.h"
+#include "STM3_ESC.h"
 #include "BufferedSerial.h"
 #include "FastPWM.h"
 #include "Servo.h"
@@ -47,10 +48,10 @@
     numof_current_sense_rs = 1.0;
     RPM_filter  = 0.0;
     dv_by_dt = 0.0;
-    s[1] = 0.25;
-    s[2] = 9.0;
-    s[3] = 0.4;
-    s[4] = 0.2;
+    sdbl[1] = 0.25;     //  Remind me. What are these all about ?
+    sdbl[2] = 9.0;
+    sdbl[3] = 0.4;
+    sdbl[4] = 0.2;
     dRPM        = 0.0;
     V_clamp =   1.0 ;       //  Used to limit top speed
     motor_poles   = 8;    //  Default to 8 pole motor
@@ -80,7 +81,7 @@
         max_rpm = (uint32_t) (((double)mode.rd(TOP_SPEED) / rpm2mph) / 10.0)  ;
         target_speed = (double)mode.rd(TOP_SPEED) / 10.0;
         numof_current_sense_rs = (double)mode.rd(current_sense_rs_offset);
-        pc.printf   ("max_rpm=%d, tp speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph);
+        pc.printf   ("max_rpm=%d, top speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph);
     }
     if  (p == 4 || p == 6 || p == 8)    {
         motor_poles = p;
@@ -152,6 +153,11 @@
 }
 
 
+extern  double  Current_Scaler_Sep_2019;
+extern  int WatchDog;
+#define DRIVING (visible_mode == MOTOR_FORWARD || visible_mode == MOTOR_REVERSE)
+#define ESTOP   (WatchDog == 0 && DRIVING)
+
 /**
 *   void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
 *
@@ -159,13 +165,13 @@
 */
 void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
 {
-    if  (p < 0.0)
+    if  (p < 0.0 || ESTOP)
         p = 0.0;
     if  (p > 1.0)
         p = 1.0;
     last_V = p;     //  Retains last voltage limit demanded by driver
     
-    if  ((V_clamp < last_V) && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE))    //  Jan 2019 limit top speed when driving
+    if  ((V_clamp < last_V) && DRIVING)    //  Jan 2019 limit top speed when driving
         p = V_clamp;    //  If motor runnable, set voltage limit to lower of last_V and V_clamp
 
     p *= 0.95;      //  need limit, ffi see MCP1630 data
@@ -173,6 +179,7 @@
     maxV.pulsewidth_ticks  ((int)(p * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
 }
 
+
 /**void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
 *
 *   Set motor current limit from zero (p=0.0) to max determined by current sense resistors (p=1.0)
@@ -195,14 +202,22 @@
 *
 *   Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents
 */
+
 void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
 {
     const   uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input
-    if  (p < 0.0)
+    if  (p < 0.0 || ESTOP)
         p = 0.0;
     if  (p > 1.0)
         p = 1.0;
     last_I = p;     //  Retains last current limit demanded by driver
+    if  (DRIVING)    {
+        if  (Current_Scaler_Sep_2019 > 1.0)
+            Current_Scaler_Sep_2019 = 1.0;
+        if  (Current_Scaler_Sep_2019 < 0.0)
+            Current_Scaler_Sep_2019 = 0.0;
+        p *= Current_Scaler_Sep_2019;
+    }
     maxI.pulsewidth_ticks  ((uint32_t)(p * MPR));  //  PWM
 }
 
@@ -226,10 +241,10 @@
 #endif
 //  Feb 2019 - coefficients currently values in ram to allow for tweaking via command line. Will be 'const' once settled upon.
 //    const   double  samp_scale  = 0.35;                  //  Tweak this value only to tune filter
-    double  samp_scale  = s[1];                  //  Tweak this value only to tune filter
+    double  samp_scale  = sdbl[1];                  //  Tweak this value only to tune filter
     double  shrink_by   = 1.0 - samp_scale;
 //    const   double  dv_scale    =   0.15;
-    double  dv_scale    =   s[3];
+    double  dv_scale    =   sdbl[3];
     double  dv_shrink   = 1.0 - dv_scale;
     double  speed_error, d, t;
     uint32_t        Hall_tot_copy = Hall_total;     //  Copy value for use throughout function as may get changed at any instant during exec !
@@ -253,7 +268,7 @@
     
     if  (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE) {  //  Speed control only makes sense when motor runnable
         speed_error    = (target_speed - dMPH) / 1000.0;                //  'P' Proportional contribution to PID control
-        d = V_clamp + (speed_error * s[2]) + ((dv_by_dt / 1000.0) * s[4]);  //  Apply P+I+D (with I=0) control
+        d = V_clamp + (speed_error * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]);  //  Apply P+I+D (with I=0) control
         if  (d > 1.0)   d = 1.0;
         if  (d < 0.0)   d = 0.0;
         V_clamp = d;