Diff: brushless_motor.cpp

Revision:

12:d1d21a2941ef

Parent:

11:bfb73f083009

--- a/brushless_motor.cpp	Sat Jan 19 11:45:01 2019 +0000
+++ b/brushless_motor.cpp	Mon Mar 04 17:51:08 2019 +0000
@@ -8,14 +8,17 @@
 #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
 
 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) 
+    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
     OP = 0;
-    H1.mode (PullUp);
+    H1.mode (PullUp);   //  PullUp resistors enabled on all Hall sensor input pins
     H2.mode (PullUp);
     H3.mode (PullUp);
     H1.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
@@ -25,34 +28,65 @@
     H3.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
     H3.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
     Hall_total = 0;  //  mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
-    latest_pulses_per_sec = 0;
-    for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
-        edge_count_table[i] = 0;
-    Hall_tab_ptr = 0;
+    Hall_previous = 0;
     maxV.period_ticks      (MAX_PWM_TICKS + 1);  //  around 18 kHz
     maxI.period_ticks      (MAX_PWM_TICKS + 1);
-    maxV.pulsewidth_ticks  (MAX_PWM_TICKS / 20);
-    maxI.pulsewidth_ticks  (MAX_PWM_TICKS / 30);
-    visible_mode    = REGENBRAKE;
-    inner_mode      = REGENBRAKE;
-    lut = lutptr;
+    maxV.pulsewidth_ticks  (MAX_PWM_TICKS - 20);    //  Motor voltage pwm is inverted, see MCP1630 data
+    maxI.pulsewidth_ticks  (MAX_PWM_TICKS / 30);    //  Motor current pwm is not inverted. Initial values for scope hanging test
+    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)
     Hall_index[0] = Hall_index[1]  = read_Halls    ();
-    ppstmp  = 0;
-    cs_ptr = 0;
     tickleon    = 0;
     direction   = 0;
     angle_cnt   = 0;        //  Incremented or decremented on each Hall event according to actual measured direction of travel
     encoder_error_cnt = 0;  //  Incremented when Hall transition not recognised as either direction
-    PPS     = 0;
-    RPM     = 0;
     last_V = last_I = 0.0;
+    Idbl = 0.0;
+    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;
+    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
 }
 
-void    brushless_motor::sniff_current   ()  {
-    current_samples[cs_ptr++] = Motor_I.read_u16    (); //
-    if  (cs_ptr >= CURRENT_SAMPLES_AVERAGED)
-        cs_ptr = 0;
+/**
+*   void    brushless_motor::sniff_current   ()  {          //  Initiate ADC current reading
+*       This to be called in response to ticker timebase interrupt.
+*       As designed, called at 200 micro second intervals (Feb 2019)
+*       Updates double I.dbl current measured in milliamps
+*       Reading not used elsewhere in this code but made available through command line for external controller
+*/
+void    brushless_motor::sniff_current   ()  {          //  Initiate ADC current reading
+    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
+    Idbl *= shrinkby;              //  Jan 2019    New recursive low pass filter
+    Idbl += dbls * sampweight;     //  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, tp speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph);
+    }
+    if  (p == 4 || p == 6 || p == 8)    {
+        motor_poles = p;
+        return  true;
+    }
+    return  false;
 }
 
 void    brushless_motor::Hall_change   ()  {
@@ -80,127 +114,163 @@
     Hall_index[1] = Hall_index[0];
 }
 
-bool    brushless_motor::motor_is_brushless   ()
-{
-    /*    int x = read_Halls  ();
-        if  (x < 1 || x > 6)
-            return  false;
-        return  true;
-        */
-    return  !dc_motor;
-}
-
 /**
-void    brushless_motor::direction_set   (int dir)  {
-Used to set direction according to mode data from eeprom
+*   void    brushless_motor::direction_set   (int dir)  {
+*   Used to set direction according to mode data from eeprom
 */
 void    brushless_motor::direction_set   (int dir)
 {
-    if  (dir != 0)
-        dir = FORWARD | REVERSE;  //  bits used in eor
-    direction = dir;
+    direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; //  bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4
 }
 
 int     brushless_motor::read_Halls  ()
 {
     int x = 0;
-    if  (H1 != 0)    x |= 1;
-    if  (H2 != 0)    x |= 2;
-    if  (H3 != 0)    x |= 4;
+    if  (H1)    x |= 1;
+    if  (H2)    x |= 2;
+    if  (H3)    x |= 4;
     return  x;
 }
 
-void    brushless_motor::high_side_off   ()
+void    brushless_motor::high_side_off   () //  Jan 2019    Only ever called from main when high side gate drive charge might need pumping up
 {
-//    uint16_t    p = *Motor_Port;
     uint16_t    p = OP;
     p &= lut[32];   //  KEEP_L_MASK_A or B
-//    *Motor_Port = p;
     OP = p;
 }
-
+/*
 void    brushless_motor::low_side_on   ()
 {
-//    uint16_t    p = *Motor_Port;
-//    p &= lut[31];   //  KEEP_L_MASK_A or B
-//    *Motor_Port = lut[31];
-    OP = lut[31];
+    maxV.pulsewidth_ticks  (1);
+    OP = lut[31];   //  KEEP_L_MASK_A or B
 }
+*/
 
-void    brushless_motor::current_calc ()
+void    brushless_motor::set_speed (double p) //  Sets target_speed
 {
-    I.min = 0x0fffffff; //  samples are 16 bit
-    I.max = 0;
-    I.ave = 0;
-    uint16_t    sample;
-    for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++)  {
-        sample  = current_samples[i];
-        I.ave += sample;
-        if  (I.min > sample)
-            I.min   = sample;
-        if  (I.max < sample)
-            I.max   = sample;
-    }
-    I.ave /= CURRENT_SAMPLES_AVERAGED;
+    target_speed = p;
 }
 
 
-void    brushless_motor::raw_V_pwm    (int    v)
-{
-    if  (v < 1) v = 1;
-    if  (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
-    maxV.pulsewidth_ticks  (v);
-}
-
-void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
+/**
+*   void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
+*
+*   Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0)
+*/
+void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
 {
     if  (p < 0.0)
         p = 0.0;
     if  (p > 1.0)
         p = 1.0;
-    last_V = p;     //  for read by diagnostics
-    p *= 0.95;   //  need limit, ffi see MCP1630 data
+    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
+        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_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)
+*       Value sent to pwm with RC integrator acting as AnalogOut.
+*       pwm capable of 0.0 <= V_out <= 3.3. This feeds MCP1630 V_Ref, range 0 to 2.7v
+*       Therefore (2.7/3.3) = 0.82 factor included.
+*   Jan 2019 - As yet uncalibrated, so let's have a go at working it out!
+*       Voltage ax current sense resistors amplified by op-amp with gain 5.7 (see EasyPC file 'BrushlessSTM3.sch', U6, R63, R64)
+*       This then put through pot divider (10k with 4k7 to ground, gain 0.32) reducing overall gain to 1.8 (accurate enough)
+*       This connects to MCP1630 CS (current sense) pin which works over the voltage range 0.0 to 0.9v
+*       Therefore 0.5v across current sense resistor is sufficient to turn driver off.
+*       0.5v across 0.05 ohm gives 10A per current sense resistor fitted.
+*       ** NOTE ** This is fast acting cycle by cycle current limit, the 10A figure is therefore peak T_on current.
+*
+*   Current flows through current sense reaistor when one high side and one low side switch are on, expect a rising ramp due to motor inductance.
+*   When either switch is off, inductor current continues to flow but not through current sense resistors, through a parasitic diode instead.
+*   Thus T_on current is measured, T_off current is not measured
+*   This means current reading should approximate to current taken from the supply. Motor average current may be considerably higher.
+*       During REGEN_BRAKING, current flows the 'wrong' way through sense resistors and can not be measured.
+*
+*   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
 {
-    int a;
+    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)
         p = 0.0;
     if  (p > 1.0)
         p = 1.0;
-    last_I = p;
-    a = (int)(p * MAX_PWM_TICKS);
-    if  (a > MAX_PWM_TICKS)
-        a = MAX_PWM_TICKS;
-    if  (a < 0)
-        a = 0;
-    maxI.pulsewidth_ticks  (a);  //  PWM
+    last_I = p;     //  Retains last current limit demanded by driver
+    maxI.pulsewidth_ticks  ((uint32_t)(p * MPR));  //  PWM
 }
 
-uint32_t    brushless_motor::pulses_per_sec   ()       //  call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
+
+/**
+*   void    brushless_motor::speed_monitor_and_control   ()       //  ** CALL THIS 32 TIMES PER SECOND **
+*   Call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep RPM and MPH figures correct
+*   Tracks total transitions on Hall sensor inputs to determine speed.
+*   Sets variables double dRPM of motor RPM, and double dMPH miles per hour
+*
+*   Speed control - double target_speed as reference input. *
+*       **  This is where any speed limit gets applied **
+*           Motor voltage reduced when at or over speed. Does NOT apply any braking
+*   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
 {
-    //  Can also test for motor running or not here
+#ifdef  USING_DC_MOTORS
     if  (dc_motor)
         return  0;
-    if  (ppstmp == Hall_total)  {
-//    if  (dc_motor || ppstmp == Hall_total)  {
-        moving_flag  = false;       //  Zero Hall transitions since previous call - motor not moving
-        tickleon    = TICKLE_TIMES;
-    } else    {
-        moving_flag  = true;
-        ppstmp = Hall_total;
+#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  shrink_by   = 1.0 - samp_scale;
+//    const   double  dv_scale    =   0.15;
+    double  dv_scale    =   s[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 !
+    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 * s[2]) + ((dv_by_dt / 1000.0) * s[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;
+        if  (V_clamp < last_V)   //  Jan 2019 limit top speed when driving
+        {    
+            d *= 0.95;   //  need limit, ffi see MCP1630 data
+            d   = 1.0 - d;  //  because pwm is wrong way up
+            maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
+        }
     }
-    latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
-    edge_count_table[Hall_tab_ptr] = ppstmp;
-    Hall_tab_ptr++;
-    if  (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
-        Hall_tab_ptr = 0;
-    PPS = latest_pulses_per_sec;
-    RPM = (latest_pulses_per_sec * 60) / 24;
-    return  latest_pulses_per_sec;
+/*    temp_tick++;
+    if  (temp_tick > 35)    {   //  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);
+    }
+*/
+    Hall_previous = Hall_tot_copy;
 }
 
 bool    brushless_motor::is_moving ()
@@ -215,7 +285,7 @@
 */
 bool    brushless_motor::set_mode (int m)
 {
-    if  ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE))  {
+    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);
         return  false;
     }
@@ -224,16 +294,15 @@
         set_I_limit (0.0);
         visible_mode = m;
     }
-    if  (m == FORWARD || m == REVERSE)
+    if  (m == MOTOR_FORWARD || m == MOTOR_REVERSE)
         m ^= direction;
     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::motor_set  ()
+void    brushless_motor::motor_set  ()  //  Energise Port with data determined by Hall sensors
 {
     Hall_index[0]  = read_Halls    ();
-//    *Motor_Port = lut[inner_mode | Hindex[0]];
     OP = lut[inner_mode | Hall_index[0]];
 }
+