Diff: brushless_motor.cpp

Revision:

16:d1e4b9ad3b8b

Parent:

14:acaa1add097b

Child:

17:cc9b854295d6

--- a/brushless_motor.cpp	Sat Nov 30 18:40:30 2019 +0000
+++ b/brushless_motor.cpp	Tue Jun 09 09:20:19 2020 +0000
@@ -1,7 +1,7 @@
 /*
     STM3_ESC    Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control.
     Jon Freeman  B. Eng Hons
-    2015 - 2019
+    2015 - 2020
 */
 #include "mbed.h"
 //#include    "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
@@ -9,19 +9,20 @@
 //#include    "stm32f411xe.h"
 #include "STM3_ESC.h"
 #include "BufferedSerial.h"
-#include "FastPWM.h"
-#include "Servo.h"
 #include "brushless_motor.h"
 
-extern  eeprom_settings     mode     ;
-extern  double  rpm2mph ;
-extern  BufferedSerial pc;            //  The two com ports used. There is also an unused com port, com3 setup @ 1200 baud
+extern  eeprom_settings     user_settings     ;
+//extern  BufferedSerial pc;            //  The two com ports used. There is also an unused com port, com3 setup @ 1200 baud
 
+/**
 brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
     const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum) 
     : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask)      //  Constructor
-{
-    //  Constructor
+*/
+brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
+    const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, uint16_t port_bit_mask, uint32_t rnum) 
+    : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask)      //  Constructor
+{    //  Constructor
     OP = 0;
     H1.mode (PullUp);   //  PullUp resistors enabled on all Hall sensor input pins
     H2.mode (PullUp);
@@ -41,7 +42,7 @@
     visible_mode    = MOTOR_REGENBRAKE;
     inner_mode      = MOTOR_REGENBRAKE;
     lut = lutptr;                       //  Pointer to motor energisation bit pattern table
-    current_sense_rs_offset = rnum;     //  This is position in mode.rd(current_sense_rs_offset)
+    current_sense_rs_offset = rnum;     //  This is position in user_settings.rd(current_sense_rs_offset)
     Hall_index[0] = Hall_index[1]  = read_Halls    ();
     tickleon    = 0;
     direction   = 0;
@@ -49,19 +50,11 @@
     encoder_error_cnt = 0;  //  Incremented when Hall transition not recognised as either direction
     last_V = last_I = 0.0;
     Idbl = 0.0;
-    numof_current_sense_rs = 1.0;
+//    numof_current_sense_rs = 1.0;
     RPM_filter  = 0.0;
-    dv_by_dt = 0.0;
-    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
-#ifdef  USING_DC_MOTORS
-    dc_motor = (Hall_index[0] == 7) ? true : false    ;
-#endif
 }
 
 /**
@@ -75,18 +68,15 @@
     const   double  sampweight  = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED    ;
     const   double  shrinkby    = 1.0 - sampweight;
     uint16_t    samp    = Motor_I.read_u16    (); //  CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019
-    double      dbls    = ((double)samp) * numof_current_sense_rs / 6.0;    //  reading in mA
+//    double      dbls    = ((double)samp) * numof_current_sense_rs / 6.0;    //  reading in mA
+//    double      dbls    = (((double)samp) / 6.0) * (double)user_settings.rd(current_sense_rs_offset);    //  reading in mA
+//    double      dbls    = sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0;    //  reading in mA
     Idbl *= shrinkby;              //  Jan 2019    New recursive low pass filter
-    Idbl += dbls * sampweight;     //  Current reading available, however not certain this is of any real use
+//    Idbl += dbls * sampweight;     //  Current reading available, however not certain this is of any real use
+    Idbl += sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0;     //  Current reading available, however not certain this is of any real use
 }
 
-bool    brushless_motor::poles  (int p)  {              //  Jan 2019 max_rom no longer used. target_speed is preferred
-    if  (!max_rpm)  {   //  Not been set since powerup
-        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, top speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph);
-    }
+bool    brushless_motor::poles  (uint32_t p)  {
     if  (p == 4 || p == 6 || p == 8)    {
         motor_poles = p;
         return  true;
@@ -110,8 +100,8 @@
     Hall_index[0]   = read_Halls    ();
     delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];
 
-    if  (delta_theta == 0)
-        encoder_error_cnt++;
+    if  (delta_theta == 0)  //  Should only ever be +1 in 1 direction, -1 in other direction. 0 indicates invalid Hall sensor transition detected.
+        encoder_error_cnt++;    //  Never used Dec 2019
     else
         angle_cnt += delta_theta;
     OP = lut[inner_mode | Hall_index[0]];  //  changed mode to inner_mode 27/04/18
@@ -120,17 +110,21 @@
 }
 
 /**
-*   void    brushless_motor::direction_set   (int dir)  {
-*   Used to set direction according to mode data from eeprom
+*   void    brushless_motor::set_direction   (int dir)  {
+*   Used to set direction according to user_settings data from eeprom
 */
-void    brushless_motor::direction_set   (int dir)
+void    brushless_motor::set_direction   (uint32_t dir)
 {
     direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; //  bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4
 }
 
-int     brushless_motor::read_Halls  ()
+//uint32_t    get_direction   ()  {
+//    return  direction;
+//}
+
+uint32_t    brushless_motor::read_Halls  ()
 {
-    int x = 0;
+    uint32_t x = 0;
     if  (H1)    x |= 1;
     if  (H2)    x |= 2;
     if  (H3)    x |= 4;
@@ -151,13 +145,7 @@
 }
 */
 
-void    brushless_motor::set_speed (double p) //  Sets target_speed
-{
-    target_speed = p;
-}
 
-
-extern  double  Current_Scaler_Sep_2019;
 extern  int WatchDog;
 #define DRIVING (visible_mode == MOTOR_FORWARD || visible_mode == MOTOR_REVERSE)
 #define ESTOP   (WatchDog == 0 && DRIVING)
@@ -167,6 +155,17 @@
 *
 *   Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0)
 */
+void    brushless_motor::motor_voltage_refresh ()  //  May alter motor voltage to reflect changes in V_clamp
+{
+    double  p = last_V;
+    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
+    p   = 1.0 - p;  //  because pwm is wrong way up
+    maxV.pulsewidth_ticks  ((int)(p * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
+}
+
 void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
 {
     if  (p < 0.0 || ESTOP)
@@ -174,13 +173,7 @@
     if  (p > 1.0)
         p = 1.0;
     last_V = p;     //  Retains last voltage limit demanded by driver
-    
-    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
-    p   = 1.0 - p;  //  because pwm is wrong way up
-    maxV.pulsewidth_ticks  ((int)(p * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
+    motor_voltage_refresh   ()  ;
 }
 
 
@@ -206,25 +199,29 @@
 *
 *   Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents
 */
+//const   double MPR = (double)((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input
+const   uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input
 
 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 || 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;
+        p *= current_scale;
     }
     maxI.pulsewidth_ticks  ((uint32_t)(p * MPR));  //  PWM
 }
 
+void    brushless_motor::I_scale (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
+{
+    current_scale = p;
+    if  (DRIVING)    {
+        maxI.pulsewidth_ticks  ((uint32_t)(last_I * p * MPR));  //  PWM
+    }
+}
 
 /**
 *   void    brushless_motor::speed_monitor_and_control   ()       //  ** CALL THIS 32 TIMES PER SECOND **
@@ -238,41 +235,41 @@
 *   Scope for further improvement of control algorithm - crude implementation of PID with no I
 */
 void    brushless_motor::speed_monitor_and_control   ()       //  call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
-{
-#ifdef  USING_DC_MOTORS     //  deprecated
-    if  (dc_motor)
-        return  0;
-#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  = 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    =   sdbl[3];
-    double  dv_shrink   = 1.0 - dv_scale;
-    double  speed_error, d, t;
+{               //
+//    const   double  samp_scale  = 0.275;                  //  Tweak this value only to tune filter
+    const   double  samp_scale  = 0.6;                  //  Increased May 2020 to improve ssl speed settling time
+    const   double  shrink_by   = 1.0 - samp_scale;
+    double  rpm, speed_error;
     uint32_t        Hall_tot_copy = Hall_total;     //  Copy value for use throughout function as may get changed at any instant during exec !
     moving_flag  = true;
     if  (Hall_previous == Hall_tot_copy)  {         //  Test for motor turning or not
         moving_flag  = false;                       //  Zero Hall transitions since previous call - motor not moving
         tickleon    = TICKLE_TIMES;                 //  May need to tickle some charge into high side switch bootstrap supplies
-    } 
-    d   = (double) ((Hall_tot_copy - Hall_previous) *640);  //  (Motor Hall sensor transitions in previous 31250us) * 640
-    d   /= motor_poles;                 //  d now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles
-    t   = RPM_filter;   //  Value from last time around
-    RPM_filter  *= shrink_by;
-    RPM_filter  += (d * samp_scale);  //  filtered RPM
-                                        //  RPM_filter[n] = shrink_by RPM_filter[n - 1] + samp_scale x[n]
-    t    -= RPM_filter;  //  more like minus dv/dt
-    dv_by_dt    *= dv_shrink;
-    dv_by_dt    += (t * dv_scale);    //  filtered rate of change, the 'D' Differential contribution to PID control
-    dRPM += RPM_filter;
-    dRPM /= 2.0;
-    dMPH    = RPM_filter * rpm2mph;     //  Completed updates of RPM and MPH
-    
-    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 * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]);  //  Apply P+I+D (with I=0) control
+    }
+    rpm   = (double) (((Hall_tot_copy - Hall_previous) * 640) / motor_poles);  //  (Motor Hall sensor transitions in previous 31250us) * 640
+    RPM_filter  *= shrink_by;   //      rpm now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles
+    RPM_filter  += (rpm * samp_scale);  //  filtered RPM
+    dRPM = RPM_filter;
+    dMPH    =  user_settings.rpm2mph(RPM_filter);     //  Completed updates of RPM and MPH
+    Hall_previous = Hall_tot_copy;
+    speed_error    = (dMPH - user_settings.top_speed());                //  'P' Proportional contribution to PID control
+    bool    clamp_change = false;
+    if  (speed_error > 0.0 && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) {  //  Speed control only makes sense when motor runnable
+        V_clamp *= (1.0 - (speed_error / 60.0));  //  Driving too fast, so lower clamp voltage a tiny bit
+        clamp_change = true;
+    }
+    else    {   //  Not going too fast, and maybe driving or not
+        if  (V_clamp < 0.99)    {
+            V_clamp += 0.015;
+            if  (V_clamp > 1.0)
+                V_clamp = 1.0;
+            clamp_change = true;
+        }
+    }
+    if  (clamp_change)  {
+        motor_voltage_refresh   ()  ;
+    }
+/*        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;
@@ -280,16 +277,20 @@
         {    
             d *= 0.95;   //  need limit, ffi see MCP1630 data
             d   = 1.0 - d;  //  because pwm is wrong way up
+//FUCKETYFUCK            maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
             maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
         }
-    }
+    }*/
 /*    temp_tick++;
-    if  (temp_tick > 35)    {   //  one and a bit second
+    if  (temp_tick > 50)    {   //  one and a bit second
         temp_tick = 0;
-        pc.printf   ("RPM %.0f, %.3f, %.3f, %.2f, dv/dt%.3f\r\n", dRPM, RPM_filter, d, dMPH, dv_by_dt);
+        pc.printf   ("top speed %.1f, mph %.1f, speed_err %.1f, V_clamp%.3f\r\n", user_settings.top_speed(), dMPH, speed_error, V_clamp);
     }
 */
-    Hall_previous = Hall_tot_copy;
+}
+
+bool    brushless_motor::exists ()  {
+    return  ((Hall_index[0] > 0) && (Hall_index[0] < 7))    ;
 }
 
 bool    brushless_motor::is_moving ()
@@ -299,10 +300,10 @@
 
 /**
 bool    brushless_motor::set_mode (int m)
-Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
+Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE. Used to also be used to set REGENBRAKE, replaced by brake(x) function.
 If this causes change of mode, also sets V and I to zero.
 */
-bool    brushless_motor::set_mode (int m)
+bool    brushless_motor::set_mode (uint32_t m)
 {
     if  ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE))  {
 //        pc.printf   ("Error in set_mode, invalid mode %d\r\n", m);
@@ -313,12 +314,30 @@
         set_I_limit (0.0);
         visible_mode = m;
     }
-    if  (m == MOTOR_FORWARD || m == MOTOR_REVERSE)
-        m ^= direction;
+    if  (m == MOTOR_FORWARD || m == MOTOR_REVERSE)  //  8 or 16 - these are effectively address bits of motor pattern lut
+        m ^= direction; //  direction set to 0 or (MOTOR_FORWARD | MOTOR_REVERSE), so has zero or two bits set
     inner_mode = m;     //  idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
     return  true;
 }
 
+void    brushless_motor::brake   (double brake_effort)    {
+//    pc.printf   ("In motor::brake, user_settings.brake_effectiveness = %3f\r\n", user_settings.brake_effectiveness());
+    set_mode    (MOTOR_REGENBRAKE);
+    if  (brake_effort > 1.0)
+        brake_effort = 1.0;
+    if  (brake_effort < 0.0)
+        brake_effort = 0.0;
+    brake_effort *= user_settings.brake_effectiveness();    //  set upper limit, this is essential - May2020 fixed, was 100 times too big
+    brake_effort = sqrt  (brake_effort);  //  to linearise effect
+    set_V_limit (brake_effort)  ;
+    set_I_limit (1.0);
+    V_clamp = 1.0;
+}
+
+uint32_t    brushless_motor::get_mode ()    {
+    return  visible_mode;
+}
+
 void    brushless_motor::motor_set  ()  //  Energise Port with data determined by Hall sensors
 {
     Hall_index[0]  = read_Halls    ();