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@11:bfb73f083009, 2019-01-19 (annotated)

Committer:
JonFreeman
Date:
Sat Jan 19 11:45:01 2019 +0000
Revision:
11:bfb73f083009
Parent:
10:e40d8724268a
Child:
12:d1d21a2941ef
Tidied class parameter passing, serial problem fixed (was hardware)

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 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 11:bfb73f083009 11
JonFreeman 11:bfb73f083009 12 brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi,
JonFreeman 11:bfb73f083009 13 const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask)
JonFreeman 11:bfb73f083009 14 : 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 15 {
JonFreeman 10:e40d8724268a 16 // Constructor
JonFreeman 11:bfb73f083009 17 OP = 0;
JonFreeman 11:bfb73f083009 18 H1.mode (PullUp);
JonFreeman 11:bfb73f083009 19 H2.mode (PullUp);
JonFreeman 11:bfb73f083009 20 H3.mode (PullUp);
JonFreeman 11:bfb73f083009 21 H1.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
JonFreeman 11:bfb73f083009 22 H1.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
JonFreeman 11:bfb73f083009 23 H2.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
JonFreeman 11:bfb73f083009 24 H2.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
JonFreeman 11:bfb73f083009 25 H3.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
JonFreeman 11:bfb73f083009 26 H3.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
JonFreeman 10:e40d8724268a 27 Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
JonFreeman 10:e40d8724268a 28 latest_pulses_per_sec = 0;
JonFreeman 10:e40d8724268a 29 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 10:e40d8724268a 30 edge_count_table[i] = 0;
JonFreeman 10:e40d8724268a 31 Hall_tab_ptr = 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 11:bfb73f083009 34 maxV.pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 11:bfb73f083009 35 maxI.pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 10:e40d8724268a 36 visible_mode = REGENBRAKE;
JonFreeman 10:e40d8724268a 37 inner_mode = REGENBRAKE;
JonFreeman 10:e40d8724268a 38 lut = lutptr;
JonFreeman 11:bfb73f083009 39 Hall_index[0] = Hall_index[1] = read_Halls ();
JonFreeman 10:e40d8724268a 40 ppstmp = 0;
JonFreeman 11:bfb73f083009 41 cs_ptr = 0;
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 PPS = 0;
JonFreeman 10:e40d8724268a 47 RPM = 0;
JonFreeman 10:e40d8724268a 48 last_V = last_I = 0.0;
JonFreeman 11:bfb73f083009 49 dc_motor = (Hall_index[0] == 7) ? true : false ;
JonFreeman 11:bfb73f083009 50 }
JonFreeman 11:bfb73f083009 51
JonFreeman 11:bfb73f083009 52 void brushless_motor::sniff_current () {
JonFreeman 11:bfb73f083009 53 current_samples[cs_ptr++] = Motor_I.read_u16 (); //
JonFreeman 11:bfb73f083009 54 if (cs_ptr >= CURRENT_SAMPLES_AVERAGED)
JonFreeman 11:bfb73f083009 55 cs_ptr = 0;
JonFreeman 11:bfb73f083009 56 }
JonFreeman 11:bfb73f083009 57
JonFreeman 11:bfb73f083009 58 void brushless_motor::Hall_change () {
JonFreeman 11:bfb73f083009 59 int32_t delta_theta;
JonFreeman 11:bfb73f083009 60 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 61 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
JonFreeman 11:bfb73f083009 62 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
JonFreeman 11:bfb73f083009 63 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
JonFreeman 11:bfb73f083009 64 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
JonFreeman 11:bfb73f083009 65 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
JonFreeman 11:bfb73f083009 66 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
JonFreeman 11:bfb73f083009 67 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
JonFreeman 11:bfb73f083009 68 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
JonFreeman 11:bfb73f083009 69 } ;
JonFreeman 11:bfb73f083009 70
JonFreeman 11:bfb73f083009 71 Hall_index[0] = read_Halls ();
JonFreeman 11:bfb73f083009 72 delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];
JonFreeman 11:bfb73f083009 73
JonFreeman 11:bfb73f083009 74 if (delta_theta == 0)
JonFreeman 11:bfb73f083009 75 encoder_error_cnt++;
JonFreeman 10:e40d8724268a 76 else
JonFreeman 11:bfb73f083009 77 angle_cnt += delta_theta;
JonFreeman 11:bfb73f083009 78 OP = lut[inner_mode | Hall_index[0]]; // changed mode to inner_mode 27/04/18
JonFreeman 11:bfb73f083009 79 Hall_total++;
JonFreeman 11:bfb73f083009 80 Hall_index[1] = Hall_index[0];
JonFreeman 10:e40d8724268a 81 }
JonFreeman 10:e40d8724268a 82
JonFreeman 10:e40d8724268a 83 bool brushless_motor::motor_is_brushless ()
JonFreeman 10:e40d8724268a 84 {
JonFreeman 10:e40d8724268a 85 /* int x = read_Halls ();
JonFreeman 10:e40d8724268a 86 if (x < 1 || x > 6)
JonFreeman 10:e40d8724268a 87 return false;
JonFreeman 10:e40d8724268a 88 return true;
JonFreeman 10:e40d8724268a 89 */
JonFreeman 10:e40d8724268a 90 return !dc_motor;
JonFreeman 10:e40d8724268a 91 }
JonFreeman 10:e40d8724268a 92
JonFreeman 10:e40d8724268a 93 /**
JonFreeman 10:e40d8724268a 94 void brushless_motor::direction_set (int dir) {
JonFreeman 10:e40d8724268a 95 Used to set direction according to mode data from eeprom
JonFreeman 10:e40d8724268a 96 */
JonFreeman 10:e40d8724268a 97 void brushless_motor::direction_set (int dir)
JonFreeman 10:e40d8724268a 98 {
JonFreeman 10:e40d8724268a 99 if (dir != 0)
JonFreeman 10:e40d8724268a 100 dir = FORWARD | REVERSE; // bits used in eor
JonFreeman 10:e40d8724268a 101 direction = dir;
JonFreeman 10:e40d8724268a 102 }
JonFreeman 10:e40d8724268a 103
JonFreeman 10:e40d8724268a 104 int brushless_motor::read_Halls ()
JonFreeman 10:e40d8724268a 105 {
JonFreeman 10:e40d8724268a 106 int x = 0;
JonFreeman 11:bfb73f083009 107 if (H1 != 0) x |= 1;
JonFreeman 11:bfb73f083009 108 if (H2 != 0) x |= 2;
JonFreeman 11:bfb73f083009 109 if (H3 != 0) x |= 4;
JonFreeman 10:e40d8724268a 110 return x;
JonFreeman 10:e40d8724268a 111 }
JonFreeman 10:e40d8724268a 112
JonFreeman 10:e40d8724268a 113 void brushless_motor::high_side_off ()
JonFreeman 10:e40d8724268a 114 {
JonFreeman 11:bfb73f083009 115 // uint16_t p = *Motor_Port;
JonFreeman 11:bfb73f083009 116 uint16_t p = OP;
JonFreeman 10:e40d8724268a 117 p &= lut[32]; // KEEP_L_MASK_A or B
JonFreeman 11:bfb73f083009 118 // *Motor_Port = p;
JonFreeman 11:bfb73f083009 119 OP = p;
JonFreeman 10:e40d8724268a 120 }
JonFreeman 10:e40d8724268a 121
JonFreeman 10:e40d8724268a 122 void brushless_motor::low_side_on ()
JonFreeman 10:e40d8724268a 123 {
JonFreeman 10:e40d8724268a 124 // uint16_t p = *Motor_Port;
JonFreeman 10:e40d8724268a 125 // p &= lut[31]; // KEEP_L_MASK_A or B
JonFreeman 11:bfb73f083009 126 // *Motor_Port = lut[31];
JonFreeman 11:bfb73f083009 127 OP = lut[31];
JonFreeman 10:e40d8724268a 128 }
JonFreeman 10:e40d8724268a 129
JonFreeman 10:e40d8724268a 130 void brushless_motor::current_calc ()
JonFreeman 10:e40d8724268a 131 {
JonFreeman 10:e40d8724268a 132 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 10:e40d8724268a 133 I.max = 0;
JonFreeman 10:e40d8724268a 134 I.ave = 0;
JonFreeman 10:e40d8724268a 135 uint16_t sample;
JonFreeman 10:e40d8724268a 136 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 10:e40d8724268a 137 sample = current_samples[i];
JonFreeman 10:e40d8724268a 138 I.ave += sample;
JonFreeman 10:e40d8724268a 139 if (I.min > sample)
JonFreeman 10:e40d8724268a 140 I.min = sample;
JonFreeman 10:e40d8724268a 141 if (I.max < sample)
JonFreeman 10:e40d8724268a 142 I.max = sample;
JonFreeman 10:e40d8724268a 143 }
JonFreeman 10:e40d8724268a 144 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 10:e40d8724268a 145 }
JonFreeman 10:e40d8724268a 146
JonFreeman 10:e40d8724268a 147
JonFreeman 10:e40d8724268a 148 void brushless_motor::raw_V_pwm (int v)
JonFreeman 10:e40d8724268a 149 {
JonFreeman 10:e40d8724268a 150 if (v < 1) v = 1;
JonFreeman 10:e40d8724268a 151 if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
JonFreeman 11:bfb73f083009 152 maxV.pulsewidth_ticks (v);
JonFreeman 10:e40d8724268a 153 }
JonFreeman 10:e40d8724268a 154
JonFreeman 10:e40d8724268a 155 void brushless_motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
JonFreeman 10:e40d8724268a 156 {
JonFreeman 10:e40d8724268a 157 if (p < 0.0)
JonFreeman 10:e40d8724268a 158 p = 0.0;
JonFreeman 10:e40d8724268a 159 if (p > 1.0)
JonFreeman 10:e40d8724268a 160 p = 1.0;
JonFreeman 10:e40d8724268a 161 last_V = p; // for read by diagnostics
JonFreeman 10:e40d8724268a 162 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 10:e40d8724268a 163 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 11:bfb73f083009 164 maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 10:e40d8724268a 165 }
JonFreeman 10:e40d8724268a 166
JonFreeman 10:e40d8724268a 167 void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 10:e40d8724268a 168 {
JonFreeman 10:e40d8724268a 169 int a;
JonFreeman 10:e40d8724268a 170 if (p < 0.0)
JonFreeman 10:e40d8724268a 171 p = 0.0;
JonFreeman 10:e40d8724268a 172 if (p > 1.0)
JonFreeman 10:e40d8724268a 173 p = 1.0;
JonFreeman 10:e40d8724268a 174 last_I = p;
JonFreeman 10:e40d8724268a 175 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 10:e40d8724268a 176 if (a > MAX_PWM_TICKS)
JonFreeman 10:e40d8724268a 177 a = MAX_PWM_TICKS;
JonFreeman 10:e40d8724268a 178 if (a < 0)
JonFreeman 10:e40d8724268a 179 a = 0;
JonFreeman 11:bfb73f083009 180 maxI.pulsewidth_ticks (a); // PWM
JonFreeman 10:e40d8724268a 181 }
JonFreeman 10:e40d8724268a 182
JonFreeman 10:e40d8724268a 183 uint32_t brushless_motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
JonFreeman 10:e40d8724268a 184 {
JonFreeman 10:e40d8724268a 185 // Can also test for motor running or not here
JonFreeman 10:e40d8724268a 186 if (dc_motor)
JonFreeman 10:e40d8724268a 187 return 0;
JonFreeman 10:e40d8724268a 188 if (ppstmp == Hall_total) {
JonFreeman 10:e40d8724268a 189 // if (dc_motor || ppstmp == Hall_total) {
JonFreeman 10:e40d8724268a 190 moving_flag = false; // Zero Hall transitions since previous call - motor not moving
JonFreeman 10:e40d8724268a 191 tickleon = TICKLE_TIMES;
JonFreeman 10:e40d8724268a 192 } else {
JonFreeman 10:e40d8724268a 193 moving_flag = true;
JonFreeman 10:e40d8724268a 194 ppstmp = Hall_total;
JonFreeman 10:e40d8724268a 195 }
JonFreeman 10:e40d8724268a 196 latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
JonFreeman 10:e40d8724268a 197 edge_count_table[Hall_tab_ptr] = ppstmp;
JonFreeman 10:e40d8724268a 198 Hall_tab_ptr++;
JonFreeman 10:e40d8724268a 199 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 10:e40d8724268a 200 Hall_tab_ptr = 0;
JonFreeman 10:e40d8724268a 201 PPS = latest_pulses_per_sec;
JonFreeman 10:e40d8724268a 202 RPM = (latest_pulses_per_sec * 60) / 24;
JonFreeman 10:e40d8724268a 203 return latest_pulses_per_sec;
JonFreeman 10:e40d8724268a 204 }
JonFreeman 10:e40d8724268a 205
JonFreeman 10:e40d8724268a 206 bool brushless_motor::is_moving ()
JonFreeman 10:e40d8724268a 207 {
JonFreeman 10:e40d8724268a 208 return moving_flag;
JonFreeman 10:e40d8724268a 209 }
JonFreeman 10:e40d8724268a 210
JonFreeman 10:e40d8724268a 211 /**
JonFreeman 10:e40d8724268a 212 bool brushless_motor::set_mode (int m)
JonFreeman 10:e40d8724268a 213 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
JonFreeman 10:e40d8724268a 214 If this causes change of mode, also sets V and I to zero.
JonFreeman 10:e40d8724268a 215 */
JonFreeman 10:e40d8724268a 216 bool brushless_motor::set_mode (int m)
JonFreeman 10:e40d8724268a 217 {
JonFreeman 10:e40d8724268a 218 if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
JonFreeman 10:e40d8724268a 219 // pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 10:e40d8724268a 220 return false;
JonFreeman 10:e40d8724268a 221 }
JonFreeman 10:e40d8724268a 222 if (visible_mode != m) { // Mode change, kill volts and amps to be safe
JonFreeman 10:e40d8724268a 223 set_V_limit (0.0);
JonFreeman 10:e40d8724268a 224 set_I_limit (0.0);
JonFreeman 10:e40d8724268a 225 visible_mode = m;
JonFreeman 10:e40d8724268a 226 }
JonFreeman 10:e40d8724268a 227 if (m == FORWARD || m == REVERSE)
JonFreeman 10:e40d8724268a 228 m ^= direction;
JonFreeman 10:e40d8724268a 229 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 230 return true;
JonFreeman 10:e40d8724268a 231 }
JonFreeman 10:e40d8724268a 232
JonFreeman 10:e40d8724268a 233
JonFreeman 10:e40d8724268a 234 void brushless_motor::motor_set ()
JonFreeman 10:e40d8724268a 235 {
JonFreeman 11:bfb73f083009 236 Hall_index[0] = read_Halls ();
JonFreeman 11:bfb73f083009 237 // *Motor_Port = lut[inner_mode | Hindex[0]];
JonFreeman 11:bfb73f083009 238 OP = lut[inner_mode | Hall_index[0]];
JonFreeman 10:e40d8724268a 239 }