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@12:d1d21a2941ef, 2019-03-04 (annotated)

Committer:
JonFreeman
Date:
Mon Mar 04 17:51:08 2019 +0000
Revision:
12:d1d21a2941ef
Parent:
11:bfb73f083009
Child:
13:ef7a06fa11de
STM3 ESC dual motor controller boards. Always 'Work In Progress', working snapshot March 2019

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 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