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
Diff: brushless_motor.cpp
- Revision:
- 12:d1d21a2941ef
- Parent:
- 11:bfb73f083009
- Child:
- 13:ef7a06fa11de
--- 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]]; } +