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

main.cpp@7:6deaeace9a3e, 2018-06-17 (annotated)

Committer:
JonFreeman
Date:
Sun Jun 17 06:59:37 2018 +0000
Revision:
7:6deaeace9a3e
Parent:
6:f289a49c1eae
Child:
8:93203f473f6e
Firmware for STM3 Twin Brushless Motor Electronic Speed Controller; Snapshot at 17th June 2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:435bf84ce48a 1 #include "mbed.h"
JonFreeman 0:435bf84ce48a 2 #include "DualBLS.h"
JonFreeman 0:435bf84ce48a 3 #include "BufferedSerial.h"
JonFreeman 0:435bf84ce48a 4 #include "FastPWM.h"
JonFreeman 4:21d91465e4b1 5 #include "Servo.h"
JonFreeman 5:ca86a7848d54 6
JonFreeman 5:ca86a7848d54 7 /*
JonFreeman 5:ca86a7848d54 8 New 29th May 2018 - YET TO CODE FOR - Fwd/Rev line from possible remote hand control box has signal routed to T5
JonFreeman 7:6deaeace9a3e 9 Also new LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable
JonFreeman 5:ca86a7848d54 10 */
JonFreeman 5:ca86a7848d54 11
JonFreeman 5:ca86a7848d54 12
JonFreeman 0:435bf84ce48a 13 /* STM32F401RE - compile using NUCLEO-F401RE
JonFreeman 6:f289a49c1eae 14 // PROJECT - Dual Brushless Motor Controller - Jon Freeman June 2018.
JonFreeman 0:435bf84ce48a 15
JonFreeman 0:435bf84ce48a 16 AnalogIn to read each motor current
JonFreeman 0:435bf84ce48a 17
JonFreeman 0:435bf84ce48a 18 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 19 CONTROL PHILOSOPHY
JonFreeman 0:435bf84ce48a 20 This Bogie drive board software should ensure sensible control when commands supplied are not sensible !
JonFreeman 0:435bf84ce48a 21
JonFreeman 0:435bf84ce48a 22 That is, smooth transition between Drive, Coast and Brake to be implemented here.
JonFreeman 0:435bf84ce48a 23 The remote controller must not be relied upon to deliver sensible command sequences.
JonFreeman 0:435bf84ce48a 24
JonFreeman 0:435bf84ce48a 25 So much the better if the remote controller does issue sensible commands, but ...
JonFreeman 0:435bf84ce48a 26
JonFreeman 0:435bf84ce48a 27 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 28
JonFreeman 0:435bf84ce48a 29
JonFreeman 0:435bf84ce48a 30 */
JonFreeman 7:6deaeace9a3e 31 #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP
JonFreeman 7:6deaeace9a3e 32 #include "F401RE.h"
JonFreeman 6:f289a49c1eae 33 #endif
JonFreeman 7:6deaeace9a3e 34 #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP
JonFreeman 7:6deaeace9a3e 35 #include "F446ZE.h"
JonFreeman 6:f289a49c1eae 36 #endif
JonFreeman 0:435bf84ce48a 37 /* Global variable declarations */
JonFreeman 3:ecb00e0e8d68 38 volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
JonFreeman 5:ca86a7848d54 39 int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
JonFreeman 0:435bf84ce48a 40 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 41 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 42 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 5:ca86a7848d54 43 AtoD_Semaphore = 0;
JonFreeman 5:ca86a7848d54 44 int IAm;
JonFreeman 0:435bf84ce48a 45 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 0:435bf84ce48a 46 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 6:f289a49c1eae 47 bool temp_sensor_exists = false;
JonFreeman 7:6deaeace9a3e 48 bool eeprom_flag; // gets set according to 24LC674 being found or not
JonFreeman 7:6deaeace9a3e 49 bool mode_good_flag = false;
JonFreeman 6:f289a49c1eae 50 char mode_bytes[36];
JonFreeman 3:ecb00e0e8d68 51
JonFreeman 6:f289a49c1eae 52 uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
JonFreeman 6:f289a49c1eae 53 last_temp_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
JonFreeman 6:f289a49c1eae 54 // struct single_bogie_options bogie;
JonFreeman 7:6deaeace9a3e 55 double rpm2mph = 0.0; // gets calculated from eeprom mode entries if present
JonFreeman 0:435bf84ce48a 56 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 57
JonFreeman 0:435bf84ce48a 58 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 59 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 6:f289a49c1eae 60 Ticker temperature_find_ticker;
JonFreeman 6:f289a49c1eae 61 Timer temperature_timer;
JonFreeman 0:435bf84ce48a 62
JonFreeman 0:435bf84ce48a 63 // Interrupt Service Routines
JonFreeman 6:f289a49c1eae 64 void ISR_temperature_find_ticker () { // every 960 us, i.e. slightly faster than once per milli sec
JonFreeman 6:f289a49c1eae 65 static bool readflag = false;
JonFreeman 6:f289a49c1eae 66 int t = temperature_timer.read_ms ();
JonFreeman 6:f289a49c1eae 67 if ((t == 5) && (!readflag)) {
JonFreeman 6:f289a49c1eae 68 last_temp_count = temp_sensor_count;
JonFreeman 6:f289a49c1eae 69 temp_sensor_count = 0;
JonFreeman 6:f289a49c1eae 70 readflag = true;
JonFreeman 6:f289a49c1eae 71 }
JonFreeman 6:f289a49c1eae 72 if (t == 6)
JonFreeman 6:f289a49c1eae 73 readflag = false;
JonFreeman 6:f289a49c1eae 74 }
JonFreeman 0:435bf84ce48a 75
JonFreeman 0:435bf84ce48a 76 /** void ISR_loop_timer ()
JonFreeman 0:435bf84ce48a 77 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 0:435bf84ce48a 78 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 0:435bf84ce48a 79 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 0:435bf84ce48a 80 */
JonFreeman 0:435bf84ce48a 81 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 82 {
JonFreeman 0:435bf84ce48a 83 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 0:435bf84ce48a 84 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 85 if ((sys_timer & 0x03) == 0)
JonFreeman 0:435bf84ce48a 86 flag_8Hz = true;
JonFreeman 0:435bf84ce48a 87 }
JonFreeman 0:435bf84ce48a 88
JonFreeman 0:435bf84ce48a 89 /** void ISR_voltage_reader ()
JonFreeman 0:435bf84ce48a 90 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 0:435bf84ce48a 91 *
JonFreeman 0:435bf84ce48a 92 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 0:435bf84ce48a 93 */
JonFreeman 0:435bf84ce48a 94
JonFreeman 5:ca86a7848d54 95 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
JonFreeman 0:435bf84ce48a 96 {
JonFreeman 0:435bf84ce48a 97 AtoD_Semaphore++;
JonFreeman 2:04761b196473 98 fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 99 }
JonFreeman 0:435bf84ce48a 100
JonFreeman 0:435bf84ce48a 101
JonFreeman 4:21d91465e4b1 102 class RControl_In
JonFreeman 4:21d91465e4b1 103 { // Read servo style pwm input
JonFreeman 0:435bf84ce48a 104 Timer t;
JonFreeman 0:435bf84ce48a 105 int32_t clock_old, clock_new;
JonFreeman 0:435bf84ce48a 106 int32_t pulse_width_us, period_us;
JonFreeman 4:21d91465e4b1 107 public:
JonFreeman 4:21d91465e4b1 108 RControl_In () {
JonFreeman 0:435bf84ce48a 109 pulse_width_us = period_us = 0;
JonFreeman 4:21d91465e4b1 110 com2.printf ("Setting up Radio_Congtrol_In\r\n");
JonFreeman 4:21d91465e4b1 111 } ;
JonFreeman 0:435bf84ce48a 112 bool validate_rx () ;
JonFreeman 0:435bf84ce48a 113 void rise () ;
JonFreeman 0:435bf84ce48a 114 void fall () ;
JonFreeman 4:21d91465e4b1 115 uint32_t pulsewidth () ;
JonFreeman 4:21d91465e4b1 116 uint32_t period () ;
JonFreeman 4:21d91465e4b1 117 bool rx_active;
JonFreeman 4:21d91465e4b1 118 } ;
JonFreeman 0:435bf84ce48a 119
JonFreeman 4:21d91465e4b1 120 uint32_t RControl_In::pulsewidth () {
JonFreeman 4:21d91465e4b1 121 return pulse_width_us;
JonFreeman 4:21d91465e4b1 122 }
JonFreeman 4:21d91465e4b1 123
JonFreeman 4:21d91465e4b1 124 uint32_t RControl_In::period () {
JonFreeman 4:21d91465e4b1 125 return period_us;
JonFreeman 4:21d91465e4b1 126 }
JonFreeman 4:21d91465e4b1 127
JonFreeman 4:21d91465e4b1 128 bool RControl_In::validate_rx ()
JonFreeman 0:435bf84ce48a 129 {
JonFreeman 0:435bf84ce48a 130 if ((clock() - clock_new) > 4)
JonFreeman 0:435bf84ce48a 131 rx_active = false;
JonFreeman 0:435bf84ce48a 132 else
JonFreeman 0:435bf84ce48a 133 rx_active = true;
JonFreeman 0:435bf84ce48a 134 return rx_active;
JonFreeman 0:435bf84ce48a 135 }
JonFreeman 0:435bf84ce48a 136
JonFreeman 6:f289a49c1eae 137 void RControl_In::rise () // These may not work as use of PortB as port may bugger InterruptIn use
JonFreeman 0:435bf84ce48a 138 {
JonFreeman 0:435bf84ce48a 139 t.stop ();
JonFreeman 0:435bf84ce48a 140 period_us = t.read_us ();
JonFreeman 0:435bf84ce48a 141 t.reset ();
JonFreeman 0:435bf84ce48a 142 t.start ();
JonFreeman 0:435bf84ce48a 143 }
JonFreeman 4:21d91465e4b1 144 void RControl_In::fall ()
JonFreeman 0:435bf84ce48a 145 {
JonFreeman 0:435bf84ce48a 146 pulse_width_us = t.read_us ();
JonFreeman 0:435bf84ce48a 147 clock_old = clock_new;
JonFreeman 0:435bf84ce48a 148 clock_new = clock();
JonFreeman 0:435bf84ce48a 149 if ((clock_new - clock_old) < 4)
JonFreeman 0:435bf84ce48a 150 rx_active = true;
JonFreeman 0:435bf84ce48a 151 }
JonFreeman 4:21d91465e4b1 152
JonFreeman 0:435bf84ce48a 153
JonFreeman 4:21d91465e4b1 154 Servo * Servos[2];
JonFreeman 4:21d91465e4b1 155
JonFreeman 0:435bf84ce48a 156 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 157 /*
JonFreeman 0:435bf84ce48a 158 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 159
JonFreeman 3:ecb00e0e8d68 160 1 1 1 1 0 0 0 ---___---___ Hall1
JonFreeman 3:ecb00e0e8d68 161 2 0 0 1 1 1 0 __---___---_ Hall2
JonFreeman 3:ecb00e0e8d68 162 4 1 0 0 0 1 1 -___---___-- Hall3
JonFreeman 0:435bf84ce48a 163
JonFreeman 0:435bf84ce48a 164 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 0:435bf84ce48a 165 */
JonFreeman 3:ecb00e0e8d68 166 const uint16_t A_tabl[] = { // Origial table
JonFreeman 0:435bf84ce48a 167 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 168 0, AWV,AVU,AWU,AUW,AUV,AVW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
JonFreeman 0:435bf84ce48a 169 0, AVW,AUV,AUW,AWU,AVU,AWV, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
JonFreeman 3:ecb00e0e8d68 170 0, BRA,BRA,BRA,BRA,BRA,BRA,0, // Regenerative Braking
JonFreeman 4:21d91465e4b1 171 KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
JonFreeman 3:ecb00e0e8d68 172 } ;
JonFreeman 4:21d91465e4b1 173 InterruptIn * AHarr[] = {
JonFreeman 4:21d91465e4b1 174 &MAH1,
JonFreeman 4:21d91465e4b1 175 &MAH2,
JonFreeman 4:21d91465e4b1 176 &MAH3
JonFreeman 3:ecb00e0e8d68 177 } ;
JonFreeman 0:435bf84ce48a 178 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 179 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 180 0, BWV,BVU,BWU,BUW,BUV,BVW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
JonFreeman 0:435bf84ce48a 181 0, BVW,BUV,BUW,BWU,BVU,BWV, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
JonFreeman 3:ecb00e0e8d68 182 0, BRB,BRB,BRB,BRB,BRB,BRB,0, // Regenerative Braking
JonFreeman 4:21d91465e4b1 183 KEEP_L_MASK_B, KEEP_H_MASK_B
JonFreeman 3:ecb00e0e8d68 184 } ;
JonFreeman 4:21d91465e4b1 185 InterruptIn * BHarr[] = {
JonFreeman 4:21d91465e4b1 186 &MBH1,
JonFreeman 4:21d91465e4b1 187 &MBH2,
JonFreeman 4:21d91465e4b1 188 &MBH3
JonFreeman 0:435bf84ce48a 189 } ;
JonFreeman 0:435bf84ce48a 190
JonFreeman 0:435bf84ce48a 191 class motor
JonFreeman 0:435bf84ce48a 192 {
JonFreeman 5:ca86a7848d54 193 uint32_t Hall_total, visible_mode, inner_mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
JonFreeman 3:ecb00e0e8d68 194 uint32_t latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp;
JonFreeman 3:ecb00e0e8d68 195 bool moving_flag;
JonFreeman 0:435bf84ce48a 196 const uint16_t * lut;
JonFreeman 0:435bf84ce48a 197 FastPWM * maxV, * maxI;
JonFreeman 0:435bf84ce48a 198 PortOut * Motor_Port;
JonFreeman 2:04761b196473 199 InterruptIn * Hall1, * Hall2, * Hall3;
JonFreeman 0:435bf84ce48a 200 public:
JonFreeman 2:04761b196473 201 struct currents {
JonFreeman 2:04761b196473 202 uint32_t max, min, ave;
JonFreeman 2:04761b196473 203 } I;
JonFreeman 3:ecb00e0e8d68 204 int32_t angle_cnt;
JonFreeman 0:435bf84ce48a 205 uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
JonFreeman 3:ecb00e0e8d68 206 uint32_t Hindex[2], tickleon, encoder_error_cnt;
JonFreeman 5:ca86a7848d54 207 uint32_t RPM, PPS;
JonFreeman 5:ca86a7848d54 208 double last_V, last_I;
JonFreeman 0:435bf84ce48a 209 motor () {} ; // Default constructor
JonFreeman 4:21d91465e4b1 210 motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
JonFreeman 0:435bf84ce48a 211 void set_V_limit (double) ; // Sets max motor voltage
JonFreeman 0:435bf84ce48a 212 void set_I_limit (double) ; // Sets max motor current
JonFreeman 0:435bf84ce48a 213 void Hall_change () ; // Called in response to edge on Hall input pin
JonFreeman 3:ecb00e0e8d68 214 void motor_set () ; // Energise Port with data determined by Hall sensors
JonFreeman 3:ecb00e0e8d68 215 void direction_set (int) ; // sets 'direction' with bit pattern to eor with FORWARD or REVERSE in set_mode
JonFreeman 3:ecb00e0e8d68 216 bool set_mode (int); // sets mode to HANDBRAKE, FORWARD, REVERSE or REGENBRAKE
JonFreeman 3:ecb00e0e8d68 217 bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs
JonFreeman 3:ecb00e0e8d68 218 void current_calc () ; // Updates 3 uint32_t I.min, I.ave, I.max
JonFreeman 3:ecb00e0e8d68 219 uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
JonFreeman 3:ecb00e0e8d68 220 int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
JonFreeman 3:ecb00e0e8d68 221 void high_side_off () ;
JonFreeman 3:ecb00e0e8d68 222 } ; //MotorA, MotorB, or even Motor[2];
JonFreeman 0:435bf84ce48a 223
JonFreeman 4:21d91465e4b1 224 motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
JonFreeman 4:21d91465e4b1 225 motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr);
JonFreeman 0:435bf84ce48a 226
JonFreeman 7:6deaeace9a3e 227 //motor * MotPtr[8]; // Array of pointers to some number of motor objects
JonFreeman 3:ecb00e0e8d68 228
JonFreeman 4:21d91465e4b1 229 motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
JonFreeman 3:ecb00e0e8d68 230 { // Constructor
JonFreeman 0:435bf84ce48a 231 maxV = _maxV_;
JonFreeman 0:435bf84ce48a 232 maxI = _maxI_;
JonFreeman 3:ecb00e0e8d68 233 Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
JonFreeman 0:435bf84ce48a 234 latest_pulses_per_sec = 0;
JonFreeman 0:435bf84ce48a 235 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 0:435bf84ce48a 236 edge_count_table[i] = 0;
JonFreeman 0:435bf84ce48a 237 if (lutptr != A_tabl && lutptr != B_tabl)
JonFreeman 0:435bf84ce48a 238 pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
JonFreeman 3:ecb00e0e8d68 239 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 240 Motor_Port = P;
JonFreeman 0:435bf84ce48a 241 pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
JonFreeman 0:435bf84ce48a 242 maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:435bf84ce48a 243 maxI->period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:435bf84ce48a 244 maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 0:435bf84ce48a 245 maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 5:ca86a7848d54 246 visible_mode = REGENBRAKE;
JonFreeman 5:ca86a7848d54 247 inner_mode = REGENBRAKE;
JonFreeman 0:435bf84ce48a 248 lut = lutptr;
JonFreeman 3:ecb00e0e8d68 249 Hindex[0] = Hindex[1] = read_Halls ();
JonFreeman 3:ecb00e0e8d68 250 ppstmp = 0;
JonFreeman 3:ecb00e0e8d68 251 tickleon = 0;
JonFreeman 3:ecb00e0e8d68 252 direction = 0;
JonFreeman 3:ecb00e0e8d68 253 angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel
JonFreeman 3:ecb00e0e8d68 254 encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction
JonFreeman 4:21d91465e4b1 255 Hall1 = Hall[0];
JonFreeman 4:21d91465e4b1 256 Hall2 = Hall[1];
JonFreeman 4:21d91465e4b1 257 Hall3 = Hall[2];
JonFreeman 5:ca86a7848d54 258 PPS = 0;
JonFreeman 5:ca86a7848d54 259 RPM = 0;
JonFreeman 5:ca86a7848d54 260 last_V = last_I = 0.0;
JonFreeman 3:ecb00e0e8d68 261 }
JonFreeman 3:ecb00e0e8d68 262
JonFreeman 5:ca86a7848d54 263 /**
JonFreeman 5:ca86a7848d54 264 void motor::direction_set (int dir) {
JonFreeman 5:ca86a7848d54 265 Used to set direction according to mode data from eeprom
JonFreeman 5:ca86a7848d54 266 */
JonFreeman 3:ecb00e0e8d68 267 void motor::direction_set (int dir) {
JonFreeman 3:ecb00e0e8d68 268 if (dir != 0)
JonFreeman 3:ecb00e0e8d68 269 dir = FORWARD | REVERSE; // bits used in eor
JonFreeman 3:ecb00e0e8d68 270 direction = dir;
JonFreeman 0:435bf84ce48a 271 }
JonFreeman 0:435bf84ce48a 272
JonFreeman 1:0fabe6fdb55b 273 int motor::read_Halls () {
JonFreeman 2:04761b196473 274 int x = 0;
JonFreeman 2:04761b196473 275 if (*Hall1 != 0) x |= 1;
JonFreeman 2:04761b196473 276 if (*Hall2 != 0) x |= 2;
JonFreeman 2:04761b196473 277 if (*Hall3 != 0) x |= 4;
JonFreeman 2:04761b196473 278 return x;
JonFreeman 2:04761b196473 279 }
JonFreeman 2:04761b196473 280
JonFreeman 3:ecb00e0e8d68 281 void motor::high_side_off () {
JonFreeman 3:ecb00e0e8d68 282 uint16_t p = *Motor_Port;
JonFreeman 3:ecb00e0e8d68 283 p &= lut[32]; // KEEP_L_MASK_A or B
JonFreeman 3:ecb00e0e8d68 284 *Motor_Port = p;
JonFreeman 1:0fabe6fdb55b 285 }
JonFreeman 1:0fabe6fdb55b 286
JonFreeman 0:435bf84ce48a 287 void motor::current_calc ()
JonFreeman 0:435bf84ce48a 288 {
JonFreeman 0:435bf84ce48a 289 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 0:435bf84ce48a 290 I.max = 0;
JonFreeman 0:435bf84ce48a 291 I.ave = 0;
JonFreeman 0:435bf84ce48a 292 uint16_t sample;
JonFreeman 0:435bf84ce48a 293 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 0:435bf84ce48a 294 sample = current_samples[i];
JonFreeman 0:435bf84ce48a 295 I.ave += sample;
JonFreeman 0:435bf84ce48a 296 if (I.min > sample)
JonFreeman 0:435bf84ce48a 297 I.min = sample;
JonFreeman 0:435bf84ce48a 298 if (I.max < sample)
JonFreeman 0:435bf84ce48a 299 I.max = sample;
JonFreeman 0:435bf84ce48a 300 }
JonFreeman 0:435bf84ce48a 301 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 0:435bf84ce48a 302 }
JonFreeman 0:435bf84ce48a 303
JonFreeman 0:435bf84ce48a 304 void motor::set_V_limit (double p) // Sets max motor voltage
JonFreeman 0:435bf84ce48a 305 {
JonFreeman 0:435bf84ce48a 306 if (p < 0.0)
JonFreeman 0:435bf84ce48a 307 p = 0.0;
JonFreeman 0:435bf84ce48a 308 if (p > 1.0)
JonFreeman 0:435bf84ce48a 309 p = 1.0;
JonFreeman 5:ca86a7848d54 310 last_V = p; // for read by diagnostics
JonFreeman 0:435bf84ce48a 311 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:435bf84ce48a 312 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:435bf84ce48a 313 maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 0:435bf84ce48a 314 }
JonFreeman 0:435bf84ce48a 315
JonFreeman 0:435bf84ce48a 316 void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 0:435bf84ce48a 317 {
JonFreeman 0:435bf84ce48a 318 int a;
JonFreeman 0:435bf84ce48a 319 if (p < 0.0)
JonFreeman 0:435bf84ce48a 320 p = 0.0;
JonFreeman 0:435bf84ce48a 321 if (p > 1.0)
JonFreeman 0:435bf84ce48a 322 p = 1.0;
JonFreeman 5:ca86a7848d54 323 last_I = p;
JonFreeman 0:435bf84ce48a 324 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:435bf84ce48a 325 if (a > MAX_PWM_TICKS)
JonFreeman 0:435bf84ce48a 326 a = MAX_PWM_TICKS;
JonFreeman 0:435bf84ce48a 327 if (a < 0)
JonFreeman 0:435bf84ce48a 328 a = 0;
JonFreeman 0:435bf84ce48a 329 maxI->pulsewidth_ticks (a); // PWM
JonFreeman 0:435bf84ce48a 330 }
JonFreeman 0:435bf84ce48a 331
JonFreeman 3:ecb00e0e8d68 332 uint32_t motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
JonFreeman 3:ecb00e0e8d68 333 { // Can also test for motor running or not here
JonFreeman 3:ecb00e0e8d68 334 if (ppstmp == Hall_total) {
JonFreeman 3:ecb00e0e8d68 335 moving_flag = false; // Zero Hall transitions since previous call - motor not moving
JonFreeman 4:21d91465e4b1 336 tickleon = TICKLE_TIMES;
JonFreeman 3:ecb00e0e8d68 337 }
JonFreeman 3:ecb00e0e8d68 338 else {
JonFreeman 3:ecb00e0e8d68 339 moving_flag = true;
JonFreeman 3:ecb00e0e8d68 340 ppstmp = Hall_total;
JonFreeman 3:ecb00e0e8d68 341 }
JonFreeman 3:ecb00e0e8d68 342 latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
JonFreeman 3:ecb00e0e8d68 343 edge_count_table[Hall_tab_ptr] = ppstmp;
JonFreeman 0:435bf84ce48a 344 Hall_tab_ptr++;
JonFreeman 0:435bf84ce48a 345 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 0:435bf84ce48a 346 Hall_tab_ptr = 0;
JonFreeman 5:ca86a7848d54 347 PPS = latest_pulses_per_sec;
JonFreeman 5:ca86a7848d54 348 RPM = (latest_pulses_per_sec * 60) / 24;
JonFreeman 0:435bf84ce48a 349 return latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 350 }
JonFreeman 0:435bf84ce48a 351
JonFreeman 3:ecb00e0e8d68 352 bool motor::is_moving ()
JonFreeman 3:ecb00e0e8d68 353 {
JonFreeman 3:ecb00e0e8d68 354 return moving_flag;
JonFreeman 3:ecb00e0e8d68 355 }
JonFreeman 3:ecb00e0e8d68 356
JonFreeman 5:ca86a7848d54 357 /**
JonFreeman 5:ca86a7848d54 358 bool motor::set_mode (int m)
JonFreeman 5:ca86a7848d54 359 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
JonFreeman 5:ca86a7848d54 360 If this causes change of mode, also sets V and I to zero.
JonFreeman 5:ca86a7848d54 361 */
JonFreeman 0:435bf84ce48a 362 bool motor::set_mode (int m)
JonFreeman 0:435bf84ce48a 363 {
JonFreeman 2:04761b196473 364 if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
JonFreeman 2:04761b196473 365 pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 0:435bf84ce48a 366 return false;
JonFreeman 2:04761b196473 367 }
JonFreeman 5:ca86a7848d54 368 if (visible_mode != m) { // Mode change, kill volts and amps to be safe
JonFreeman 5:ca86a7848d54 369 set_V_limit (0.0);
JonFreeman 5:ca86a7848d54 370 set_I_limit (0.0);
JonFreeman 5:ca86a7848d54 371 visible_mode = m;
JonFreeman 5:ca86a7848d54 372 }
JonFreeman 3:ecb00e0e8d68 373 if (m == FORWARD || m == REVERSE)
JonFreeman 3:ecb00e0e8d68 374 m ^= direction;
JonFreeman 5:ca86a7848d54 375 inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
JonFreeman 0:435bf84ce48a 376 return true;
JonFreeman 0:435bf84ce48a 377 }
JonFreeman 0:435bf84ce48a 378
JonFreeman 0:435bf84ce48a 379 void motor::Hall_change ()
JonFreeman 0:435bf84ce48a 380 {
JonFreeman 3:ecb00e0e8d68 381 const int32_t delta_theta_lut[] = // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
JonFreeman 3:ecb00e0e8d68 382 {
JonFreeman 3:ecb00e0e8d68 383 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
JonFreeman 3:ecb00e0e8d68 384 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
JonFreeman 3:ecb00e0e8d68 385 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
JonFreeman 3:ecb00e0e8d68 386 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
JonFreeman 3:ecb00e0e8d68 387 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
JonFreeman 3:ecb00e0e8d68 388 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
JonFreeman 3:ecb00e0e8d68 389 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
JonFreeman 3:ecb00e0e8d68 390 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
JonFreeman 3:ecb00e0e8d68 391 } ;
JonFreeman 3:ecb00e0e8d68 392 int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
JonFreeman 3:ecb00e0e8d68 393 if (delta_theta == 0)
JonFreeman 3:ecb00e0e8d68 394 encoder_error_cnt++;
JonFreeman 3:ecb00e0e8d68 395 else
JonFreeman 3:ecb00e0e8d68 396 angle_cnt += delta_theta;
JonFreeman 5:ca86a7848d54 397 *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18
JonFreeman 0:435bf84ce48a 398 Hall_total++;
JonFreeman 3:ecb00e0e8d68 399 Hindex[1] = Hindex[0];
JonFreeman 0:435bf84ce48a 400 }
JonFreeman 2:04761b196473 401
JonFreeman 5:ca86a7848d54 402
JonFreeman 6:f289a49c1eae 403 void temp_sensor_isr () { // got rising edge from LMT01. ALMOST CERTAIN this misses some
JonFreeman 6:f289a49c1eae 404 int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
JonFreeman 6:f289a49c1eae 405 temperature_timer.reset ();
JonFreeman 5:ca86a7848d54 406 temp_sensor_count++;
JonFreeman 6:f289a49c1eae 407 if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
JonFreeman 6:f289a49c1eae 408 temp_sensor_count++;
JonFreeman 6:f289a49c1eae 409 // T2 = !T2; // scope hanger
JonFreeman 5:ca86a7848d54 410 }
JonFreeman 5:ca86a7848d54 411
JonFreeman 3:ecb00e0e8d68 412 void MAH_isr ()
JonFreeman 0:435bf84ce48a 413 {
JonFreeman 3:ecb00e0e8d68 414 uint32_t x = 0;
JonFreeman 3:ecb00e0e8d68 415 MotorA.high_side_off ();
JonFreeman 3:ecb00e0e8d68 416 T3 = !T3;
JonFreeman 3:ecb00e0e8d68 417 if (MAH1 != 0) x |= 1;
JonFreeman 3:ecb00e0e8d68 418 if (MAH2 != 0) x |= 2;
JonFreeman 3:ecb00e0e8d68 419 if (MAH3 != 0) x |= 4;
JonFreeman 3:ecb00e0e8d68 420 MotorA.Hindex[0] = x; // New in [0], old in [1]
JonFreeman 0:435bf84ce48a 421 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 422 }
JonFreeman 0:435bf84ce48a 423
JonFreeman 3:ecb00e0e8d68 424 void MBH_isr ()
JonFreeman 0:435bf84ce48a 425 {
JonFreeman 3:ecb00e0e8d68 426 uint32_t x = 0;
JonFreeman 3:ecb00e0e8d68 427 MotorB.high_side_off ();
JonFreeman 3:ecb00e0e8d68 428 if (MBH1 != 0) x |= 1;
JonFreeman 3:ecb00e0e8d68 429 if (MBH2 != 0) x |= 2;
JonFreeman 3:ecb00e0e8d68 430 if (MBH3 != 0) x |= 4;
JonFreeman 3:ecb00e0e8d68 431 MotorB.Hindex[0] = x;
JonFreeman 0:435bf84ce48a 432 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 433 }
JonFreeman 0:435bf84ce48a 434
JonFreeman 0:435bf84ce48a 435
JonFreeman 0:435bf84ce48a 436 // End of Interrupt Service Routines
JonFreeman 0:435bf84ce48a 437
JonFreeman 3:ecb00e0e8d68 438 void motor::motor_set ()
JonFreeman 3:ecb00e0e8d68 439 {
JonFreeman 3:ecb00e0e8d68 440 Hindex[0] = read_Halls ();
JonFreeman 5:ca86a7848d54 441 *Motor_Port = lut[inner_mode | Hindex[0]];
JonFreeman 3:ecb00e0e8d68 442 }
JonFreeman 3:ecb00e0e8d68 443
JonFreeman 2:04761b196473 444 void setVI (double v, double i) {
JonFreeman 4:21d91465e4b1 445 MotorA.set_V_limit (v); // Sets max motor voltage
JonFreeman 4:21d91465e4b1 446 MotorA.set_I_limit (i); // Sets max motor current
JonFreeman 4:21d91465e4b1 447 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 448 MotorB.set_I_limit (i);
JonFreeman 4:21d91465e4b1 449 }
JonFreeman 4:21d91465e4b1 450 void setV (double v) {
JonFreeman 2:04761b196473 451 MotorA.set_V_limit (v);
JonFreeman 4:21d91465e4b1 452 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 453 }
JonFreeman 4:21d91465e4b1 454 void setI (double i) {
JonFreeman 2:04761b196473 455 MotorA.set_I_limit (i);
JonFreeman 2:04761b196473 456 MotorB.set_I_limit (i);
JonFreeman 0:435bf84ce48a 457 }
JonFreeman 2:04761b196473 458
JonFreeman 5:ca86a7848d54 459 void read_RPM (uint32_t * dest) {
JonFreeman 5:ca86a7848d54 460 dest[0] = MotorA.RPM;
JonFreeman 5:ca86a7848d54 461 dest[1] = MotorB.RPM;
JonFreeman 5:ca86a7848d54 462 }
JonFreeman 5:ca86a7848d54 463
JonFreeman 5:ca86a7848d54 464 void read_PPS (uint32_t * dest) {
JonFreeman 5:ca86a7848d54 465 dest[0] = MotorA.PPS;
JonFreeman 5:ca86a7848d54 466 dest[1] = MotorB.PPS;
JonFreeman 5:ca86a7848d54 467 }
JonFreeman 5:ca86a7848d54 468
JonFreeman 5:ca86a7848d54 469 void read_last_VI (double * d) { // only for test from cli
JonFreeman 5:ca86a7848d54 470 d[0] = MotorA.last_V;
JonFreeman 5:ca86a7848d54 471 d[1] = MotorA.last_I;
JonFreeman 5:ca86a7848d54 472 d[2] = MotorB.last_V;
JonFreeman 5:ca86a7848d54 473 d[3] = MotorB.last_I;
JonFreeman 5:ca86a7848d54 474 }
JonFreeman 5:ca86a7848d54 475
JonFreeman 3:ecb00e0e8d68 476
JonFreeman 5:ca86a7848d54 477 /**
JonFreeman 5:ca86a7848d54 478 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 5:ca86a7848d54 479 Not part of ISR
JonFreeman 5:ca86a7848d54 480 */
JonFreeman 3:ecb00e0e8d68 481 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 0:435bf84ce48a 482 {
JonFreeman 6:f289a49c1eae 483 static uint32_t i = 0, tab_ptr = 0;
JonFreeman 3:ecb00e0e8d68 484
JonFreeman 3:ecb00e0e8d68 485 if (MotorA.tickleon)
JonFreeman 3:ecb00e0e8d68 486 MotorA.high_side_off ();
JonFreeman 3:ecb00e0e8d68 487 if (MotorB.tickleon)
JonFreeman 3:ecb00e0e8d68 488 MotorB.high_side_off ();
JonFreeman 0:435bf84ce48a 489 if (AtoD_Semaphore > 20) {
JonFreeman 0:435bf84ce48a 490 pc.printf ("WARNING - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore);
JonFreeman 0:435bf84ce48a 491 AtoD_Semaphore = 20;
JonFreeman 0:435bf84ce48a 492 }
JonFreeman 0:435bf84ce48a 493 while (AtoD_Semaphore > 0) {
JonFreeman 0:435bf84ce48a 494 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 495 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 496 switch (i) { //
JonFreeman 0:435bf84ce48a 497 case 0:
JonFreeman 0:435bf84ce48a 498 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 499 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 500 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 501 case 1:
JonFreeman 0:435bf84ce48a 502 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 503 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 504 break;
JonFreeman 0:435bf84ce48a 505 case 2:
JonFreeman 0:435bf84ce48a 506 MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 507 break;
JonFreeman 0:435bf84ce48a 508 case 3:
JonFreeman 0:435bf84ce48a 509 MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 510 if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter
JonFreeman 0:435bf84ce48a 511 tab_ptr = 0;
JonFreeman 0:435bf84ce48a 512 break;
JonFreeman 0:435bf84ce48a 513 }
JonFreeman 0:435bf84ce48a 514 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 515 if (i > 3)
JonFreeman 0:435bf84ce48a 516 i = 0;
JonFreeman 3:ecb00e0e8d68 517 } // end of while (AtoD_Semaphore > 0) {
JonFreeman 3:ecb00e0e8d68 518 if (MotorA.tickleon) {
JonFreeman 3:ecb00e0e8d68 519 MotorA.tickleon--;
JonFreeman 5:ca86a7848d54 520 MotorA.motor_set (); // Reactivate any high side switches turned off above
JonFreeman 3:ecb00e0e8d68 521 }
JonFreeman 3:ecb00e0e8d68 522 if (MotorB.tickleon) {
JonFreeman 3:ecb00e0e8d68 523 MotorB.tickleon--;
JonFreeman 3:ecb00e0e8d68 524 MotorB.motor_set ();
JonFreeman 0:435bf84ce48a 525 }
JonFreeman 0:435bf84ce48a 526 }
JonFreeman 0:435bf84ce48a 527
JonFreeman 0:435bf84ce48a 528 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 529 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 0:435bf84ce48a 530 * ISR also filters signal
JonFreeman 0:435bf84ce48a 531 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 532 */
JonFreeman 0:435bf84ce48a 533 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 534 {
JonFreeman 5:ca86a7848d54 535 return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 536 }
JonFreeman 0:435bf84ce48a 537
JonFreeman 0:435bf84ce48a 538 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 539 {
JonFreeman 5:ca86a7848d54 540 return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 541 }
JonFreeman 0:435bf84ce48a 542
JonFreeman 5:ca86a7848d54 543 void read_supply_vi (double * val) { // called from cli
JonFreeman 5:ca86a7848d54 544 val[0] = MotorA.I.ave;
JonFreeman 5:ca86a7848d54 545 val[1] = MotorB.I.ave;
JonFreeman 5:ca86a7848d54 546 val[2] = Read_BatteryVolts ();
JonFreeman 0:435bf84ce48a 547 }
JonFreeman 0:435bf84ce48a 548
JonFreeman 5:ca86a7848d54 549 void mode_set (int mode, double val) { // called from cli to set fw, re, rb, hb
JonFreeman 2:04761b196473 550 MotorA.set_mode (mode);
JonFreeman 2:04761b196473 551 MotorB.set_mode (mode);
JonFreeman 2:04761b196473 552 if (mode == REGENBRAKE) {
JonFreeman 5:ca86a7848d54 553 if (val > 1.0)
JonFreeman 5:ca86a7848d54 554 val = 1.0;
JonFreeman 5:ca86a7848d54 555 if (val < 0.0)
JonFreeman 5:ca86a7848d54 556 val = 0.0;
JonFreeman 5:ca86a7848d54 557 val *= 0.9; // set upper limit, this is essential
JonFreeman 5:ca86a7848d54 558 val = sqrt (val); // to linearise effect
JonFreeman 5:ca86a7848d54 559 setVI (val, 1.0);
JonFreeman 2:04761b196473 560 }
JonFreeman 2:04761b196473 561 }
JonFreeman 0:435bf84ce48a 562
JonFreeman 7:6deaeace9a3e 563 extern void setup_comms () ;
JonFreeman 7:6deaeace9a3e 564 extern void command_line_interpreter_pc () ;
JonFreeman 7:6deaeace9a3e 565 extern void command_line_interpreter_loco () ;
JonFreeman 0:435bf84ce48a 566 extern int check_24LC64 () ; // Call from near top of main() to init i2c bus
JonFreeman 0:435bf84ce48a 567 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 0:435bf84ce48a 568 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 0:435bf84ce48a 569
JonFreeman 5:ca86a7848d54 570 /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes
JonFreeman 3:ecb00e0e8d68 571 uint8_t MotA_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 572 MotB_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 573 gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode
JonFreeman 3:ecb00e0e8d68 574 serv1, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 575 serv2, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 576 cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
JonFreeman 3:ecb00e0e8d68 577 last;
JonFreeman 3:ecb00e0e8d68 578 } ;
JonFreeman 5:ca86a7848d54 579 */
JonFreeman 3:ecb00e0e8d68 580 int I_Am () { // Returns boards id number as ASCII char
JonFreeman 5:ca86a7848d54 581 return IAm;
JonFreeman 3:ecb00e0e8d68 582 }
JonFreeman 3:ecb00e0e8d68 583
JonFreeman 7:6deaeace9a3e 584 double mph (int rpm) {
JonFreeman 7:6deaeace9a3e 585 if (mode_good_flag) {
JonFreeman 7:6deaeace9a3e 586 return rpm2mph * (double) rpm;
JonFreeman 7:6deaeace9a3e 587 }
JonFreeman 7:6deaeace9a3e 588 return -1.0;
JonFreeman 7:6deaeace9a3e 589 }
JonFreeman 4:21d91465e4b1 590
JonFreeman 0:435bf84ce48a 591 int main()
JonFreeman 0:435bf84ce48a 592 {
JonFreeman 4:21d91465e4b1 593 int eighth_sec_count = 0;
JonFreeman 0:435bf84ce48a 594
JonFreeman 4:21d91465e4b1 595 MotA = 0; // Output all 0s to Motor drive ports A and B
JonFreeman 0:435bf84ce48a 596 MotB = 0;
JonFreeman 7:6deaeace9a3e 597 // MotPtr[0] = &MotorA; // Pointers to motor class objects
JonFreeman 7:6deaeace9a3e 598 // MotPtr[1] = &MotorB;
JonFreeman 5:ca86a7848d54 599
JonFreeman 6:f289a49c1eae 600 Temperature_pin.fall (&temp_sensor_isr);
JonFreeman 6:f289a49c1eae 601 Temperature_pin.mode (PullUp);
JonFreeman 4:21d91465e4b1 602
JonFreeman 4:21d91465e4b1 603 MAH1.rise (& MAH_isr); // Set up interrupt vectors
JonFreeman 3:ecb00e0e8d68 604 MAH1.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 605 MAH2.rise (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 606 MAH2.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 607 MAH3.rise (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 608 MAH3.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 609
JonFreeman 3:ecb00e0e8d68 610 MBH1.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 611 MBH1.fall (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 612 MBH2.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 613 MBH2.fall (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 614 MBH3.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 615 MBH3.fall (& MBH_isr);
JonFreeman 4:21d91465e4b1 616
JonFreeman 0:435bf84ce48a 617 MAH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 618 MAH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 619 MAH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 620 MBH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 621 MBH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 622 MBH3.mode (PullUp);
JonFreeman 4:21d91465e4b1 623 Servo1_i.mode (PullUp);
JonFreeman 4:21d91465e4b1 624 Servo2_i.mode (PullUp);
JonFreeman 4:21d91465e4b1 625
JonFreeman 0:435bf84ce48a 626 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 0:435bf84ce48a 627 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 628 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 6:f289a49c1eae 629 temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960);
JonFreeman 0:435bf84ce48a 630 // Done setting up system interrupt timers
JonFreeman 6:f289a49c1eae 631 temperature_timer.start ();
JonFreeman 0:435bf84ce48a 632
JonFreeman 6:f289a49c1eae 633 // const int TXTBUFSIZ = 36;
JonFreeman 6:f289a49c1eae 634 // char buff[TXTBUFSIZ];
JonFreeman 3:ecb00e0e8d68 635 pc.baud (9600);
JonFreeman 4:21d91465e4b1 636 com3.baud (1200);
JonFreeman 4:21d91465e4b1 637 com2.baud (19200);
JonFreeman 7:6deaeace9a3e 638 setup_comms ();
JonFreeman 6:f289a49c1eae 639
JonFreeman 7:6deaeace9a3e 640 IAm = '0';
JonFreeman 5:ca86a7848d54 641 if (check_24LC64() != 0xa0) { // searches for i2c devices, returns address of highest found
JonFreeman 0:435bf84ce48a 642 pc.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 5:ca86a7848d54 643 com2.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 7:6deaeace9a3e 644 eeprom_flag = false;
JonFreeman 0:435bf84ce48a 645 }
JonFreeman 5:ca86a7848d54 646 else { // Found 24LC64 memory on I2C
JonFreeman 7:6deaeace9a3e 647 eeprom_flag = true;
JonFreeman 5:ca86a7848d54 648 bool k;
JonFreeman 6:f289a49c1eae 649 k = rd_24LC64 (0, mode_bytes, 32);
JonFreeman 5:ca86a7848d54 650 if (!k)
JonFreeman 5:ca86a7848d54 651 com2.printf ("Error reading from eeprom\r\n");
JonFreeman 5:ca86a7848d54 652
JonFreeman 5:ca86a7848d54 653 int err = 0;
JonFreeman 6:f289a49c1eae 654 for (int i = 0; i < numof_eeprom_options; i++) {
JonFreeman 6:f289a49c1eae 655 if ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max)) {
JonFreeman 5:ca86a7848d54 656 com2.printf ("EEROM error with %s\r\n", option_list[i].t);
JonFreeman 5:ca86a7848d54 657 err++;
JonFreeman 5:ca86a7848d54 658 }
JonFreeman 5:ca86a7848d54 659 // else
JonFreeman 5:ca86a7848d54 660 // com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
JonFreeman 5:ca86a7848d54 661 }
JonFreeman 7:6deaeace9a3e 662 rpm2mph = 0.0;
JonFreeman 5:ca86a7848d54 663 if (err == 0) {
JonFreeman 7:6deaeace9a3e 664 mode_good_flag = true;
JonFreeman 6:f289a49c1eae 665 MotorA.direction_set (mode_bytes[MOTADIR]);
JonFreeman 6:f289a49c1eae 666 MotorB.direction_set (mode_bytes[MOTBDIR]);
JonFreeman 6:f289a49c1eae 667 IAm = mode_bytes[ID];
JonFreeman 7:6deaeace9a3e 668 rpm2mph = 60.0 // to Motor Revs per hour;
JonFreeman 7:6deaeace9a3e 669 * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR]) // Wheel revs per hour
JonFreeman 7:6deaeace9a3e 670 * PI * ((double)mode_bytes[WHEELDIA] / 1000.0) // metres per hour
JonFreeman 7:6deaeace9a3e 671 * 39.37 / (1760.0 * 36.0); // miles per hour
JonFreeman 5:ca86a7848d54 672 }
JonFreeman 5:ca86a7848d54 673 // Alternative ID 1 to 9
JonFreeman 5:ca86a7848d54 674 // com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
JonFreeman 5:ca86a7848d54 675 }
JonFreeman 6:f289a49c1eae 676 // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
JonFreeman 5:ca86a7848d54 677 T2 = 0; // T2, T3, T4 As yet unused pins
JonFreeman 0:435bf84ce48a 678 T3 = 0;
JonFreeman 0:435bf84ce48a 679 T4 = 0;
JonFreeman 5:ca86a7848d54 680 // T5 = 0; now input from fw/re on remote control box
JonFreeman 0:435bf84ce48a 681 T6 = 0;
JonFreeman 3:ecb00e0e8d68 682 // MotPtr[0]->set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 683 MotorA.set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 684 MotorB.set_mode (REGENBRAKE);
JonFreeman 5:ca86a7848d54 685 setVI (0.9, 0.5);
JonFreeman 5:ca86a7848d54 686
JonFreeman 4:21d91465e4b1 687 Servos[0] = Servos[1] = NULL;
JonFreeman 4:21d91465e4b1 688 // NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
JonFreeman 4:21d91465e4b1 689 // Only works with unconditional inline code
JonFreeman 4:21d91465e4b1 690 // However, setup code for Input by default,
JonFreeman 4:21d91465e4b1 691 // This can be used to monitor Servo output also !
JonFreeman 4:21d91465e4b1 692 Servo Servo1 (PB_8) ;
JonFreeman 4:21d91465e4b1 693 Servos[0] = & Servo1;
JonFreeman 4:21d91465e4b1 694 Servo Servo2 (PB_9) ;
JonFreeman 4:21d91465e4b1 695 Servos[1] = & Servo2;
JonFreeman 6:f289a49c1eae 696
JonFreeman 7:6deaeace9a3e 697 // pc.printf ("last_temp_count = %d\r\n", last_temp_count); // Has had time to do at least 1 conversion
JonFreeman 6:f289a49c1eae 698 if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C
JonFreeman 6:f289a49c1eae 699 temp_sensor_exists = true;
JonFreeman 4:21d91465e4b1 700 /*
JonFreeman 0:435bf84ce48a 701 // Setup Complete ! Can now start main control forever loop.
JonFreeman 3:ecb00e0e8d68 702 // March 16th 2018 thoughts !!!
JonFreeman 3:ecb00e0e8d68 703 // Control from one of several sources and types as selected in options bytes in eeprom.
JonFreeman 3:ecb00e0e8d68 704 // Control could be from e.g. Pot, Com2, Servos etc.
JonFreeman 3:ecb00e0e8d68 705 // Suggest e.g.
JonFreeman 4:21d91465e4b1 706 */ /*
JonFreeman 3:ecb00e0e8d68 707 switch (mode_byte) { // executed once only upon startup
JonFreeman 3:ecb00e0e8d68 708 case POT:
JonFreeman 3:ecb00e0e8d68 709 while (1) { // forever loop
JonFreeman 3:ecb00e0e8d68 710 call common_stuff ();
JonFreeman 3:ecb00e0e8d68 711 ...
JonFreeman 3:ecb00e0e8d68 712 }
JonFreeman 3:ecb00e0e8d68 713 break;
JonFreeman 3:ecb00e0e8d68 714 case COM2:
JonFreeman 3:ecb00e0e8d68 715 while (1) { // forever loop
JonFreeman 3:ecb00e0e8d68 716 call common_stuff ();
JonFreeman 3:ecb00e0e8d68 717 ...
JonFreeman 3:ecb00e0e8d68 718 }
JonFreeman 3:ecb00e0e8d68 719 break;
JonFreeman 3:ecb00e0e8d68 720 }
JonFreeman 3:ecb00e0e8d68 721 */
JonFreeman 7:6deaeace9a3e 722 // pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
JonFreeman 7:6deaeace9a3e 723 pc.printf ("About to start!\r\n");
JonFreeman 0:435bf84ce48a 724 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 725 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 7:6deaeace9a3e 726 command_line_interpreter_pc () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 7:6deaeace9a3e 727 command_line_interpreter_loco () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 728 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 0:435bf84ce48a 729 }
JonFreeman 0:435bf84ce48a 730 loop_flag = false; // Clear flag set by ticker interrupt handler
JonFreeman 5:ca86a7848d54 731 MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 5:ca86a7848d54 732 MotorB.pulses_per_sec (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
JonFreeman 3:ecb00e0e8d68 733 T4 = !T4; // toggle to hang scope on to verify loop execution
JonFreeman 0:435bf84ce48a 734 // do stuff
JonFreeman 0:435bf84ce48a 735 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 736 flag_8Hz = false;
JonFreeman 3:ecb00e0e8d68 737 LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 5:ca86a7848d54 738 WatchDog--;
JonFreeman 5:ca86a7848d54 739 if (WatchDog == 0) { // Deal with WatchDog timer timeout here
JonFreeman 5:ca86a7848d54 740 setVI (0.0, 0.0); // set motor volts and amps to zero
JonFreeman 5:ca86a7848d54 741 com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
JonFreeman 5:ca86a7848d54 742 } // End of dealing with WatchDog timer timeout
JonFreeman 5:ca86a7848d54 743 if (WatchDog < 0)
JonFreeman 5:ca86a7848d54 744 WatchDog = 0;
JonFreeman 4:21d91465e4b1 745 eighth_sec_count++;
JonFreeman 4:21d91465e4b1 746 if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 4:21d91465e4b1 747 eighth_sec_count = 0;
JonFreeman 6:f289a49c1eae 748 MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
JonFreeman 6:f289a49c1eae 749 MotorB.current_calc ();
JonFreeman 7:6deaeace9a3e 750 /* if (temp_sensor_exists) {
JonFreeman 6:f289a49c1eae 751 double tmprt = (double) last_temp_count;
JonFreeman 6:f289a49c1eae 752 tmprt /= 16.0;
JonFreeman 6:f289a49c1eae 753 tmprt -= 50.0;
JonFreeman 6:f289a49c1eae 754 pc.printf ("Temp %.2f\r\n", tmprt);
JonFreeman 7:6deaeace9a3e 755 }*/
JonFreeman 5:ca86a7848d54 756 // com2.printf ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IAmin %d, IAave %d, IAmax %d, IB %d, Arpm %d, Brpm %d\r\n", Read_BatteryVolts(), Read_DriverPot(), MotorA.read_Halls (), MotorB.read_Halls (), MotorA.I.min, MotorA.I.ave, MotorA.I.max, MotorB.I.ave, (Apps * 60) / 24, (Bpps * 60) / 24);
JonFreeman 0:435bf84ce48a 757 }
JonFreeman 0:435bf84ce48a 758 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 759 } // End of main programme loop
JonFreeman 0:435bf84ce48a 760 }