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@13:ef7a06fa11de, 2019-09-29 (annotated)

Committer:
JonFreeman
Date:
Sun Sep 29 16:34:37 2019 +0000
Revision:
13:ef7a06fa11de
Parent:
12:d1d21a2941ef
Child:
14:acaa1add097b
Stable code as at end of 2019 running season

Who changed what in which revision?

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