Jon Freeman / Mbed 2 deprecated Brushless_STM3_ESC_2019_10

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@17:cc9b854295d6, 2020-08-16 (annotated)

Committer:
JonFreeman
Date:
Sun Aug 16 14:13:19 2020 +0000
Revision:
17:cc9b854295d6
Parent:
16:d1e4b9ad3b8b 
August 2020. Checked Radio Control input ops.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 14:acaa1add097b 1 /*
JonFreeman 14:acaa1add097b 2 STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control.
JonFreeman 14:acaa1add097b 3 Jon Freeman B. Eng Hons
JonFreeman 16:d1e4b9ad3b8b 4 2015 - 2020
JonFreeman 14:acaa1add097b 5 */
JonFreeman 10:e40d8724268a 6 #include "mbed.h"
JonFreeman 10:e40d8724268a 7 //#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
JonFreeman 13:ef7a06fa11de 8 //#include "stm32f401xe.h"
JonFreeman 13:ef7a06fa11de 9 //#include "stm32f411xe.h"
JonFreeman 13:ef7a06fa11de 10 #include "STM3_ESC.h"
JonFreeman 10:e40d8724268a 11 #include "BufferedSerial.h"
JonFreeman 10:e40d8724268a 12 #include "brushless_motor.h"
JonFreeman 11:bfb73f083009 13
JonFreeman 16:d1e4b9ad3b8b 14 extern eeprom_settings user_settings ;
JonFreeman 16:d1e4b9ad3b8b 15 //extern BufferedSerial pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud
JonFreeman 11:bfb73f083009 16
JonFreeman 16:d1e4b9ad3b8b 17 /**
JonFreeman 11:bfb73f083009 18 brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi,
JonFreeman 12:d1d21a2941ef 19 const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum)
JonFreeman 11:bfb73f083009 20 : 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
JonFreeman 16:d1e4b9ad3b8b 21 */
JonFreeman 16:d1e4b9ad3b8b 22 brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi,
JonFreeman 17:cc9b854295d6 23 const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, const uint16_t port_bit_mask, const uint32_t rnum)
JonFreeman 16:d1e4b9ad3b8b 24 : 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
JonFreeman 16:d1e4b9ad3b8b 25 { // Constructor
JonFreeman 11:bfb73f083009 26 OP = 0;
JonFreeman 12:d1d21a2941ef 27 H1.mode (PullUp); // PullUp resistors enabled on all Hall sensor input pins
JonFreeman 11:bfb73f083009 28 H2.mode (PullUp);
JonFreeman 11:bfb73f083009 29 H3.mode (PullUp);
JonFreeman 11:bfb73f083009 30 H1.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
JonFreeman 11:bfb73f083009 31 H1.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
JonFreeman 11:bfb73f083009 32 H2.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
JonFreeman 11:bfb73f083009 33 H2.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
JonFreeman 11:bfb73f083009 34 H3.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
JonFreeman 11:bfb73f083009 35 H3.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
JonFreeman 10:e40d8724268a 36 Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
JonFreeman 12:d1d21a2941ef 37 Hall_previous = 0;
JonFreeman 11:bfb73f083009 38 maxV.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 11:bfb73f083009 39 maxI.period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 12:d1d21a2941ef 40 maxV.pulsewidth_ticks (MAX_PWM_TICKS - 20); // Motor voltage pwm is inverted, see MCP1630 data
JonFreeman 12:d1d21a2941ef 41 maxI.pulsewidth_ticks (MAX_PWM_TICKS / 30); // Motor current pwm is not inverted. Initial values for scope hanging test
JonFreeman 12:d1d21a2941ef 42 visible_mode = MOTOR_REGENBRAKE;
JonFreeman 12:d1d21a2941ef 43 inner_mode = MOTOR_REGENBRAKE;
JonFreeman 12:d1d21a2941ef 44 lut = lutptr; // Pointer to motor energisation bit pattern table
JonFreeman 16:d1e4b9ad3b8b 45 current_sense_rs_offset = rnum; // This is position in user_settings.rd(current_sense_rs_offset)
JonFreeman 11:bfb73f083009 46 Hall_index[0] = Hall_index[1] = read_Halls ();
JonFreeman 10:e40d8724268a 47 tickleon = 0;
JonFreeman 10:e40d8724268a 48 direction = 0;
JonFreeman 10:e40d8724268a 49 angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel
JonFreeman 10:e40d8724268a 50 encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction
JonFreeman 10:e40d8724268a 51 last_V = last_I = 0.0;
JonFreeman 12:d1d21a2941ef 52 Idbl = 0.0;
JonFreeman 16:d1e4b9ad3b8b 53 // numof_current_sense_rs = 1.0;
JonFreeman 12:d1d21a2941ef 54 RPM_filter = 0.0;
JonFreeman 12:d1d21a2941ef 55 dRPM = 0.0;
JonFreeman 12:d1d21a2941ef 56 V_clamp = 1.0 ; // Used to limit top speed
JonFreeman 12:d1d21a2941ef 57 motor_poles = 8; // Default to 8 pole motor
JonFreeman 11:bfb73f083009 58 }
JonFreeman 11:bfb73f083009 59
JonFreeman 12:d1d21a2941ef 60 /**
JonFreeman 14:acaa1add097b 61 * void brushless_motor::sniff_current () { // Initiate ADC current reading of approx motor average current
JonFreeman 12:d1d21a2941ef 62 * This to be called in response to ticker timebase interrupt.
JonFreeman 12:d1d21a2941ef 63 * As designed, called at 200 micro second intervals (Feb 2019)
JonFreeman 12:d1d21a2941ef 64 * Updates double I.dbl current measured in milliamps
JonFreeman 12:d1d21a2941ef 65 * Reading not used elsewhere in this code but made available through command line for external controller
JonFreeman 12:d1d21a2941ef 66 */
JonFreeman 12:d1d21a2941ef 67 void brushless_motor::sniff_current () { // Initiate ADC current reading
JonFreeman 12:d1d21a2941ef 68 const double sampweight = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED ;
JonFreeman 12:d1d21a2941ef 69 const double shrinkby = 1.0 - sampweight;
JonFreeman 12:d1d21a2941ef 70 uint16_t samp = Motor_I.read_u16 (); // CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019
JonFreeman 16:d1e4b9ad3b8b 71 // double dbls = ((double)samp) * numof_current_sense_rs / 6.0; // reading in mA
JonFreeman 16:d1e4b9ad3b8b 72 // double dbls = (((double)samp) / 6.0) * (double)user_settings.rd(current_sense_rs_offset); // reading in mA
JonFreeman 16:d1e4b9ad3b8b 73 // double dbls = sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0; // reading in mA
JonFreeman 12:d1d21a2941ef 74 Idbl *= shrinkby; // Jan 2019 New recursive low pass filter
JonFreeman 16:d1e4b9ad3b8b 75 // Idbl += dbls * sampweight; // Current reading available, however not certain this is of any real use
JonFreeman 16:d1e4b9ad3b8b 76 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
JonFreeman 12:d1d21a2941ef 77 }
JonFreeman 12:d1d21a2941ef 78
JonFreeman 16:d1e4b9ad3b8b 79 bool brushless_motor::poles (uint32_t p) {
JonFreeman 12:d1d21a2941ef 80 if (p == 4 || p == 6 || p == 8) {
JonFreeman 12:d1d21a2941ef 81 motor_poles = p;
JonFreeman 12:d1d21a2941ef 82 return true;
JonFreeman 12:d1d21a2941ef 83 }
JonFreeman 12:d1d21a2941ef 84 return false;
JonFreeman 11:bfb73f083009 85 }
JonFreeman 11:bfb73f083009 86
JonFreeman 11:bfb73f083009 87 void brushless_motor::Hall_change () {
JonFreeman 11:bfb73f083009 88 int32_t delta_theta;
JonFreeman 11:bfb73f083009 89 const int32_t delta_theta_lut[] = { // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
JonFreeman 11:bfb73f083009 90 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
JonFreeman 11:bfb73f083009 91 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
JonFreeman 11:bfb73f083009 92 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
JonFreeman 11:bfb73f083009 93 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
JonFreeman 11:bfb73f083009 94 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
JonFreeman 11:bfb73f083009 95 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
JonFreeman 11:bfb73f083009 96 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
JonFreeman 11:bfb73f083009 97 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
JonFreeman 11:bfb73f083009 98 } ;
JonFreeman 11:bfb73f083009 99
JonFreeman 11:bfb73f083009 100 Hall_index[0] = read_Halls ();
JonFreeman 11:bfb73f083009 101 delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];
JonFreeman 11:bfb73f083009 102
JonFreeman 16:d1e4b9ad3b8b 103 if (delta_theta == 0) // Should only ever be +1 in 1 direction, -1 in other direction. 0 indicates invalid Hall sensor transition detected.
JonFreeman 16:d1e4b9ad3b8b 104 encoder_error_cnt++; // Never used Dec 2019
JonFreeman 10:e40d8724268a 105 else
JonFreeman 11:bfb73f083009 106 angle_cnt += delta_theta;
JonFreeman 11:bfb73f083009 107 OP = lut[inner_mode | Hall_index[0]]; // changed mode to inner_mode 27/04/18
JonFreeman 11:bfb73f083009 108 Hall_total++;
JonFreeman 11:bfb73f083009 109 Hall_index[1] = Hall_index[0];
JonFreeman 10:e40d8724268a 110 }
JonFreeman 10:e40d8724268a 111
JonFreeman 10:e40d8724268a 112 /**
JonFreeman 16:d1e4b9ad3b8b 113 * void brushless_motor::set_direction (int dir) {
JonFreeman 16:d1e4b9ad3b8b 114 * Used to set direction according to user_settings data from eeprom
JonFreeman 10:e40d8724268a 115 */
JonFreeman 16:d1e4b9ad3b8b 116 void brushless_motor::set_direction (uint32_t dir)
JonFreeman 10:e40d8724268a 117 {
JonFreeman 12:d1d21a2941ef 118 direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; // bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4
JonFreeman 10:e40d8724268a 119 }
JonFreeman 10:e40d8724268a 120
JonFreeman 16:d1e4b9ad3b8b 121 //uint32_t get_direction () {
JonFreeman 16:d1e4b9ad3b8b 122 // return direction;
JonFreeman 16:d1e4b9ad3b8b 123 //}
JonFreeman 16:d1e4b9ad3b8b 124
JonFreeman 16:d1e4b9ad3b8b 125 uint32_t brushless_motor::read_Halls ()
JonFreeman 10:e40d8724268a 126 {
JonFreeman 16:d1e4b9ad3b8b 127 uint32_t x = 0;
JonFreeman 12:d1d21a2941ef 128 if (H1) x |= 1;
JonFreeman 12:d1d21a2941ef 129 if (H2) x |= 2;
JonFreeman 12:d1d21a2941ef 130 if (H3) x |= 4;
JonFreeman 10:e40d8724268a 131 return x;
JonFreeman 10:e40d8724268a 132 }
JonFreeman 10:e40d8724268a 133
JonFreeman 12:d1d21a2941ef 134 void brushless_motor::high_side_off () // Jan 2019 Only ever called from main when high side gate drive charge might need pumping up
JonFreeman 10:e40d8724268a 135 {
JonFreeman 11:bfb73f083009 136 uint16_t p = OP;
JonFreeman 10:e40d8724268a 137 p &= lut[32]; // KEEP_L_MASK_A or B
JonFreeman 11:bfb73f083009 138 OP = p;
JonFreeman 10:e40d8724268a 139 }
JonFreeman 12:d1d21a2941ef 140 /*
JonFreeman 10:e40d8724268a 141 void brushless_motor::low_side_on ()
JonFreeman 10:e40d8724268a 142 {
JonFreeman 12:d1d21a2941ef 143 maxV.pulsewidth_ticks (1);
JonFreeman 12:d1d21a2941ef 144 OP = lut[31]; // KEEP_L_MASK_A or B
JonFreeman 10:e40d8724268a 145 }
JonFreeman 12:d1d21a2941ef 146 */
JonFreeman 10:e40d8724268a 147
JonFreeman 10:e40d8724268a 148
JonFreeman 13:ef7a06fa11de 149 extern int WatchDog;
JonFreeman 13:ef7a06fa11de 150 #define DRIVING (visible_mode == MOTOR_FORWARD || visible_mode == MOTOR_REVERSE)
JonFreeman 13:ef7a06fa11de 151 #define ESTOP (WatchDog == 0 && DRIVING)
JonFreeman 13:ef7a06fa11de 152
JonFreeman 12:d1d21a2941ef 153 /**
JonFreeman 12:d1d21a2941ef 154 * void brushless_motor::set_V_limit (double p) // Sets max motor voltage.
JonFreeman 12:d1d21a2941ef 155 *
JonFreeman 12:d1d21a2941ef 156 * Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0)
JonFreeman 12:d1d21a2941ef 157 */
JonFreeman 16:d1e4b9ad3b8b 158 void brushless_motor::motor_voltage_refresh () // May alter motor voltage to reflect changes in V_clamp
JonFreeman 16:d1e4b9ad3b8b 159 {
JonFreeman 16:d1e4b9ad3b8b 160 double p = last_V;
JonFreeman 16:d1e4b9ad3b8b 161 if ((V_clamp < last_V) && DRIVING) // Jan 2019 limit top speed when driving
JonFreeman 16:d1e4b9ad3b8b 162 p = V_clamp; // If motor runnable, set voltage limit to lower of last_V and V_clamp
JonFreeman 16:d1e4b9ad3b8b 163
JonFreeman 16:d1e4b9ad3b8b 164 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 16:d1e4b9ad3b8b 165 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 16:d1e4b9ad3b8b 166 maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 16:d1e4b9ad3b8b 167 }
JonFreeman 16:d1e4b9ad3b8b 168
JonFreeman 12:d1d21a2941ef 169 void brushless_motor::set_V_limit (double p) // Sets max motor voltage.
JonFreeman 10:e40d8724268a 170 {
JonFreeman 13:ef7a06fa11de 171 if (p < 0.0 || ESTOP)
JonFreeman 10:e40d8724268a 172 p = 0.0;
JonFreeman 10:e40d8724268a 173 if (p > 1.0)
JonFreeman 10:e40d8724268a 174 p = 1.0;
JonFreeman 12:d1d21a2941ef 175 last_V = p; // Retains last voltage limit demanded by driver
JonFreeman 16:d1e4b9ad3b8b 176 motor_voltage_refresh () ;
JonFreeman 10:e40d8724268a 177 }
JonFreeman 10:e40d8724268a 178
JonFreeman 13:ef7a06fa11de 179
JonFreeman 12:d1d21a2941ef 180 /**void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 12:d1d21a2941ef 181 *
JonFreeman 12:d1d21a2941ef 182 * Set motor current limit from zero (p=0.0) to max determined by current sense resistors (p=1.0)
JonFreeman 12:d1d21a2941ef 183 * Value sent to pwm with RC integrator acting as AnalogOut.
JonFreeman 12:d1d21a2941ef 184 * pwm capable of 0.0 <= V_out <= 3.3. This feeds MCP1630 V_Ref, range 0 to 2.7v
JonFreeman 12:d1d21a2941ef 185 * Therefore (2.7/3.3) = 0.82 factor included.
JonFreeman 12:d1d21a2941ef 186 * Jan 2019 - As yet uncalibrated, so let's have a go at working it out!
JonFreeman 12:d1d21a2941ef 187 * Voltage ax current sense resistors amplified by op-amp with gain 5.7 (see EasyPC file 'BrushlessSTM3.sch', U6, R63, R64)
JonFreeman 12:d1d21a2941ef 188 * This then put through pot divider (10k with 4k7 to ground, gain 0.32) reducing overall gain to 1.8 (accurate enough)
JonFreeman 12:d1d21a2941ef 189 * This connects to MCP1630 CS (current sense) pin which works over the voltage range 0.0 to 0.9v
JonFreeman 12:d1d21a2941ef 190 * Therefore 0.5v across current sense resistor is sufficient to turn driver off.
JonFreeman 12:d1d21a2941ef 191 * 0.5v across 0.05 ohm gives 10A per current sense resistor fitted.
JonFreeman 12:d1d21a2941ef 192 * ** NOTE ** This is fast acting cycle by cycle current limit, the 10A figure is therefore peak T_on current.
JonFreeman 12:d1d21a2941ef 193 *
JonFreeman 12:d1d21a2941ef 194 * 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.
JonFreeman 12:d1d21a2941ef 195 * When either switch is off, inductor current continues to flow but not through current sense resistors, through a parasitic diode instead.
JonFreeman 12:d1d21a2941ef 196 * Thus T_on current is measured, T_off current is not measured
JonFreeman 12:d1d21a2941ef 197 * This means current reading should approximate to current taken from the supply. Motor average current may be considerably higher.
JonFreeman 12:d1d21a2941ef 198 * During REGEN_BRAKING, current flows the 'wrong' way through sense resistors and can not be measured.
JonFreeman 12:d1d21a2941ef 199 *
JonFreeman 12:d1d21a2941ef 200 * Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents
JonFreeman 12:d1d21a2941ef 201 */
JonFreeman 16:d1e4b9ad3b8b 202 //const double MPR = (double)((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input
JonFreeman 16:d1e4b9ad3b8b 203 const uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input
JonFreeman 13:ef7a06fa11de 204
JonFreeman 10:e40d8724268a 205 void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 10:e40d8724268a 206 {
JonFreeman 13:ef7a06fa11de 207 if (p < 0.0 || ESTOP)
JonFreeman 10:e40d8724268a 208 p = 0.0;
JonFreeman 10:e40d8724268a 209 if (p > 1.0)
JonFreeman 10:e40d8724268a 210 p = 1.0;
JonFreeman 12:d1d21a2941ef 211 last_I = p; // Retains last current limit demanded by driver
JonFreeman 13:ef7a06fa11de 212 if (DRIVING) {
JonFreeman 16:d1e4b9ad3b8b 213 p *= current_scale;
JonFreeman 13:ef7a06fa11de 214 }
JonFreeman 12:d1d21a2941ef 215 maxI.pulsewidth_ticks ((uint32_t)(p * MPR)); // PWM
JonFreeman 10:e40d8724268a 216 }
JonFreeman 10:e40d8724268a 217
JonFreeman 16:d1e4b9ad3b8b 218 void brushless_motor::I_scale (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 16:d1e4b9ad3b8b 219 {
JonFreeman 16:d1e4b9ad3b8b 220 current_scale = p;
JonFreeman 16:d1e4b9ad3b8b 221 if (DRIVING) {
JonFreeman 16:d1e4b9ad3b8b 222 maxI.pulsewidth_ticks ((uint32_t)(last_I * p * MPR)); // PWM
JonFreeman 16:d1e4b9ad3b8b 223 }
JonFreeman 16:d1e4b9ad3b8b 224 }
JonFreeman 12:d1d21a2941ef 225
JonFreeman 12:d1d21a2941ef 226 /**
JonFreeman 12:d1d21a2941ef 227 * void brushless_motor::speed_monitor_and_control () // ** CALL THIS 32 TIMES PER SECOND **
JonFreeman 12:d1d21a2941ef 228 * Call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep RPM and MPH figures correct
JonFreeman 12:d1d21a2941ef 229 * Tracks total transitions on Hall sensor inputs to determine speed.
JonFreeman 12:d1d21a2941ef 230 * Sets variables double dRPM of motor RPM, and double dMPH miles per hour
JonFreeman 12:d1d21a2941ef 231 *
JonFreeman 12:d1d21a2941ef 232 * Speed control - double target_speed as reference input. *
JonFreeman 12:d1d21a2941ef 233 * ** This is where any speed limit gets applied **
JonFreeman 12:d1d21a2941ef 234 * Motor voltage reduced when at or over speed. Does NOT apply any braking
JonFreeman 12:d1d21a2941ef 235 * Scope for further improvement of control algorithm - crude implementation of PID with no I
JonFreeman 12:d1d21a2941ef 236 */
JonFreeman 12:d1d21a2941ef 237 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
JonFreeman 16:d1e4b9ad3b8b 238 { //
JonFreeman 16:d1e4b9ad3b8b 239 // const double samp_scale = 0.275; // Tweak this value only to tune filter
JonFreeman 16:d1e4b9ad3b8b 240 const double samp_scale = 0.6; // Increased May 2020 to improve ssl speed settling time
JonFreeman 16:d1e4b9ad3b8b 241 const double shrink_by = 1.0 - samp_scale;
JonFreeman 16:d1e4b9ad3b8b 242 double rpm, speed_error;
JonFreeman 12:d1d21a2941ef 243 uint32_t Hall_tot_copy = Hall_total; // Copy value for use throughout function as may get changed at any instant during exec !
JonFreeman 12:d1d21a2941ef 244 moving_flag = true;
JonFreeman 12:d1d21a2941ef 245 if (Hall_previous == Hall_tot_copy) { // Test for motor turning or not
JonFreeman 12:d1d21a2941ef 246 moving_flag = false; // Zero Hall transitions since previous call - motor not moving
JonFreeman 12:d1d21a2941ef 247 tickleon = TICKLE_TIMES; // May need to tickle some charge into high side switch bootstrap supplies
JonFreeman 16:d1e4b9ad3b8b 248 }
JonFreeman 16:d1e4b9ad3b8b 249 rpm = (double) (((Hall_tot_copy - Hall_previous) * 640) / motor_poles); // (Motor Hall sensor transitions in previous 31250us) * 640
JonFreeman 16:d1e4b9ad3b8b 250 RPM_filter *= shrink_by; // rpm now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles
JonFreeman 16:d1e4b9ad3b8b 251 RPM_filter += (rpm * samp_scale); // filtered RPM
JonFreeman 16:d1e4b9ad3b8b 252 dRPM = RPM_filter;
JonFreeman 16:d1e4b9ad3b8b 253 dMPH = user_settings.rpm2mph(RPM_filter); // Completed updates of RPM and MPH
JonFreeman 16:d1e4b9ad3b8b 254 Hall_previous = Hall_tot_copy;
JonFreeman 16:d1e4b9ad3b8b 255 speed_error = (dMPH - user_settings.top_speed()); // 'P' Proportional contribution to PID control
JonFreeman 16:d1e4b9ad3b8b 256 bool clamp_change = false;
JonFreeman 16:d1e4b9ad3b8b 257 if (speed_error > 0.0 && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) { // Speed control only makes sense when motor runnable
JonFreeman 16:d1e4b9ad3b8b 258 V_clamp *= (1.0 - (speed_error / 60.0)); // Driving too fast, so lower clamp voltage a tiny bit
JonFreeman 16:d1e4b9ad3b8b 259 clamp_change = true;
JonFreeman 16:d1e4b9ad3b8b 260 }
JonFreeman 16:d1e4b9ad3b8b 261 else { // Not going too fast, and maybe driving or not
JonFreeman 16:d1e4b9ad3b8b 262 if (V_clamp < 0.99) {
JonFreeman 16:d1e4b9ad3b8b 263 V_clamp += 0.015;
JonFreeman 16:d1e4b9ad3b8b 264 if (V_clamp > 1.0)
JonFreeman 16:d1e4b9ad3b8b 265 V_clamp = 1.0;
JonFreeman 16:d1e4b9ad3b8b 266 clamp_change = true;
JonFreeman 16:d1e4b9ad3b8b 267 }
JonFreeman 16:d1e4b9ad3b8b 268 }
JonFreeman 16:d1e4b9ad3b8b 269 if (clamp_change) {
JonFreeman 16:d1e4b9ad3b8b 270 motor_voltage_refresh () ;
JonFreeman 16:d1e4b9ad3b8b 271 }
JonFreeman 16:d1e4b9ad3b8b 272 /* d = V_clamp + (speed_error * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]); // Apply P+I+D (with I=0) control
JonFreeman 12:d1d21a2941ef 273 if (d > 1.0) d = 1.0;
JonFreeman 12:d1d21a2941ef 274 if (d < 0.0) d = 0.0;
JonFreeman 12:d1d21a2941ef 275 V_clamp = d;
JonFreeman 12:d1d21a2941ef 276 if (V_clamp < last_V) // Jan 2019 limit top speed when driving
JonFreeman 12:d1d21a2941ef 277 {
JonFreeman 12:d1d21a2941ef 278 d *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 12:d1d21a2941ef 279 d = 1.0 - d; // because pwm is wrong way up
JonFreeman 16:d1e4b9ad3b8b 280 //FUCKETYFUCK maxV.pulsewidth_ticks ((int)(d * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 12:d1d21a2941ef 281 maxV.pulsewidth_ticks ((int)(d * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 12:d1d21a2941ef 282 }
JonFreeman 16:d1e4b9ad3b8b 283 }*/
JonFreeman 12:d1d21a2941ef 284 /* temp_tick++;
JonFreeman 16:d1e4b9ad3b8b 285 if (temp_tick > 50) { // one and a bit second
JonFreeman 12:d1d21a2941ef 286 temp_tick = 0;
JonFreeman 16:d1e4b9ad3b8b 287 pc.printf ("top speed %.1f, mph %.1f, speed_err %.1f, V_clamp%.3f\r\n", user_settings.top_speed(), dMPH, speed_error, V_clamp);
JonFreeman 12:d1d21a2941ef 288 }
JonFreeman 12:d1d21a2941ef 289 */
JonFreeman 16:d1e4b9ad3b8b 290 }
JonFreeman 16:d1e4b9ad3b8b 291
JonFreeman 16:d1e4b9ad3b8b 292 bool brushless_motor::exists () {
JonFreeman 16:d1e4b9ad3b8b 293 return ((Hall_index[0] > 0) && (Hall_index[0] < 7)) ;
JonFreeman 10:e40d8724268a 294 }
JonFreeman 10:e40d8724268a 295
JonFreeman 10:e40d8724268a 296 bool brushless_motor::is_moving ()
JonFreeman 10:e40d8724268a 297 {
JonFreeman 10:e40d8724268a 298 return moving_flag;
JonFreeman 10:e40d8724268a 299 }
JonFreeman 10:e40d8724268a 300
JonFreeman 10:e40d8724268a 301 /**
JonFreeman 10:e40d8724268a 302 bool brushless_motor::set_mode (int m)
JonFreeman 16:d1e4b9ad3b8b 303 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE. Used to also be used to set REGENBRAKE, replaced by brake(x) function.
JonFreeman 10:e40d8724268a 304 If this causes change of mode, also sets V and I to zero.
JonFreeman 10:e40d8724268a 305 */
JonFreeman 16:d1e4b9ad3b8b 306 bool brushless_motor::set_mode (uint32_t m)
JonFreeman 10:e40d8724268a 307 {
JonFreeman 12:d1d21a2941ef 308 if ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE)) {
JonFreeman 10:e40d8724268a 309 // pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 10:e40d8724268a 310 return false;
JonFreeman 10:e40d8724268a 311 }
JonFreeman 10:e40d8724268a 312 if (visible_mode != m) { // Mode change, kill volts and amps to be safe
JonFreeman 10:e40d8724268a 313 set_V_limit (0.0);
JonFreeman 10:e40d8724268a 314 set_I_limit (0.0);
JonFreeman 10:e40d8724268a 315 visible_mode = m;
JonFreeman 10:e40d8724268a 316 }
JonFreeman 16:d1e4b9ad3b8b 317 if (m == MOTOR_FORWARD || m == MOTOR_REVERSE) // 8 or 16 - these are effectively address bits of motor pattern lut
JonFreeman 16:d1e4b9ad3b8b 318 m ^= direction; // direction set to 0 or (MOTOR_FORWARD | MOTOR_REVERSE), so has zero or two bits set
JonFreeman 10:e40d8724268a 319 inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
JonFreeman 10:e40d8724268a 320 return true;
JonFreeman 10:e40d8724268a 321 }
JonFreeman 10:e40d8724268a 322
JonFreeman 16:d1e4b9ad3b8b 323 void brushless_motor::brake (double brake_effort) {
JonFreeman 16:d1e4b9ad3b8b 324 // pc.printf ("In motor::brake, user_settings.brake_effectiveness = %3f\r\n", user_settings.brake_effectiveness());
JonFreeman 16:d1e4b9ad3b8b 325 set_mode (MOTOR_REGENBRAKE);
JonFreeman 16:d1e4b9ad3b8b 326 if (brake_effort > 1.0)
JonFreeman 16:d1e4b9ad3b8b 327 brake_effort = 1.0;
JonFreeman 16:d1e4b9ad3b8b 328 if (brake_effort < 0.0)
JonFreeman 16:d1e4b9ad3b8b 329 brake_effort = 0.0;
JonFreeman 16:d1e4b9ad3b8b 330 brake_effort *= user_settings.brake_effectiveness(); // set upper limit, this is essential - May2020 fixed, was 100 times too big
JonFreeman 16:d1e4b9ad3b8b 331 brake_effort = sqrt (brake_effort); // to linearise effect
JonFreeman 16:d1e4b9ad3b8b 332 set_V_limit (brake_effort) ;
JonFreeman 16:d1e4b9ad3b8b 333 set_I_limit (1.0);
JonFreeman 16:d1e4b9ad3b8b 334 V_clamp = 1.0;
JonFreeman 16:d1e4b9ad3b8b 335 }
JonFreeman 16:d1e4b9ad3b8b 336
JonFreeman 16:d1e4b9ad3b8b 337 uint32_t brushless_motor::get_mode () {
JonFreeman 16:d1e4b9ad3b8b 338 return visible_mode;
JonFreeman 16:d1e4b9ad3b8b 339 }
JonFreeman 16:d1e4b9ad3b8b 340
JonFreeman 12:d1d21a2941ef 341 void brushless_motor::motor_set () // Energise Port with data determined by Hall sensors
JonFreeman 10:e40d8724268a 342 {
JonFreeman 11:bfb73f083009 343 Hall_index[0] = read_Halls ();
JonFreeman 11:bfb73f083009 344 OP = lut[inner_mode | Hall_index[0]];
JonFreeman 10:e40d8724268a 345 }
JonFreeman 12:d1d21a2941ef 346