brushless_motor.cpp

00001 /*
00002     STM3_ESC    Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control.
00003     Jon Freeman  B. Eng Hons
00004     2015 - 2020
00005 */
00006 #include "mbed.h"
00007 //#include    "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
00008 //#include    "stm32f401xe.h"
00009 //#include    "stm32f411xe.h"
00010 #include "STM3_ESC.h"
00011 #include "BufferedSerial.h"
00012 #include "brushless_motor.h"
00013 
00014 extern  eeprom_settings     user_settings     ;
00015 //extern  BufferedSerial pc;            //  The two com ports used. There is also an unused com port, com3 setup @ 1200 baud
00016 
00017 /**
00018 brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
00019     const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum) 
00020     : 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
00021 */
00022 brushless_motor::brushless_motor    (PinName iadc, PinName pwv, PinName pwi, 
00023     const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, const uint16_t port_bit_mask, const uint32_t rnum) 
00024     : 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
00025 {    //  Constructor
00026     OP = 0;
00027     H1.mode (PullUp);   //  PullUp resistors enabled on all Hall sensor input pins
00028     H2.mode (PullUp);
00029     H3.mode (PullUp);
00030     H1.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
00031     H1.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
00032     H2.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
00033     H2.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
00034     H3.rise (callback(this, &brushless_motor::Hall_change));     // Attach handler to the rising interruptIn edge
00035     H3.fall (callback(this, &brushless_motor::Hall_change));     // Attach handler to the falling interruptIn edge
00036     Hall_total = 0;  //  mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
00037     Hall_previous = 0;
00038     maxV.period_ticks      (MAX_PWM_TICKS + 1);  //  around 18 kHz
00039     maxI.period_ticks      (MAX_PWM_TICKS + 1);
00040     maxV.pulsewidth_ticks  (MAX_PWM_TICKS - 20);    //  Motor voltage pwm is inverted, see MCP1630 data
00041     maxI.pulsewidth_ticks  (MAX_PWM_TICKS / 30);    //  Motor current pwm is not inverted. Initial values for scope hanging test
00042     visible_mode    = MOTOR_REGENBRAKE;
00043     inner_mode      = MOTOR_REGENBRAKE;
00044     lut = lutptr;                       //  Pointer to motor energisation bit pattern table
00045     current_sense_rs_offset = rnum;     //  This is position in user_settings.rd(current_sense_rs_offset)
00046     Hall_index[0] = Hall_index[1]  = read_Halls    ();
00047     tickleon    = 0;
00048     direction   = 0;
00049     angle_cnt   = 0;        //  Incremented or decremented on each Hall event according to actual measured direction of travel
00050     encoder_error_cnt = 0;  //  Incremented when Hall transition not recognised as either direction
00051     last_V = last_I = 0.0;
00052     Idbl = 0.0;
00053 //    numof_current_sense_rs = 1.0;
00054     RPM_filter  = 0.0;
00055     dRPM        = 0.0;
00056     V_clamp =   1.0 ;       //  Used to limit top speed
00057     motor_poles   = 8;    //  Default to 8 pole motor
00058 }
00059 
00060 /**
00061 *   void    brushless_motor::sniff_current   ()  {          //  Initiate ADC current reading of approx motor average current
00062 *       This to be called in response to ticker timebase interrupt.
00063 *       As designed, called at 200 micro second intervals (Feb 2019)
00064 *       Updates double I.dbl current measured in milliamps
00065 *       Reading not used elsewhere in this code but made available through command line for external controller
00066 */
00067 void    brushless_motor::sniff_current   ()  {          //  Initiate ADC current reading
00068     const   double  sampweight  = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED    ;
00069     const   double  shrinkby    = 1.0 - sampweight;
00070     uint16_t    samp    = Motor_I.read_u16    (); //  CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019
00071 //    double      dbls    = ((double)samp) * numof_current_sense_rs / 6.0;    //  reading in mA
00072 //    double      dbls    = (((double)samp) / 6.0) * (double)user_settings.rd(current_sense_rs_offset);    //  reading in mA
00073 //    double      dbls    = sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0;    //  reading in mA
00074     Idbl *= shrinkby;              //  Jan 2019    New recursive low pass filter
00075 //    Idbl += dbls * sampweight;     //  Current reading available, however not certain this is of any real use
00076     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
00077 }
00078 
00079 bool    brushless_motor::poles  (uint32_t p)  {
00080     if  (p == 4 || p == 6 || p == 8)    {
00081         motor_poles = p;
00082         return  true;
00083     }
00084     return  false;
00085 }
00086 
00087 void    brushless_motor::Hall_change   ()  {
00088     int32_t     delta_theta;
00089     const   int32_t delta_theta_lut[] = {   //  Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
00090         0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 0
00091         0, 0, 0,-1, 0, 1, 0, 0,     //  Previous Hindex was 1
00092         0, 0, 0, 1, 0, 0,-1, 0,     //  Previous Hindex was 2
00093         0, 1,-1, 0, 0, 0, 0, 0,     //  Previous Hindex was 3
00094         0, 0, 0, 0, 0,-1, 1, 0,     //  Previous Hindex was 4
00095         0,-1, 0, 0, 1, 0, 0, 0,     //  Previous Hindex was 5
00096         0, 0, 1, 0,-1, 0, 0, 0,     //  Previous Hindex was 6
00097         0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 7
00098     }  ;
00099 
00100     Hall_index[0]   = read_Halls    ();
00101     delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];
00102 
00103     if  (delta_theta == 0)  //  Should only ever be +1 in 1 direction, -1 in other direction. 0 indicates invalid Hall sensor transition detected.
00104         encoder_error_cnt++;    //  Never used Dec 2019
00105     else
00106         angle_cnt += delta_theta;
00107     OP = lut[inner_mode | Hall_index[0]];  //  changed mode to inner_mode 27/04/18
00108     Hall_total++;
00109     Hall_index[1] = Hall_index[0];
00110 }
00111 
00112 /**
00113 *   void    brushless_motor::set_direction   (int dir)  {
00114 *   Used to set direction according to user_settings data from eeprom
00115 */
00116 void    brushless_motor::set_direction   (uint32_t dir)
00117 {
00118     direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; //  bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4
00119 }
00120 
00121 //uint32_t    get_direction   ()  {
00122 //    return  direction;
00123 //}
00124 
00125 uint32_t    brushless_motor::read_Halls  ()
00126 {
00127     uint32_t x = 0;
00128     if  (H1)    x |= 1;
00129     if  (H2)    x |= 2;
00130     if  (H3)    x |= 4;
00131     return  x;
00132 }
00133 
00134 void    brushless_motor::high_side_off   () //  Jan 2019    Only ever called from main when high side gate drive charge might need pumping up
00135 {
00136     uint16_t    p = OP;
00137     p &= lut[32];   //  KEEP_L_MASK_A or B
00138     OP = p;
00139 }
00140 /*
00141 void    brushless_motor::low_side_on   ()
00142 {
00143     maxV.pulsewidth_ticks  (1);
00144     OP = lut[31];   //  KEEP_L_MASK_A or B
00145 }
00146 */
00147 
00148 
00149 extern  int WatchDog;
00150 #define DRIVING (visible_mode == MOTOR_FORWARD || visible_mode == MOTOR_REVERSE)
00151 #define ESTOP   (WatchDog == 0 && DRIVING)
00152 
00153 /**
00154 *   void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
00155 *
00156 *   Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0)
00157 */
00158 void    brushless_motor::motor_voltage_refresh ()  //  May alter motor voltage to reflect changes in V_clamp
00159 {
00160     double  p = last_V;
00161     if  ((V_clamp < last_V) && DRIVING)    //  Jan 2019 limit top speed when driving
00162         p = V_clamp;    //  If motor runnable, set voltage limit to lower of last_V and V_clamp
00163 
00164     p *= 0.95;      //  need limit, ffi see MCP1630 data
00165     p   = 1.0 - p;  //  because pwm is wrong way up
00166     maxV.pulsewidth_ticks  ((int)(p * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
00167 }
00168 
00169 void    brushless_motor::set_V_limit (double p)  //  Sets max motor voltage.
00170 {
00171     if  (p < 0.0 || ESTOP)
00172         p = 0.0;
00173     if  (p > 1.0)
00174         p = 1.0;
00175     last_V = p;     //  Retains last voltage limit demanded by driver
00176     motor_voltage_refresh   ()  ;
00177 }
00178 
00179 
00180 /**void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
00181 *
00182 *   Set motor current limit from zero (p=0.0) to max determined by current sense resistors (p=1.0)
00183 *       Value sent to pwm with RC integrator acting as AnalogOut.
00184 *       pwm capable of 0.0 <= V_out <= 3.3. This feeds MCP1630 V_Ref, range 0 to 2.7v
00185 *       Therefore (2.7/3.3) = 0.82 factor included.
00186 *   Jan 2019 - As yet uncalibrated, so let's have a go at working it out!
00187 *       Voltage ax current sense resistors amplified by op-amp with gain 5.7 (see EasyPC file 'BrushlessSTM3.sch', U6, R63, R64)
00188 *       This then put through pot divider (10k with 4k7 to ground, gain 0.32) reducing overall gain to 1.8 (accurate enough)
00189 *       This connects to MCP1630 CS (current sense) pin which works over the voltage range 0.0 to 0.9v
00190 *       Therefore 0.5v across current sense resistor is sufficient to turn driver off.
00191 *       0.5v across 0.05 ohm gives 10A per current sense resistor fitted.
00192 *       ** NOTE ** This is fast acting cycle by cycle current limit, the 10A figure is therefore peak T_on current.
00193 *
00194 *   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.
00195 *   When either switch is off, inductor current continues to flow but not through current sense resistors, through a parasitic diode instead.
00196 *   Thus T_on current is measured, T_off current is not measured
00197 *   This means current reading should approximate to current taken from the supply. Motor average current may be considerably higher.
00198 *       During REGEN_BRAKING, current flows the 'wrong' way through sense resistors and can not be measured.
00199 *
00200 *   Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents
00201 */
00202 //const   double MPR = (double)((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input
00203 const   uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11);    //  Scales 3.3v pwm DAC output to 2.7v V_Ref input
00204 
00205 void    brushless_motor::set_I_limit (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
00206 {
00207     if  (p < 0.0 || ESTOP)
00208         p = 0.0;
00209     if  (p > 1.0)
00210         p = 1.0;
00211     last_I = p;     //  Retains last current limit demanded by driver
00212     if  (DRIVING)    {
00213         p *= current_scale;
00214     }
00215     maxI.pulsewidth_ticks  ((uint32_t)(p * MPR));  //  PWM
00216 }
00217 
00218 void    brushless_motor::I_scale (double p)     //  Sets max motor current. pwm integrated to dc ref voltage level
00219 {
00220     current_scale = p;
00221     if  (DRIVING)    {
00222         maxI.pulsewidth_ticks  ((uint32_t)(last_I * p * MPR));  //  PWM
00223     }
00224 }
00225 
00226 /**
00227 *   void    brushless_motor::speed_monitor_and_control   ()       //  ** CALL THIS 32 TIMES PER SECOND **
00228 *   Call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep RPM and MPH figures correct
00229 *   Tracks total transitions on Hall sensor inputs to determine speed.
00230 *   Sets variables double dRPM of motor RPM, and double dMPH miles per hour
00231 *
00232 *   Speed control - double target_speed as reference input. *
00233 *       **  This is where any speed limit gets applied **
00234 *           Motor voltage reduced when at or over speed. Does NOT apply any braking
00235 *   Scope for further improvement of control algorithm - crude implementation of PID with no I
00236 */
00237 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
00238 {               //
00239 //    const   double  samp_scale  = 0.275;                  //  Tweak this value only to tune filter
00240     const   double  samp_scale  = 0.6;                  //  Increased May 2020 to improve ssl speed settling time
00241     const   double  shrink_by   = 1.0 - samp_scale;
00242     double  rpm, speed_error;
00243     uint32_t        Hall_tot_copy = Hall_total;     //  Copy value for use throughout function as may get changed at any instant during exec !
00244     moving_flag  = true;
00245     if  (Hall_previous == Hall_tot_copy)  {         //  Test for motor turning or not
00246         moving_flag  = false;                       //  Zero Hall transitions since previous call - motor not moving
00247         tickleon    = TICKLE_TIMES;                 //  May need to tickle some charge into high side switch bootstrap supplies
00248     }
00249     rpm   = (double) (((Hall_tot_copy - Hall_previous) * 640) / motor_poles);  //  (Motor Hall sensor transitions in previous 31250us) * 640
00250     RPM_filter  *= shrink_by;   //      rpm now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles
00251     RPM_filter  += (rpm * samp_scale);  //  filtered RPM
00252     dRPM = RPM_filter;
00253     dMPH    =  user_settings.rpm2mph(RPM_filter);     //  Completed updates of RPM and MPH
00254     Hall_previous = Hall_tot_copy;
00255     speed_error    = (dMPH - user_settings.top_speed());                //  'P' Proportional contribution to PID control
00256     bool    clamp_change = false;
00257     if  (speed_error > 0.0 && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) {  //  Speed control only makes sense when motor runnable
00258         V_clamp *= (1.0 - (speed_error / 60.0));  //  Driving too fast, so lower clamp voltage a tiny bit
00259         clamp_change = true;
00260     }
00261     else    {   //  Not going too fast, and maybe driving or not
00262         if  (V_clamp < 0.99)    {
00263             V_clamp += 0.015;
00264             if  (V_clamp > 1.0)
00265                 V_clamp = 1.0;
00266             clamp_change = true;
00267         }
00268     }
00269     if  (clamp_change)  {
00270         motor_voltage_refresh   ()  ;
00271     }
00272 /*        d = V_clamp + (speed_error * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]);  //  Apply P+I+D (with I=0) control
00273         if  (d > 1.0)   d = 1.0;
00274         if  (d < 0.0)   d = 0.0;
00275         V_clamp = d;
00276         if  (V_clamp < last_V)   //  Jan 2019 limit top speed when driving
00277         {    
00278             d *= 0.95;   //  need limit, ffi see MCP1630 data
00279             d   = 1.0 - d;  //  because pwm is wrong way up
00280 //FUCKETYFUCK            maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
00281             maxV.pulsewidth_ticks  ((int)(d * MAX_PWM_TICKS));  //  PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
00282         }
00283     }*/
00284 /*    temp_tick++;
00285     if  (temp_tick > 50)    {   //  one and a bit second
00286         temp_tick = 0;
00287         pc.printf   ("top speed %.1f, mph %.1f, speed_err %.1f, V_clamp%.3f\r\n", user_settings.top_speed(), dMPH, speed_error, V_clamp);
00288     }
00289 */
00290 }
00291 
00292 bool    brushless_motor::exists ()  {
00293     return  ((Hall_index[0] > 0) && (Hall_index[0] < 7))    ;
00294 }
00295 
00296 bool    brushless_motor::is_moving ()
00297 {
00298     return  moving_flag;
00299 }
00300 
00301 /**
00302 bool    brushless_motor::set_mode (int m)
00303 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE. Used to also be used to set REGENBRAKE, replaced by brake(x) function.
00304 If this causes change of mode, also sets V and I to zero.
00305 */
00306 bool    brushless_motor::set_mode (uint32_t m)
00307 {
00308     if  ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE))  {
00309 //        pc.printf   ("Error in set_mode, invalid mode %d\r\n", m);
00310         return  false;
00311     }
00312     if  (visible_mode != m) {   //  Mode change, kill volts and amps to be safe
00313         set_V_limit (0.0);
00314         set_I_limit (0.0);
00315         visible_mode = m;
00316     }
00317     if  (m == MOTOR_FORWARD || m == MOTOR_REVERSE)  //  8 or 16 - these are effectively address bits of motor pattern lut
00318         m ^= direction; //  direction set to 0 or (MOTOR_FORWARD | MOTOR_REVERSE), so has zero or two bits set
00319     inner_mode = m;     //  idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
00320     return  true;
00321 }
00322 
00323 void    brushless_motor::brake   (double brake_effort)    {
00324 //    pc.printf   ("In motor::brake, user_settings.brake_effectiveness = %3f\r\n", user_settings.brake_effectiveness());
00325     set_mode    (MOTOR_REGENBRAKE);
00326     if  (brake_effort > 1.0)
00327         brake_effort = 1.0;
00328     if  (brake_effort < 0.0)
00329         brake_effort = 0.0;
00330     brake_effort *= user_settings.brake_effectiveness();    //  set upper limit, this is essential - May2020 fixed, was 100 times too big
00331     brake_effort = sqrt  (brake_effort);  //  to linearise effect
00332     set_V_limit (brake_effort)  ;
00333     set_I_limit (1.0);
00334     V_clamp = 1.0;
00335 }
00336 
00337 uint32_t    brushless_motor::get_mode ()    {
00338     return  visible_mode;
00339 }
00340 
00341 void    brushless_motor::motor_set  ()  //  Energise Port with data determined by Hall sensors
00342 {
00343     Hall_index[0]  = read_Halls    ();
00344     OP = lut[inner_mode | Hall_index[0]];
00345 }
00346