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@14:acaa1add097b, 2019-11-30 (annotated)

Committer:
JonFreeman
Date:
Sat Nov 30 16:34:58 2019 +0000
Revision:
14:acaa1add097b
Parent:
13:ef7a06fa11de
Child:
16:d1e4b9ad3b8b
Proved inverter board for radio control inputs work.

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