STM3 ESC dual brushless motor controller. 10-60v, motor power rating tiny to kW. Ganged or independent motor control As used in 'The Brute' locomotive - www.jons-workshop.com
Dependencies: mbed BufferedSerial Servo FastPWM
brushless_motor.cpp@12:d1d21a2941ef, 2019-03-04 (annotated)
- Committer:
- JonFreeman
- Date:
- Mon Mar 04 17:51:08 2019 +0000
- Revision:
- 12:d1d21a2941ef
- Parent:
- 11:bfb73f083009
STM3 ESC dual motor controller boards. Always 'Work In Progress', working snapshot March 2019
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JonFreeman | 10:e40d8724268a | 1 | // Cloned from 'DualBLS2018_06' on 23 November 2018 |
JonFreeman | 10:e40d8724268a | 2 | #include "mbed.h" |
JonFreeman | 10:e40d8724268a | 3 | //#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h" |
JonFreeman | 10:e40d8724268a | 4 | #include "stm32f401xe.h" |
JonFreeman | 10:e40d8724268a | 5 | #include "DualBLS.h" |
JonFreeman | 10:e40d8724268a | 6 | #include "BufferedSerial.h" |
JonFreeman | 10:e40d8724268a | 7 | #include "FastPWM.h" |
JonFreeman | 10:e40d8724268a | 8 | #include "Servo.h" |
JonFreeman | 10:e40d8724268a | 9 | #include "brushless_motor.h" |
JonFreeman | 11:bfb73f083009 | 10 | |
JonFreeman | 12:d1d21a2941ef | 11 | extern eeprom_settings mode ; |
JonFreeman | 12:d1d21a2941ef | 12 | extern double rpm2mph ; |
JonFreeman | 12:d1d21a2941ef | 13 | extern BufferedSerial pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud |
JonFreeman | 11:bfb73f083009 | 14 | |
JonFreeman | 11:bfb73f083009 | 15 | brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi, |
JonFreeman | 12:d1d21a2941ef | 16 | const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum) |
JonFreeman | 11:bfb73f083009 | 17 | : 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 | 10:e40d8724268a | 18 | { |
JonFreeman | 10:e40d8724268a | 19 | // Constructor |
JonFreeman | 11:bfb73f083009 | 20 | OP = 0; |
JonFreeman | 12:d1d21a2941ef | 21 | H1.mode (PullUp); // PullUp resistors enabled on all Hall sensor input pins |
JonFreeman | 11:bfb73f083009 | 22 | H2.mode (PullUp); |
JonFreeman | 11:bfb73f083009 | 23 | H3.mode (PullUp); |
JonFreeman | 11:bfb73f083009 | 24 | H1.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge |
JonFreeman | 11:bfb73f083009 | 25 | H1.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge |
JonFreeman | 11:bfb73f083009 | 26 | H2.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge |
JonFreeman | 11:bfb73f083009 | 27 | H2.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge |
JonFreeman | 11:bfb73f083009 | 28 | H3.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge |
JonFreeman | 11:bfb73f083009 | 29 | H3.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge |
JonFreeman | 10:e40d8724268a | 30 | 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 | 31 | Hall_previous = 0; |
JonFreeman | 11:bfb73f083009 | 32 | maxV.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz |
JonFreeman | 11:bfb73f083009 | 33 | maxI.period_ticks (MAX_PWM_TICKS + 1); |
JonFreeman | 12:d1d21a2941ef | 34 | maxV.pulsewidth_ticks (MAX_PWM_TICKS - 20); // Motor voltage pwm is inverted, see MCP1630 data |
JonFreeman | 12:d1d21a2941ef | 35 | maxI.pulsewidth_ticks (MAX_PWM_TICKS / 30); // Motor current pwm is not inverted. Initial values for scope hanging test |
JonFreeman | 12:d1d21a2941ef | 36 | visible_mode = MOTOR_REGENBRAKE; |
JonFreeman | 12:d1d21a2941ef | 37 | inner_mode = MOTOR_REGENBRAKE; |
JonFreeman | 12:d1d21a2941ef | 38 | lut = lutptr; // Pointer to motor energisation bit pattern table |
JonFreeman | 12:d1d21a2941ef | 39 | current_sense_rs_offset = rnum; // This is position in mode.rd(current_sense_rs_offset) |
JonFreeman | 11:bfb73f083009 | 40 | Hall_index[0] = Hall_index[1] = read_Halls (); |
JonFreeman | 10:e40d8724268a | 41 | tickleon = 0; |
JonFreeman | 10:e40d8724268a | 42 | direction = 0; |
JonFreeman | 10:e40d8724268a | 43 | angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel |
JonFreeman | 10:e40d8724268a | 44 | encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction |
JonFreeman | 10:e40d8724268a | 45 | last_V = last_I = 0.0; |
JonFreeman | 12:d1d21a2941ef | 46 | Idbl = 0.0; |
JonFreeman | 12:d1d21a2941ef | 47 | numof_current_sense_rs = 1.0; |
JonFreeman | 12:d1d21a2941ef | 48 | RPM_filter = 0.0; |
JonFreeman | 12:d1d21a2941ef | 49 | dv_by_dt = 0.0; |
JonFreeman | 12:d1d21a2941ef | 50 | s[1] = 0.25; |
JonFreeman | 12:d1d21a2941ef | 51 | s[2] = 9.0; |
JonFreeman | 12:d1d21a2941ef | 52 | s[3] = 0.4; |
JonFreeman | 12:d1d21a2941ef | 53 | s[4] = 0.2; |
JonFreeman | 12:d1d21a2941ef | 54 | dRPM = 0.0; |
JonFreeman | 12:d1d21a2941ef | 55 | V_clamp = 1.0 ; // Used to limit top speed |
JonFreeman | 12:d1d21a2941ef | 56 | motor_poles = 8; // Default to 8 pole motor |
JonFreeman | 12:d1d21a2941ef | 57 | #ifdef USING_DC_MOTORS |
JonFreeman | 11:bfb73f083009 | 58 | dc_motor = (Hall_index[0] == 7) ? true : false ; |
JonFreeman | 12:d1d21a2941ef | 59 | #endif |
JonFreeman | 11:bfb73f083009 | 60 | } |
JonFreeman | 11:bfb73f083009 | 61 | |
JonFreeman | 12:d1d21a2941ef | 62 | /** |
JonFreeman | 12:d1d21a2941ef | 63 | * void brushless_motor::sniff_current () { // Initiate ADC current reading |
JonFreeman | 12:d1d21a2941ef | 64 | * This to be called in response to ticker timebase interrupt. |
JonFreeman | 12:d1d21a2941ef | 65 | * As designed, called at 200 micro second intervals (Feb 2019) |
JonFreeman | 12:d1d21a2941ef | 66 | * Updates double I.dbl current measured in milliamps |
JonFreeman | 12:d1d21a2941ef | 67 | * Reading not used elsewhere in this code but made available through command line for external controller |
JonFreeman | 12:d1d21a2941ef | 68 | */ |
JonFreeman | 12:d1d21a2941ef | 69 | void brushless_motor::sniff_current () { // Initiate ADC current reading |
JonFreeman | 12:d1d21a2941ef | 70 | const double sampweight = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED ; |
JonFreeman | 12:d1d21a2941ef | 71 | const double shrinkby = 1.0 - sampweight; |
JonFreeman | 12:d1d21a2941ef | 72 | uint16_t samp = Motor_I.read_u16 (); // CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019 |
JonFreeman | 12:d1d21a2941ef | 73 | double dbls = ((double)samp) * numof_current_sense_rs / 6.0; // reading in mA |
JonFreeman | 12:d1d21a2941ef | 74 | Idbl *= shrinkby; // Jan 2019 New recursive low pass filter |
JonFreeman | 12:d1d21a2941ef | 75 | Idbl += dbls * sampweight; // Current reading available, however not certain this is of any real use |
JonFreeman | 12:d1d21a2941ef | 76 | } |
JonFreeman | 12:d1d21a2941ef | 77 | |
JonFreeman | 12:d1d21a2941ef | 78 | bool brushless_motor::poles (int p) { // Jan 2019 max_rom no longer used. target_speed is preferred |
JonFreeman | 12:d1d21a2941ef | 79 | if (!max_rpm) { // Not been set since powerup |
JonFreeman | 12:d1d21a2941ef | 80 | max_rpm = (uint32_t) (((double)mode.rd(TOP_SPEED) / rpm2mph) / 10.0) ; |
JonFreeman | 12:d1d21a2941ef | 81 | target_speed = (double)mode.rd(TOP_SPEED) / 10.0; |
JonFreeman | 12:d1d21a2941ef | 82 | numof_current_sense_rs = (double)mode.rd(current_sense_rs_offset); |
JonFreeman | 12:d1d21a2941ef | 83 | pc.printf ("max_rpm=%d, tp speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph); |
JonFreeman | 12:d1d21a2941ef | 84 | } |
JonFreeman | 12:d1d21a2941ef | 85 | if (p == 4 || p == 6 || p == 8) { |
JonFreeman | 12:d1d21a2941ef | 86 | motor_poles = p; |
JonFreeman | 12:d1d21a2941ef | 87 | return true; |
JonFreeman | 12:d1d21a2941ef | 88 | } |
JonFreeman | 12:d1d21a2941ef | 89 | return false; |
JonFreeman | 11:bfb73f083009 | 90 | } |
JonFreeman | 11:bfb73f083009 | 91 | |
JonFreeman | 11:bfb73f083009 | 92 | void brushless_motor::Hall_change () { |
JonFreeman | 11:bfb73f083009 | 93 | int32_t delta_theta; |
JonFreeman | 11:bfb73f083009 | 94 | 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 | 95 | 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0 |
JonFreeman | 11:bfb73f083009 | 96 | 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1 |
JonFreeman | 11:bfb73f083009 | 97 | 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2 |
JonFreeman | 11:bfb73f083009 | 98 | 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3 |
JonFreeman | 11:bfb73f083009 | 99 | 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4 |
JonFreeman | 11:bfb73f083009 | 100 | 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5 |
JonFreeman | 11:bfb73f083009 | 101 | 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6 |
JonFreeman | 11:bfb73f083009 | 102 | 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7 |
JonFreeman | 11:bfb73f083009 | 103 | } ; |
JonFreeman | 11:bfb73f083009 | 104 | |
JonFreeman | 11:bfb73f083009 | 105 | Hall_index[0] = read_Halls (); |
JonFreeman | 11:bfb73f083009 | 106 | delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]]; |
JonFreeman | 11:bfb73f083009 | 107 | |
JonFreeman | 11:bfb73f083009 | 108 | if (delta_theta == 0) |
JonFreeman | 11:bfb73f083009 | 109 | encoder_error_cnt++; |
JonFreeman | 10:e40d8724268a | 110 | else |
JonFreeman | 11:bfb73f083009 | 111 | angle_cnt += delta_theta; |
JonFreeman | 11:bfb73f083009 | 112 | OP = lut[inner_mode | Hall_index[0]]; // changed mode to inner_mode 27/04/18 |
JonFreeman | 11:bfb73f083009 | 113 | Hall_total++; |
JonFreeman | 11:bfb73f083009 | 114 | Hall_index[1] = Hall_index[0]; |
JonFreeman | 10:e40d8724268a | 115 | } |
JonFreeman | 10:e40d8724268a | 116 | |
JonFreeman | 10:e40d8724268a | 117 | /** |
JonFreeman | 12:d1d21a2941ef | 118 | * void brushless_motor::direction_set (int dir) { |
JonFreeman | 12:d1d21a2941ef | 119 | * Used to set direction according to mode data from eeprom |
JonFreeman | 10:e40d8724268a | 120 | */ |
JonFreeman | 10:e40d8724268a | 121 | void brushless_motor::direction_set (int dir) |
JonFreeman | 10:e40d8724268a | 122 | { |
JonFreeman | 12:d1d21a2941ef | 123 | direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; // bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4 |
JonFreeman | 10:e40d8724268a | 124 | } |
JonFreeman | 10:e40d8724268a | 125 | |
JonFreeman | 10:e40d8724268a | 126 | int brushless_motor::read_Halls () |
JonFreeman | 10:e40d8724268a | 127 | { |
JonFreeman | 10:e40d8724268a | 128 | int x = 0; |
JonFreeman | 12:d1d21a2941ef | 129 | if (H1) x |= 1; |
JonFreeman | 12:d1d21a2941ef | 130 | if (H2) x |= 2; |
JonFreeman | 12:d1d21a2941ef | 131 | if (H3) x |= 4; |
JonFreeman | 10:e40d8724268a | 132 | return x; |
JonFreeman | 10:e40d8724268a | 133 | } |
JonFreeman | 10:e40d8724268a | 134 | |
JonFreeman | 12:d1d21a2941ef | 135 | 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 | 136 | { |
JonFreeman | 11:bfb73f083009 | 137 | uint16_t p = OP; |
JonFreeman | 10:e40d8724268a | 138 | p &= lut[32]; // KEEP_L_MASK_A or B |
JonFreeman | 11:bfb73f083009 | 139 | OP = p; |
JonFreeman | 10:e40d8724268a | 140 | } |
JonFreeman | 12:d1d21a2941ef | 141 | /* |
JonFreeman | 10:e40d8724268a | 142 | void brushless_motor::low_side_on () |
JonFreeman | 10:e40d8724268a | 143 | { |
JonFreeman | 12:d1d21a2941ef | 144 | maxV.pulsewidth_ticks (1); |
JonFreeman | 12:d1d21a2941ef | 145 | OP = lut[31]; // KEEP_L_MASK_A or B |
JonFreeman | 10:e40d8724268a | 146 | } |
JonFreeman | 12:d1d21a2941ef | 147 | */ |
JonFreeman | 10:e40d8724268a | 148 | |
JonFreeman | 12:d1d21a2941ef | 149 | void brushless_motor::set_speed (double p) // Sets target_speed |
JonFreeman | 10:e40d8724268a | 150 | { |
JonFreeman | 12:d1d21a2941ef | 151 | target_speed = p; |
JonFreeman | 10:e40d8724268a | 152 | } |
JonFreeman | 10:e40d8724268a | 153 | |
JonFreeman | 10:e40d8724268a | 154 | |
JonFreeman | 12:d1d21a2941ef | 155 | /** |
JonFreeman | 12:d1d21a2941ef | 156 | * void brushless_motor::set_V_limit (double p) // Sets max motor voltage. |
JonFreeman | 12:d1d21a2941ef | 157 | * |
JonFreeman | 12:d1d21a2941ef | 158 | * Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0) |
JonFreeman | 12:d1d21a2941ef | 159 | */ |
JonFreeman | 12:d1d21a2941ef | 160 | void brushless_motor::set_V_limit (double p) // Sets max motor voltage. |
JonFreeman | 10:e40d8724268a | 161 | { |
JonFreeman | 10:e40d8724268a | 162 | if (p < 0.0) |
JonFreeman | 10:e40d8724268a | 163 | p = 0.0; |
JonFreeman | 10:e40d8724268a | 164 | if (p > 1.0) |
JonFreeman | 10:e40d8724268a | 165 | p = 1.0; |
JonFreeman | 12:d1d21a2941ef | 166 | last_V = p; // Retains last voltage limit demanded by driver |
JonFreeman | 12:d1d21a2941ef | 167 | |
JonFreeman | 12:d1d21a2941ef | 168 | if ((V_clamp < last_V) && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) // Jan 2019 limit top speed when driving |
JonFreeman | 12:d1d21a2941ef | 169 | p = V_clamp; // If motor runnable, set voltage limit to lower of last_V and V_clamp |
JonFreeman | 12:d1d21a2941ef | 170 | |
JonFreeman | 12:d1d21a2941ef | 171 | p *= 0.95; // need limit, ffi see MCP1630 data |
JonFreeman | 10:e40d8724268a | 172 | p = 1.0 - p; // because pwm is wrong way up |
JonFreeman | 11:bfb73f083009 | 173 | maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts |
JonFreeman | 10:e40d8724268a | 174 | } |
JonFreeman | 10:e40d8724268a | 175 | |
JonFreeman | 12:d1d21a2941ef | 176 | /**void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level |
JonFreeman | 12:d1d21a2941ef | 177 | * |
JonFreeman | 12:d1d21a2941ef | 178 | * Set motor current limit from zero (p=0.0) to max determined by current sense resistors (p=1.0) |
JonFreeman | 12:d1d21a2941ef | 179 | * Value sent to pwm with RC integrator acting as AnalogOut. |
JonFreeman | 12:d1d21a2941ef | 180 | * pwm capable of 0.0 <= V_out <= 3.3. This feeds MCP1630 V_Ref, range 0 to 2.7v |
JonFreeman | 12:d1d21a2941ef | 181 | * Therefore (2.7/3.3) = 0.82 factor included. |
JonFreeman | 12:d1d21a2941ef | 182 | * Jan 2019 - As yet uncalibrated, so let's have a go at working it out! |
JonFreeman | 12:d1d21a2941ef | 183 | * Voltage ax current sense resistors amplified by op-amp with gain 5.7 (see EasyPC file 'BrushlessSTM3.sch', U6, R63, R64) |
JonFreeman | 12:d1d21a2941ef | 184 | * 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 | 185 | * This connects to MCP1630 CS (current sense) pin which works over the voltage range 0.0 to 0.9v |
JonFreeman | 12:d1d21a2941ef | 186 | * Therefore 0.5v across current sense resistor is sufficient to turn driver off. |
JonFreeman | 12:d1d21a2941ef | 187 | * 0.5v across 0.05 ohm gives 10A per current sense resistor fitted. |
JonFreeman | 12:d1d21a2941ef | 188 | * ** NOTE ** This is fast acting cycle by cycle current limit, the 10A figure is therefore peak T_on current. |
JonFreeman | 12:d1d21a2941ef | 189 | * |
JonFreeman | 12:d1d21a2941ef | 190 | * 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 | 191 | * When either switch is off, inductor current continues to flow but not through current sense resistors, through a parasitic diode instead. |
JonFreeman | 12:d1d21a2941ef | 192 | * Thus T_on current is measured, T_off current is not measured |
JonFreeman | 12:d1d21a2941ef | 193 | * This means current reading should approximate to current taken from the supply. Motor average current may be considerably higher. |
JonFreeman | 12:d1d21a2941ef | 194 | * During REGEN_BRAKING, current flows the 'wrong' way through sense resistors and can not be measured. |
JonFreeman | 12:d1d21a2941ef | 195 | * |
JonFreeman | 12:d1d21a2941ef | 196 | * 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 | 197 | */ |
JonFreeman | 10:e40d8724268a | 198 | void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level |
JonFreeman | 10:e40d8724268a | 199 | { |
JonFreeman | 12:d1d21a2941ef | 200 | const uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input |
JonFreeman | 10:e40d8724268a | 201 | if (p < 0.0) |
JonFreeman | 10:e40d8724268a | 202 | p = 0.0; |
JonFreeman | 10:e40d8724268a | 203 | if (p > 1.0) |
JonFreeman | 10:e40d8724268a | 204 | p = 1.0; |
JonFreeman | 12:d1d21a2941ef | 205 | last_I = p; // Retains last current limit demanded by driver |
JonFreeman | 12:d1d21a2941ef | 206 | maxI.pulsewidth_ticks ((uint32_t)(p * MPR)); // PWM |
JonFreeman | 10:e40d8724268a | 207 | } |
JonFreeman | 10:e40d8724268a | 208 | |
JonFreeman | 12:d1d21a2941ef | 209 | |
JonFreeman | 12:d1d21a2941ef | 210 | /** |
JonFreeman | 12:d1d21a2941ef | 211 | * void brushless_motor::speed_monitor_and_control () // ** CALL THIS 32 TIMES PER SECOND ** |
JonFreeman | 12:d1d21a2941ef | 212 | * Call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep RPM and MPH figures correct |
JonFreeman | 12:d1d21a2941ef | 213 | * Tracks total transitions on Hall sensor inputs to determine speed. |
JonFreeman | 12:d1d21a2941ef | 214 | * Sets variables double dRPM of motor RPM, and double dMPH miles per hour |
JonFreeman | 12:d1d21a2941ef | 215 | * |
JonFreeman | 12:d1d21a2941ef | 216 | * Speed control - double target_speed as reference input. * |
JonFreeman | 12:d1d21a2941ef | 217 | * ** This is where any speed limit gets applied ** |
JonFreeman | 12:d1d21a2941ef | 218 | * Motor voltage reduced when at or over speed. Does NOT apply any braking |
JonFreeman | 12:d1d21a2941ef | 219 | * Scope for further improvement of control algorithm - crude implementation of PID with no I |
JonFreeman | 12:d1d21a2941ef | 220 | */ |
JonFreeman | 12:d1d21a2941ef | 221 | 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 | 10:e40d8724268a | 222 | { |
JonFreeman | 12:d1d21a2941ef | 223 | #ifdef USING_DC_MOTORS |
JonFreeman | 10:e40d8724268a | 224 | if (dc_motor) |
JonFreeman | 10:e40d8724268a | 225 | return 0; |
JonFreeman | 12:d1d21a2941ef | 226 | #endif |
JonFreeman | 12:d1d21a2941ef | 227 | // Feb 2019 - coefficients currently values in ram to allow for tweaking via command line. Will be 'const' once settled upon. |
JonFreeman | 12:d1d21a2941ef | 228 | // const double samp_scale = 0.35; // Tweak this value only to tune filter |
JonFreeman | 12:d1d21a2941ef | 229 | double samp_scale = s[1]; // Tweak this value only to tune filter |
JonFreeman | 12:d1d21a2941ef | 230 | double shrink_by = 1.0 - samp_scale; |
JonFreeman | 12:d1d21a2941ef | 231 | // const double dv_scale = 0.15; |
JonFreeman | 12:d1d21a2941ef | 232 | double dv_scale = s[3]; |
JonFreeman | 12:d1d21a2941ef | 233 | double dv_shrink = 1.0 - dv_scale; |
JonFreeman | 12:d1d21a2941ef | 234 | double speed_error, d, t; |
JonFreeman | 12:d1d21a2941ef | 235 | 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 | 236 | moving_flag = true; |
JonFreeman | 12:d1d21a2941ef | 237 | if (Hall_previous == Hall_tot_copy) { // Test for motor turning or not |
JonFreeman | 12:d1d21a2941ef | 238 | moving_flag = false; // Zero Hall transitions since previous call - motor not moving |
JonFreeman | 12:d1d21a2941ef | 239 | tickleon = TICKLE_TIMES; // May need to tickle some charge into high side switch bootstrap supplies |
JonFreeman | 12:d1d21a2941ef | 240 | } |
JonFreeman | 12:d1d21a2941ef | 241 | d = (double) ((Hall_tot_copy - Hall_previous) *640); // (Motor Hall sensor transitions in previous 31250us) * 640 |
JonFreeman | 12:d1d21a2941ef | 242 | d /= motor_poles; // d now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles |
JonFreeman | 12:d1d21a2941ef | 243 | t = RPM_filter; // Value from last time around |
JonFreeman | 12:d1d21a2941ef | 244 | RPM_filter *= shrink_by; |
JonFreeman | 12:d1d21a2941ef | 245 | RPM_filter += (d * samp_scale); // filtered RPM |
JonFreeman | 12:d1d21a2941ef | 246 | // RPM_filter[n] = shrink_by RPM_filter[n - 1] + samp_scale x[n] |
JonFreeman | 12:d1d21a2941ef | 247 | t -= RPM_filter; // more like minus dv/dt |
JonFreeman | 12:d1d21a2941ef | 248 | dv_by_dt *= dv_shrink; |
JonFreeman | 12:d1d21a2941ef | 249 | dv_by_dt += (t * dv_scale); // filtered rate of change, the 'D' Differential contribution to PID control |
JonFreeman | 12:d1d21a2941ef | 250 | dRPM += RPM_filter; |
JonFreeman | 12:d1d21a2941ef | 251 | dRPM /= 2.0; |
JonFreeman | 12:d1d21a2941ef | 252 | dMPH = RPM_filter * rpm2mph; // Completed updates of RPM and MPH |
JonFreeman | 12:d1d21a2941ef | 253 | |
JonFreeman | 12:d1d21a2941ef | 254 | if (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE) { // Speed control only makes sense when motor runnable |
JonFreeman | 12:d1d21a2941ef | 255 | speed_error = (target_speed - dMPH) / 1000.0; // 'P' Proportional contribution to PID control |
JonFreeman | 12:d1d21a2941ef | 256 | d = V_clamp + (speed_error * s[2]) + ((dv_by_dt / 1000.0) * s[4]); // Apply P+I+D (with I=0) control |
JonFreeman | 12:d1d21a2941ef | 257 | if (d > 1.0) d = 1.0; |
JonFreeman | 12:d1d21a2941ef | 258 | if (d < 0.0) d = 0.0; |
JonFreeman | 12:d1d21a2941ef | 259 | V_clamp = d; |
JonFreeman | 12:d1d21a2941ef | 260 | if (V_clamp < last_V) // Jan 2019 limit top speed when driving |
JonFreeman | 12:d1d21a2941ef | 261 | { |
JonFreeman | 12:d1d21a2941ef | 262 | d *= 0.95; // need limit, ffi see MCP1630 data |
JonFreeman | 12:d1d21a2941ef | 263 | d = 1.0 - d; // because pwm is wrong way up |
JonFreeman | 12:d1d21a2941ef | 264 | maxV.pulsewidth_ticks ((int)(d * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts |
JonFreeman | 12:d1d21a2941ef | 265 | } |
JonFreeman | 10:e40d8724268a | 266 | } |
JonFreeman | 12:d1d21a2941ef | 267 | /* temp_tick++; |
JonFreeman | 12:d1d21a2941ef | 268 | if (temp_tick > 35) { // one and a bit second |
JonFreeman | 12:d1d21a2941ef | 269 | temp_tick = 0; |
JonFreeman | 12:d1d21a2941ef | 270 | pc.printf ("RPM %.0f, %.3f, %.3f, %.2f, dv/dt%.3f\r\n", dRPM, RPM_filter, d, dMPH, dv_by_dt); |
JonFreeman | 12:d1d21a2941ef | 271 | } |
JonFreeman | 12:d1d21a2941ef | 272 | */ |
JonFreeman | 12:d1d21a2941ef | 273 | Hall_previous = Hall_tot_copy; |
JonFreeman | 10:e40d8724268a | 274 | } |
JonFreeman | 10:e40d8724268a | 275 | |
JonFreeman | 10:e40d8724268a | 276 | bool brushless_motor::is_moving () |
JonFreeman | 10:e40d8724268a | 277 | { |
JonFreeman | 10:e40d8724268a | 278 | return moving_flag; |
JonFreeman | 10:e40d8724268a | 279 | } |
JonFreeman | 10:e40d8724268a | 280 | |
JonFreeman | 10:e40d8724268a | 281 | /** |
JonFreeman | 10:e40d8724268a | 282 | bool brushless_motor::set_mode (int m) |
JonFreeman | 10:e40d8724268a | 283 | Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE. |
JonFreeman | 10:e40d8724268a | 284 | If this causes change of mode, also sets V and I to zero. |
JonFreeman | 10:e40d8724268a | 285 | */ |
JonFreeman | 10:e40d8724268a | 286 | bool brushless_motor::set_mode (int m) |
JonFreeman | 10:e40d8724268a | 287 | { |
JonFreeman | 12:d1d21a2941ef | 288 | if ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE)) { |
JonFreeman | 10:e40d8724268a | 289 | // pc.printf ("Error in set_mode, invalid mode %d\r\n", m); |
JonFreeman | 10:e40d8724268a | 290 | return false; |
JonFreeman | 10:e40d8724268a | 291 | } |
JonFreeman | 10:e40d8724268a | 292 | if (visible_mode != m) { // Mode change, kill volts and amps to be safe |
JonFreeman | 10:e40d8724268a | 293 | set_V_limit (0.0); |
JonFreeman | 10:e40d8724268a | 294 | set_I_limit (0.0); |
JonFreeman | 10:e40d8724268a | 295 | visible_mode = m; |
JonFreeman | 10:e40d8724268a | 296 | } |
JonFreeman | 12:d1d21a2941ef | 297 | if (m == MOTOR_FORWARD || m == MOTOR_REVERSE) |
JonFreeman | 10:e40d8724268a | 298 | m ^= direction; |
JonFreeman | 10:e40d8724268a | 299 | 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 | 300 | return true; |
JonFreeman | 10:e40d8724268a | 301 | } |
JonFreeman | 10:e40d8724268a | 302 | |
JonFreeman | 12:d1d21a2941ef | 303 | void brushless_motor::motor_set () // Energise Port with data determined by Hall sensors |
JonFreeman | 10:e40d8724268a | 304 | { |
JonFreeman | 11:bfb73f083009 | 305 | Hall_index[0] = read_Halls (); |
JonFreeman | 11:bfb73f083009 | 306 | OP = lut[inner_mode | Hall_index[0]]; |
JonFreeman | 10:e40d8724268a | 307 | } |
JonFreeman | 12:d1d21a2941ef | 308 |