Jon Freeman / Mbed 2 deprecated Brushless_STM3_ESC_2019_10

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BufferedSerial Servo PCT2075 FastPWM

brushless_motor.cpp@10:e40d8724268a, 2019-01-15 (annotated)

Committer:
JonFreeman
Date:
Tue Jan 15 09:03:57 2019 +0000
Revision:
10:e40d8724268a
Child:
11:bfb73f083009
Buggered serial comms to TS controller

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 10:e40d8724268a 10 brushless_motor::brushless_motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
JonFreeman 10:e40d8724268a 11 {
JonFreeman 10:e40d8724268a 12 // Constructor
JonFreeman 10:e40d8724268a 13 maxV = _maxV_;
JonFreeman 10:e40d8724268a 14 maxI = _maxI_;
JonFreeman 10:e40d8724268a 15 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 16 latest_pulses_per_sec = 0;
JonFreeman 10:e40d8724268a 17 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 10:e40d8724268a 18 edge_count_table[i] = 0;
JonFreeman 10:e40d8724268a 19 // if (lutptr != A_tabl && lutptr != B_tabl)
JonFreeman 10:e40d8724268a 20 // pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
JonFreeman 10:e40d8724268a 21 Hall_tab_ptr = 0;
JonFreeman 10:e40d8724268a 22 Motor_Port = P;
JonFreeman 10:e40d8724268a 23 // pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
JonFreeman 10:e40d8724268a 24 maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 10:e40d8724268a 25 maxI->period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 10:e40d8724268a 26 maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 10:e40d8724268a 27 maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 10:e40d8724268a 28 visible_mode = REGENBRAKE;
JonFreeman 10:e40d8724268a 29 inner_mode = REGENBRAKE;
JonFreeman 10:e40d8724268a 30 lut = lutptr;
JonFreeman 10:e40d8724268a 31 Hindex[0] = Hindex[1] = read_Halls ();
JonFreeman 10:e40d8724268a 32 ppstmp = 0;
JonFreeman 10:e40d8724268a 33 tickleon = 0;
JonFreeman 10:e40d8724268a 34 direction = 0;
JonFreeman 10:e40d8724268a 35 angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel
JonFreeman 10:e40d8724268a 36 encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction
JonFreeman 10:e40d8724268a 37 Hall1 = Hall[0];
JonFreeman 10:e40d8724268a 38 Hall2 = Hall[1];
JonFreeman 10:e40d8724268a 39 Hall3 = Hall[2];
JonFreeman 10:e40d8724268a 40 PPS = 0;
JonFreeman 10:e40d8724268a 41 RPM = 0;
JonFreeman 10:e40d8724268a 42 last_V = last_I = 0.0;
JonFreeman 10:e40d8724268a 43 int x = read_Halls ();
JonFreeman 10:e40d8724268a 44 if (x == 7)
JonFreeman 10:e40d8724268a 45 dc_motor = true;
JonFreeman 10:e40d8724268a 46 else
JonFreeman 10:e40d8724268a 47 dc_motor = false;
JonFreeman 10:e40d8724268a 48 }
JonFreeman 10:e40d8724268a 49
JonFreeman 10:e40d8724268a 50 bool brushless_motor::motor_is_brushless ()
JonFreeman 10:e40d8724268a 51 {
JonFreeman 10:e40d8724268a 52 /* int x = read_Halls ();
JonFreeman 10:e40d8724268a 53 if (x < 1 || x > 6)
JonFreeman 10:e40d8724268a 54 return false;
JonFreeman 10:e40d8724268a 55 return true;
JonFreeman 10:e40d8724268a 56 */
JonFreeman 10:e40d8724268a 57 return !dc_motor;
JonFreeman 10:e40d8724268a 58 }
JonFreeman 10:e40d8724268a 59
JonFreeman 10:e40d8724268a 60 /**
JonFreeman 10:e40d8724268a 61 void brushless_motor::direction_set (int dir) {
JonFreeman 10:e40d8724268a 62 Used to set direction according to mode data from eeprom
JonFreeman 10:e40d8724268a 63 */
JonFreeman 10:e40d8724268a 64 void brushless_motor::direction_set (int dir)
JonFreeman 10:e40d8724268a 65 {
JonFreeman 10:e40d8724268a 66 if (dir != 0)
JonFreeman 10:e40d8724268a 67 dir = FORWARD | REVERSE; // bits used in eor
JonFreeman 10:e40d8724268a 68 direction = dir;
JonFreeman 10:e40d8724268a 69 }
JonFreeman 10:e40d8724268a 70
JonFreeman 10:e40d8724268a 71 int brushless_motor::read_Halls ()
JonFreeman 10:e40d8724268a 72 {
JonFreeman 10:e40d8724268a 73 int x = 0;
JonFreeman 10:e40d8724268a 74 if (*Hall1 != 0) x |= 1;
JonFreeman 10:e40d8724268a 75 if (*Hall2 != 0) x |= 2;
JonFreeman 10:e40d8724268a 76 if (*Hall3 != 0) x |= 4;
JonFreeman 10:e40d8724268a 77 return x;
JonFreeman 10:e40d8724268a 78 }
JonFreeman 10:e40d8724268a 79
JonFreeman 10:e40d8724268a 80 void brushless_motor::high_side_off ()
JonFreeman 10:e40d8724268a 81 {
JonFreeman 10:e40d8724268a 82 uint16_t p = *Motor_Port;
JonFreeman 10:e40d8724268a 83 p &= lut[32]; // KEEP_L_MASK_A or B
JonFreeman 10:e40d8724268a 84 *Motor_Port = p;
JonFreeman 10:e40d8724268a 85 }
JonFreeman 10:e40d8724268a 86
JonFreeman 10:e40d8724268a 87 void brushless_motor::low_side_on ()
JonFreeman 10:e40d8724268a 88 {
JonFreeman 10:e40d8724268a 89 // uint16_t p = *Motor_Port;
JonFreeman 10:e40d8724268a 90 // p &= lut[31]; // KEEP_L_MASK_A or B
JonFreeman 10:e40d8724268a 91 *Motor_Port = lut[31];
JonFreeman 10:e40d8724268a 92 }
JonFreeman 10:e40d8724268a 93
JonFreeman 10:e40d8724268a 94 void brushless_motor::current_calc ()
JonFreeman 10:e40d8724268a 95 {
JonFreeman 10:e40d8724268a 96 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 10:e40d8724268a 97 I.max = 0;
JonFreeman 10:e40d8724268a 98 I.ave = 0;
JonFreeman 10:e40d8724268a 99 uint16_t sample;
JonFreeman 10:e40d8724268a 100 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 10:e40d8724268a 101 sample = current_samples[i];
JonFreeman 10:e40d8724268a 102 I.ave += sample;
JonFreeman 10:e40d8724268a 103 if (I.min > sample)
JonFreeman 10:e40d8724268a 104 I.min = sample;
JonFreeman 10:e40d8724268a 105 if (I.max < sample)
JonFreeman 10:e40d8724268a 106 I.max = sample;
JonFreeman 10:e40d8724268a 107 }
JonFreeman 10:e40d8724268a 108 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 10:e40d8724268a 109 }
JonFreeman 10:e40d8724268a 110
JonFreeman 10:e40d8724268a 111
JonFreeman 10:e40d8724268a 112 void brushless_motor::raw_V_pwm (int v)
JonFreeman 10:e40d8724268a 113 {
JonFreeman 10:e40d8724268a 114 if (v < 1) v = 1;
JonFreeman 10:e40d8724268a 115 if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
JonFreeman 10:e40d8724268a 116 maxV->pulsewidth_ticks (v);
JonFreeman 10:e40d8724268a 117 }
JonFreeman 10:e40d8724268a 118
JonFreeman 10:e40d8724268a 119 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 120 {
JonFreeman 10:e40d8724268a 121 if (p < 0.0)
JonFreeman 10:e40d8724268a 122 p = 0.0;
JonFreeman 10:e40d8724268a 123 if (p > 1.0)
JonFreeman 10:e40d8724268a 124 p = 1.0;
JonFreeman 10:e40d8724268a 125 last_V = p; // for read by diagnostics
JonFreeman 10:e40d8724268a 126 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 10:e40d8724268a 127 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 10:e40d8724268a 128 maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 10:e40d8724268a 129 }
JonFreeman 10:e40d8724268a 130
JonFreeman 10:e40d8724268a 131 void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 10:e40d8724268a 132 {
JonFreeman 10:e40d8724268a 133 int a;
JonFreeman 10:e40d8724268a 134 if (p < 0.0)
JonFreeman 10:e40d8724268a 135 p = 0.0;
JonFreeman 10:e40d8724268a 136 if (p > 1.0)
JonFreeman 10:e40d8724268a 137 p = 1.0;
JonFreeman 10:e40d8724268a 138 last_I = p;
JonFreeman 10:e40d8724268a 139 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 10:e40d8724268a 140 if (a > MAX_PWM_TICKS)
JonFreeman 10:e40d8724268a 141 a = MAX_PWM_TICKS;
JonFreeman 10:e40d8724268a 142 if (a < 0)
JonFreeman 10:e40d8724268a 143 a = 0;
JonFreeman 10:e40d8724268a 144 maxI->pulsewidth_ticks (a); // PWM
JonFreeman 10:e40d8724268a 145 }
JonFreeman 10:e40d8724268a 146
JonFreeman 10:e40d8724268a 147 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 148 {
JonFreeman 10:e40d8724268a 149 // Can also test for motor running or not here
JonFreeman 10:e40d8724268a 150 if (dc_motor)
JonFreeman 10:e40d8724268a 151 return 0;
JonFreeman 10:e40d8724268a 152 if (ppstmp == Hall_total) {
JonFreeman 10:e40d8724268a 153 // if (dc_motor || ppstmp == Hall_total) {
JonFreeman 10:e40d8724268a 154 moving_flag = false; // Zero Hall transitions since previous call - motor not moving
JonFreeman 10:e40d8724268a 155 tickleon = TICKLE_TIMES;
JonFreeman 10:e40d8724268a 156 } else {
JonFreeman 10:e40d8724268a 157 moving_flag = true;
JonFreeman 10:e40d8724268a 158 ppstmp = Hall_total;
JonFreeman 10:e40d8724268a 159 }
JonFreeman 10:e40d8724268a 160 latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
JonFreeman 10:e40d8724268a 161 edge_count_table[Hall_tab_ptr] = ppstmp;
JonFreeman 10:e40d8724268a 162 Hall_tab_ptr++;
JonFreeman 10:e40d8724268a 163 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 10:e40d8724268a 164 Hall_tab_ptr = 0;
JonFreeman 10:e40d8724268a 165 PPS = latest_pulses_per_sec;
JonFreeman 10:e40d8724268a 166 RPM = (latest_pulses_per_sec * 60) / 24;
JonFreeman 10:e40d8724268a 167 return latest_pulses_per_sec;
JonFreeman 10:e40d8724268a 168 }
JonFreeman 10:e40d8724268a 169
JonFreeman 10:e40d8724268a 170 bool brushless_motor::is_moving ()
JonFreeman 10:e40d8724268a 171 {
JonFreeman 10:e40d8724268a 172 return moving_flag;
JonFreeman 10:e40d8724268a 173 }
JonFreeman 10:e40d8724268a 174
JonFreeman 10:e40d8724268a 175 /**
JonFreeman 10:e40d8724268a 176 bool brushless_motor::set_mode (int m)
JonFreeman 10:e40d8724268a 177 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
JonFreeman 10:e40d8724268a 178 If this causes change of mode, also sets V and I to zero.
JonFreeman 10:e40d8724268a 179 */
JonFreeman 10:e40d8724268a 180 bool brushless_motor::set_mode (int m)
JonFreeman 10:e40d8724268a 181 {
JonFreeman 10:e40d8724268a 182 if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
JonFreeman 10:e40d8724268a 183 // pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 10:e40d8724268a 184 return false;
JonFreeman 10:e40d8724268a 185 }
JonFreeman 10:e40d8724268a 186 if (visible_mode != m) { // Mode change, kill volts and amps to be safe
JonFreeman 10:e40d8724268a 187 set_V_limit (0.0);
JonFreeman 10:e40d8724268a 188 set_I_limit (0.0);
JonFreeman 10:e40d8724268a 189 visible_mode = m;
JonFreeman 10:e40d8724268a 190 }
JonFreeman 10:e40d8724268a 191 if (m == FORWARD || m == REVERSE)
JonFreeman 10:e40d8724268a 192 m ^= direction;
JonFreeman 10:e40d8724268a 193 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 194 return true;
JonFreeman 10:e40d8724268a 195 }
JonFreeman 10:e40d8724268a 196
JonFreeman 10:e40d8724268a 197 void brushless_motor::Hall_change ()
JonFreeman 10:e40d8724268a 198 {
JonFreeman 10:e40d8724268a 199 const int32_t delta_theta_lut[] = { // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
JonFreeman 10:e40d8724268a 200 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
JonFreeman 10:e40d8724268a 201 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
JonFreeman 10:e40d8724268a 202 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
JonFreeman 10:e40d8724268a 203 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
JonFreeman 10:e40d8724268a 204 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
JonFreeman 10:e40d8724268a 205 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
JonFreeman 10:e40d8724268a 206 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
JonFreeman 10:e40d8724268a 207 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
JonFreeman 10:e40d8724268a 208 } ;
JonFreeman 10:e40d8724268a 209 int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
JonFreeman 10:e40d8724268a 210 if (delta_theta == 0)
JonFreeman 10:e40d8724268a 211 encoder_error_cnt++;
JonFreeman 10:e40d8724268a 212 else
JonFreeman 10:e40d8724268a 213 angle_cnt += delta_theta;
JonFreeman 10:e40d8724268a 214 *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18
JonFreeman 10:e40d8724268a 215 Hall_total++;
JonFreeman 10:e40d8724268a 216 Hindex[1] = Hindex[0];
JonFreeman 10:e40d8724268a 217 }
JonFreeman 10:e40d8724268a 218
JonFreeman 10:e40d8724268a 219 void brushless_motor::motor_set ()
JonFreeman 10:e40d8724268a 220 {
JonFreeman 10:e40d8724268a 221 Hindex[0] = read_Halls ();
JonFreeman 10:e40d8724268a 222 *Motor_Port = lut[inner_mode | Hindex[0]];
JonFreeman 10:e40d8724268a 223 }