Dual Brushless Motor ESC, 10-62V, up to 50A per motor. Motors ganged or independent, multiple control input methods, cycle-by-cycle current limit, speed mode and torque mode control. Motors tiny to kW. Speed limit and other parameters easily set in firmware. As used in 'The Brushless Brutalist' locomotive - www.jons-workshop.com. See also Model Engineer magazine June-October 2019.
Dependencies: mbed BufferedSerial Servo PCT2075 FastPWM
Update 17th August 2020 Radio control inputs completed
brushless_motor.cpp
- Committer:
- JonFreeman
- Date:
- 2019-11-30
- Revision:
- 14:acaa1add097b
- Parent:
- 13:ef7a06fa11de
- Child:
- 16:d1e4b9ad3b8b
File content as of revision 14:acaa1add097b:
/* STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control. Jon Freeman B. Eng Hons 2015 - 2019 */ #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 "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 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 OP = 0; 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 H1.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge H2.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge H2.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge 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 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); // 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 (); 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 last_V = last_I = 0.0; Idbl = 0.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 } /** * void brushless_motor::sniff_current () { // Initiate ADC current reading of approx motor average current * 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, top 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 () { int32_t delta_theta; const int32_t delta_theta_lut[] = { // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7 } ; Hall_index[0] = read_Halls (); delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]]; if (delta_theta == 0) encoder_error_cnt++; else angle_cnt += delta_theta; OP = lut[inner_mode | Hall_index[0]]; // changed mode to inner_mode 27/04/18 Hall_total++; Hall_index[1] = Hall_index[0]; } /** * void brushless_motor::direction_set (int dir) { * Used to set direction according to mode data from eeprom */ void brushless_motor::direction_set (int 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) x |= 1; if (H2) x |= 2; if (H3) x |= 4; return x; } 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 = OP; p &= lut[32]; // KEEP_L_MASK_A or B OP = p; } /* void brushless_motor::low_side_on () { maxV.pulsewidth_ticks (1); OP = lut[31]; // KEEP_L_MASK_A or B } */ 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) /** * 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 || 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) && 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_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 { 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; } maxI.pulsewidth_ticks ((uint32_t)(p * MPR)); // PWM } /** * 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 { #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; 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 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 } } /* 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 () { return moving_flag; } /** bool brushless_motor::set_mode (int m) Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE. If this causes change of mode, also sets V and I to zero. */ bool brushless_motor::set_mode (int 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); return false; } if (visible_mode != m) { // Mode change, kill volts and amps to be safe set_V_limit (0.0); set_I_limit (0.0); visible_mode = m; } 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 () // Energise Port with data determined by Hall sensors { Hall_index[0] = read_Halls (); OP = lut[inner_mode | Hall_index[0]]; }