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@8:93203f473f6e, 2018-08-18 (annotated)

Committer:
JonFreeman
Date:
Sat Aug 18 12:51:35 2018 +0000
Revision:
8:93203f473f6e
Parent:
7:6deaeace9a3e
Child:
9:ac2412df01be
Work underway to drive brushed motors.; Code as supplied to Rob

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:435bf84ce48a 1 #include "mbed.h"
JonFreeman 8:93203f473f6e 2 //#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
JonFreeman 8:93203f473f6e 3 #include "stm32f401xe.h"
JonFreeman 0:435bf84ce48a 4 #include "DualBLS.h"
JonFreeman 0:435bf84ce48a 5 #include "BufferedSerial.h"
JonFreeman 0:435bf84ce48a 6 #include "FastPWM.h"
JonFreeman 4:21d91465e4b1 7 #include "Servo.h"
JonFreeman 5:ca86a7848d54 8
JonFreeman 5:ca86a7848d54 9 /*
JonFreeman 5:ca86a7848d54 10 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 11 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 12 */
JonFreeman 5:ca86a7848d54 13
JonFreeman 5:ca86a7848d54 14
JonFreeman 0:435bf84ce48a 15 /* STM32F401RE - compile using NUCLEO-F401RE
JonFreeman 6:f289a49c1eae 16 // PROJECT - Dual Brushless Motor Controller - Jon Freeman June 2018.
JonFreeman 0:435bf84ce48a 17
JonFreeman 0:435bf84ce48a 18 AnalogIn to read each motor current
JonFreeman 0:435bf84ce48a 19
JonFreeman 0:435bf84ce48a 20 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 21 CONTROL PHILOSOPHY
JonFreeman 0:435bf84ce48a 22 This Bogie drive board software should ensure sensible control when commands supplied are not sensible !
JonFreeman 0:435bf84ce48a 23
JonFreeman 0:435bf84ce48a 24 That is, smooth transition between Drive, Coast and Brake to be implemented here.
JonFreeman 0:435bf84ce48a 25 The remote controller must not be relied upon to deliver sensible command sequences.
JonFreeman 0:435bf84ce48a 26
JonFreeman 0:435bf84ce48a 27 So much the better if the remote controller does issue sensible commands, but ...
JonFreeman 0:435bf84ce48a 28
JonFreeman 0:435bf84ce48a 29 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 30
JonFreeman 0:435bf84ce48a 31
JonFreeman 0:435bf84ce48a 32 */
JonFreeman 8:93203f473f6e 33
JonFreeman 7:6deaeace9a3e 34 #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP
JonFreeman 7:6deaeace9a3e 35 #include "F401RE.h"
JonFreeman 6:f289a49c1eae 36 #endif
JonFreeman 7:6deaeace9a3e 37 #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP
JonFreeman 7:6deaeace9a3e 38 #include "F446ZE.h"
JonFreeman 6:f289a49c1eae 39 #endif
JonFreeman 0:435bf84ce48a 40 /* Global variable declarations */
JonFreeman 3:ecb00e0e8d68 41 volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
JonFreeman 5:ca86a7848d54 42 int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
JonFreeman 8:93203f473f6e 43 bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable
JonFreeman 0:435bf84ce48a 44 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 45 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 46 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 5:ca86a7848d54 47 AtoD_Semaphore = 0;
JonFreeman 5:ca86a7848d54 48 int IAm;
JonFreeman 0:435bf84ce48a 49 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 0:435bf84ce48a 50 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 6:f289a49c1eae 51 bool temp_sensor_exists = false;
JonFreeman 7:6deaeace9a3e 52 bool eeprom_flag; // gets set according to 24LC674 being found or not
JonFreeman 7:6deaeace9a3e 53 bool mode_good_flag = false;
JonFreeman 6:f289a49c1eae 54 char mode_bytes[36];
JonFreeman 3:ecb00e0e8d68 55
JonFreeman 6:f289a49c1eae 56 uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
JonFreeman 6:f289a49c1eae 57 last_temp_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
JonFreeman 8:93203f473f6e 58 // struct single_bogie_options bogie;
JonFreeman 8:93203f473f6e 59 double rpm2mph = 0.0; // gets calculated from eeprom mode entries if present
JonFreeman 0:435bf84ce48a 60 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 61
JonFreeman 0:435bf84ce48a 62 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 63 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 6:f289a49c1eae 64 Ticker temperature_find_ticker;
JonFreeman 6:f289a49c1eae 65 Timer temperature_timer;
JonFreeman 8:93203f473f6e 66 Timer dc_motor_kicker_timer;
JonFreeman 8:93203f473f6e 67 Timeout motors_restore;
JonFreeman 0:435bf84ce48a 68
JonFreeman 0:435bf84ce48a 69 // Interrupt Service Routines
JonFreeman 8:93203f473f6e 70 void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec
JonFreeman 8:93203f473f6e 71 {
JonFreeman 6:f289a49c1eae 72 static bool readflag = false;
JonFreeman 6:f289a49c1eae 73 int t = temperature_timer.read_ms ();
JonFreeman 6:f289a49c1eae 74 if ((t == 5) && (!readflag)) {
JonFreeman 6:f289a49c1eae 75 last_temp_count = temp_sensor_count;
JonFreeman 6:f289a49c1eae 76 temp_sensor_count = 0;
JonFreeman 6:f289a49c1eae 77 readflag = true;
JonFreeman 6:f289a49c1eae 78 }
JonFreeman 6:f289a49c1eae 79 if (t == 6)
JonFreeman 6:f289a49c1eae 80 readflag = false;
JonFreeman 6:f289a49c1eae 81 }
JonFreeman 0:435bf84ce48a 82
JonFreeman 0:435bf84ce48a 83 /** void ISR_loop_timer ()
JonFreeman 0:435bf84ce48a 84 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 0:435bf84ce48a 85 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 0:435bf84ce48a 86 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 0:435bf84ce48a 87 */
JonFreeman 0:435bf84ce48a 88 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 89 {
JonFreeman 0:435bf84ce48a 90 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 0:435bf84ce48a 91 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 92 if ((sys_timer & 0x03) == 0)
JonFreeman 0:435bf84ce48a 93 flag_8Hz = true;
JonFreeman 0:435bf84ce48a 94 }
JonFreeman 0:435bf84ce48a 95
JonFreeman 0:435bf84ce48a 96 /** void ISR_voltage_reader ()
JonFreeman 0:435bf84ce48a 97 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 0:435bf84ce48a 98 *
JonFreeman 0:435bf84ce48a 99 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 0:435bf84ce48a 100 */
JonFreeman 0:435bf84ce48a 101
JonFreeman 5:ca86a7848d54 102 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
JonFreeman 0:435bf84ce48a 103 {
JonFreeman 0:435bf84ce48a 104 AtoD_Semaphore++;
JonFreeman 2:04761b196473 105 fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 106 }
JonFreeman 0:435bf84ce48a 107
JonFreeman 0:435bf84ce48a 108
JonFreeman 4:21d91465e4b1 109 class RControl_In
JonFreeman 8:93203f473f6e 110 {
JonFreeman 8:93203f473f6e 111 // Read servo style pwm input
JonFreeman 0:435bf84ce48a 112 Timer t;
JonFreeman 0:435bf84ce48a 113 int32_t clock_old, clock_new;
JonFreeman 0:435bf84ce48a 114 int32_t pulse_width_us, period_us;
JonFreeman 4:21d91465e4b1 115 public:
JonFreeman 4:21d91465e4b1 116 RControl_In () {
JonFreeman 0:435bf84ce48a 117 pulse_width_us = period_us = 0;
JonFreeman 4:21d91465e4b1 118 com2.printf ("Setting up Radio_Congtrol_In\r\n");
JonFreeman 4:21d91465e4b1 119 } ;
JonFreeman 0:435bf84ce48a 120 bool validate_rx () ;
JonFreeman 0:435bf84ce48a 121 void rise () ;
JonFreeman 0:435bf84ce48a 122 void fall () ;
JonFreeman 4:21d91465e4b1 123 uint32_t pulsewidth () ;
JonFreeman 4:21d91465e4b1 124 uint32_t period () ;
JonFreeman 4:21d91465e4b1 125 bool rx_active;
JonFreeman 4:21d91465e4b1 126 } ;
JonFreeman 0:435bf84ce48a 127
JonFreeman 8:93203f473f6e 128 uint32_t RControl_In::pulsewidth ()
JonFreeman 8:93203f473f6e 129 {
JonFreeman 4:21d91465e4b1 130 return pulse_width_us;
JonFreeman 4:21d91465e4b1 131 }
JonFreeman 4:21d91465e4b1 132
JonFreeman 8:93203f473f6e 133 uint32_t RControl_In::period ()
JonFreeman 8:93203f473f6e 134 {
JonFreeman 4:21d91465e4b1 135 return period_us;
JonFreeman 4:21d91465e4b1 136 }
JonFreeman 4:21d91465e4b1 137
JonFreeman 4:21d91465e4b1 138 bool RControl_In::validate_rx ()
JonFreeman 0:435bf84ce48a 139 {
JonFreeman 0:435bf84ce48a 140 if ((clock() - clock_new) > 4)
JonFreeman 0:435bf84ce48a 141 rx_active = false;
JonFreeman 0:435bf84ce48a 142 else
JonFreeman 0:435bf84ce48a 143 rx_active = true;
JonFreeman 0:435bf84ce48a 144 return rx_active;
JonFreeman 0:435bf84ce48a 145 }
JonFreeman 0:435bf84ce48a 146
JonFreeman 6:f289a49c1eae 147 void RControl_In::rise () // These may not work as use of PortB as port may bugger InterruptIn use
JonFreeman 0:435bf84ce48a 148 {
JonFreeman 0:435bf84ce48a 149 t.stop ();
JonFreeman 0:435bf84ce48a 150 period_us = t.read_us ();
JonFreeman 0:435bf84ce48a 151 t.reset ();
JonFreeman 0:435bf84ce48a 152 t.start ();
JonFreeman 0:435bf84ce48a 153 }
JonFreeman 4:21d91465e4b1 154 void RControl_In::fall ()
JonFreeman 0:435bf84ce48a 155 {
JonFreeman 0:435bf84ce48a 156 pulse_width_us = t.read_us ();
JonFreeman 0:435bf84ce48a 157 clock_old = clock_new;
JonFreeman 0:435bf84ce48a 158 clock_new = clock();
JonFreeman 0:435bf84ce48a 159 if ((clock_new - clock_old) < 4)
JonFreeman 0:435bf84ce48a 160 rx_active = true;
JonFreeman 0:435bf84ce48a 161 }
JonFreeman 4:21d91465e4b1 162
JonFreeman 0:435bf84ce48a 163
JonFreeman 4:21d91465e4b1 164 Servo * Servos[2];
JonFreeman 4:21d91465e4b1 165
JonFreeman 0:435bf84ce48a 166 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 167 /*
JonFreeman 0:435bf84ce48a 168 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 169
JonFreeman 3:ecb00e0e8d68 170 1 1 1 1 0 0 0 ---___---___ Hall1
JonFreeman 3:ecb00e0e8d68 171 2 0 0 1 1 1 0 __---___---_ Hall2
JonFreeman 3:ecb00e0e8d68 172 4 1 0 0 0 1 1 -___---___-- Hall3
JonFreeman 0:435bf84ce48a 173
JonFreeman 0:435bf84ce48a 174 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 8:93203f473f6e 175
JonFreeman 8:93203f473f6e 176 8th July 2018
JonFreeman 8:93203f473f6e 177 Added drive to DC brushed motors.
JonFreeman 8:93203f473f6e 178 Connect U and W to dc motor, leave V open.
JonFreeman 8:93203f473f6e 179
JonFreeman 8:93203f473f6e 180 Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors
JonFreeman 8:93203f473f6e 181
JonFreeman 0:435bf84ce48a 182 */
JonFreeman 3:ecb00e0e8d68 183 const uint16_t A_tabl[] = { // Origial table
JonFreeman 0:435bf84ce48a 184 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 8:93203f473f6e 185 0, AWV,AVU,AWU,AUW,AUV,AVW,AUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
JonFreeman 8:93203f473f6e 186 0, AVW,AUV,AUW,AWU,AVU,AWV,AWU, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
JonFreeman 8:93203f473f6e 187 0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking
JonFreeman 4:21d91465e4b1 188 KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
JonFreeman 3:ecb00e0e8d68 189 } ;
JonFreeman 4:21d91465e4b1 190 InterruptIn * AHarr[] = {
JonFreeman 4:21d91465e4b1 191 &MAH1,
JonFreeman 4:21d91465e4b1 192 &MAH2,
JonFreeman 4:21d91465e4b1 193 &MAH3
JonFreeman 3:ecb00e0e8d68 194 } ;
JonFreeman 0:435bf84ce48a 195 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 196 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 8:93203f473f6e 197 0, BWV,BVU,BWU,BUW,BUV,BVW,BUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
JonFreeman 8:93203f473f6e 198 0, BVW,BUV,BUW,BWU,BVU,BWV,BWU, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
JonFreeman 8:93203f473f6e 199 0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking
JonFreeman 4:21d91465e4b1 200 KEEP_L_MASK_B, KEEP_H_MASK_B
JonFreeman 3:ecb00e0e8d68 201 } ;
JonFreeman 4:21d91465e4b1 202 InterruptIn * BHarr[] = {
JonFreeman 4:21d91465e4b1 203 &MBH1,
JonFreeman 4:21d91465e4b1 204 &MBH2,
JonFreeman 4:21d91465e4b1 205 &MBH3
JonFreeman 0:435bf84ce48a 206 } ;
JonFreeman 0:435bf84ce48a 207
JonFreeman 0:435bf84ce48a 208 class motor
JonFreeman 0:435bf84ce48a 209 {
JonFreeman 5:ca86a7848d54 210 uint32_t Hall_total, visible_mode, inner_mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
JonFreeman 3:ecb00e0e8d68 211 uint32_t latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp;
JonFreeman 3:ecb00e0e8d68 212 bool moving_flag;
JonFreeman 0:435bf84ce48a 213 const uint16_t * lut;
JonFreeman 0:435bf84ce48a 214 FastPWM * maxV, * maxI;
JonFreeman 0:435bf84ce48a 215 PortOut * Motor_Port;
JonFreeman 2:04761b196473 216 InterruptIn * Hall1, * Hall2, * Hall3;
JonFreeman 0:435bf84ce48a 217 public:
JonFreeman 8:93203f473f6e 218 bool dc_motor;
JonFreeman 2:04761b196473 219 struct currents {
JonFreeman 2:04761b196473 220 uint32_t max, min, ave;
JonFreeman 2:04761b196473 221 } I;
JonFreeman 3:ecb00e0e8d68 222 int32_t angle_cnt;
JonFreeman 0:435bf84ce48a 223 uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
JonFreeman 3:ecb00e0e8d68 224 uint32_t Hindex[2], tickleon, encoder_error_cnt;
JonFreeman 5:ca86a7848d54 225 uint32_t RPM, PPS;
JonFreeman 5:ca86a7848d54 226 double last_V, last_I;
JonFreeman 0:435bf84ce48a 227 motor () {} ; // Default constructor
JonFreeman 4:21d91465e4b1 228 motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
JonFreeman 0:435bf84ce48a 229 void set_V_limit (double) ; // Sets max motor voltage
JonFreeman 0:435bf84ce48a 230 void set_I_limit (double) ; // Sets max motor current
JonFreeman 0:435bf84ce48a 231 void Hall_change () ; // Called in response to edge on Hall input pin
JonFreeman 3:ecb00e0e8d68 232 void motor_set () ; // Energise Port with data determined by Hall sensors
JonFreeman 3:ecb00e0e8d68 233 void direction_set (int) ; // sets 'direction' with bit pattern to eor with FORWARD or REVERSE in set_mode
JonFreeman 3:ecb00e0e8d68 234 bool set_mode (int); // sets mode to HANDBRAKE, FORWARD, REVERSE or REGENBRAKE
JonFreeman 3:ecb00e0e8d68 235 bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs
JonFreeman 3:ecb00e0e8d68 236 void current_calc () ; // Updates 3 uint32_t I.min, I.ave, I.max
JonFreeman 3:ecb00e0e8d68 237 uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
JonFreeman 3:ecb00e0e8d68 238 int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
JonFreeman 8:93203f473f6e 239 bool motor_is_brushless ();
JonFreeman 3:ecb00e0e8d68 240 void high_side_off () ;
JonFreeman 8:93203f473f6e 241 void low_side_on () ;
JonFreeman 8:93203f473f6e 242 void raw_V_pwm (int);
JonFreeman 3:ecb00e0e8d68 243 } ; //MotorA, MotorB, or even Motor[2];
JonFreeman 0:435bf84ce48a 244
JonFreeman 4:21d91465e4b1 245 motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
JonFreeman 4:21d91465e4b1 246 motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr);
JonFreeman 0:435bf84ce48a 247
JonFreeman 7:6deaeace9a3e 248 //motor * MotPtr[8]; // Array of pointers to some number of motor objects
JonFreeman 3:ecb00e0e8d68 249
JonFreeman 4:21d91465e4b1 250 motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
JonFreeman 8:93203f473f6e 251 {
JonFreeman 8:93203f473f6e 252 // Constructor
JonFreeman 0:435bf84ce48a 253 maxV = _maxV_;
JonFreeman 0:435bf84ce48a 254 maxI = _maxI_;
JonFreeman 3:ecb00e0e8d68 255 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 256 latest_pulses_per_sec = 0;
JonFreeman 0:435bf84ce48a 257 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 0:435bf84ce48a 258 edge_count_table[i] = 0;
JonFreeman 0:435bf84ce48a 259 if (lutptr != A_tabl && lutptr != B_tabl)
JonFreeman 0:435bf84ce48a 260 pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
JonFreeman 3:ecb00e0e8d68 261 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 262 Motor_Port = P;
JonFreeman 0:435bf84ce48a 263 pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
JonFreeman 0:435bf84ce48a 264 maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:435bf84ce48a 265 maxI->period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:435bf84ce48a 266 maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 0:435bf84ce48a 267 maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 5:ca86a7848d54 268 visible_mode = REGENBRAKE;
JonFreeman 5:ca86a7848d54 269 inner_mode = REGENBRAKE;
JonFreeman 0:435bf84ce48a 270 lut = lutptr;
JonFreeman 3:ecb00e0e8d68 271 Hindex[0] = Hindex[1] = read_Halls ();
JonFreeman 3:ecb00e0e8d68 272 ppstmp = 0;
JonFreeman 3:ecb00e0e8d68 273 tickleon = 0;
JonFreeman 3:ecb00e0e8d68 274 direction = 0;
JonFreeman 3:ecb00e0e8d68 275 angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel
JonFreeman 3:ecb00e0e8d68 276 encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction
JonFreeman 4:21d91465e4b1 277 Hall1 = Hall[0];
JonFreeman 4:21d91465e4b1 278 Hall2 = Hall[1];
JonFreeman 4:21d91465e4b1 279 Hall3 = Hall[2];
JonFreeman 5:ca86a7848d54 280 PPS = 0;
JonFreeman 5:ca86a7848d54 281 RPM = 0;
JonFreeman 5:ca86a7848d54 282 last_V = last_I = 0.0;
JonFreeman 8:93203f473f6e 283 int x = read_Halls ();
JonFreeman 8:93203f473f6e 284 if (x == 7)
JonFreeman 8:93203f473f6e 285 dc_motor = true;
JonFreeman 8:93203f473f6e 286 else
JonFreeman 8:93203f473f6e 287 dc_motor = false;
JonFreeman 8:93203f473f6e 288 }
JonFreeman 8:93203f473f6e 289
JonFreeman 8:93203f473f6e 290 bool motor::motor_is_brushless ()
JonFreeman 8:93203f473f6e 291 {
JonFreeman 8:93203f473f6e 292 /* int x = read_Halls ();
JonFreeman 8:93203f473f6e 293 if (x < 1 || x > 6)
JonFreeman 8:93203f473f6e 294 return false;
JonFreeman 8:93203f473f6e 295 return true;
JonFreeman 8:93203f473f6e 296 */
JonFreeman 8:93203f473f6e 297 return !dc_motor;
JonFreeman 3:ecb00e0e8d68 298 }
JonFreeman 3:ecb00e0e8d68 299
JonFreeman 5:ca86a7848d54 300 /**
JonFreeman 5:ca86a7848d54 301 void motor::direction_set (int dir) {
JonFreeman 5:ca86a7848d54 302 Used to set direction according to mode data from eeprom
JonFreeman 5:ca86a7848d54 303 */
JonFreeman 8:93203f473f6e 304 void motor::direction_set (int dir)
JonFreeman 8:93203f473f6e 305 {
JonFreeman 3:ecb00e0e8d68 306 if (dir != 0)
JonFreeman 3:ecb00e0e8d68 307 dir = FORWARD | REVERSE; // bits used in eor
JonFreeman 3:ecb00e0e8d68 308 direction = dir;
JonFreeman 0:435bf84ce48a 309 }
JonFreeman 0:435bf84ce48a 310
JonFreeman 8:93203f473f6e 311 int motor::read_Halls ()
JonFreeman 8:93203f473f6e 312 {
JonFreeman 2:04761b196473 313 int x = 0;
JonFreeman 2:04761b196473 314 if (*Hall1 != 0) x |= 1;
JonFreeman 2:04761b196473 315 if (*Hall2 != 0) x |= 2;
JonFreeman 2:04761b196473 316 if (*Hall3 != 0) x |= 4;
JonFreeman 2:04761b196473 317 return x;
JonFreeman 2:04761b196473 318 }
JonFreeman 2:04761b196473 319
JonFreeman 8:93203f473f6e 320 void motor::high_side_off ()
JonFreeman 8:93203f473f6e 321 {
JonFreeman 3:ecb00e0e8d68 322 uint16_t p = *Motor_Port;
JonFreeman 3:ecb00e0e8d68 323 p &= lut[32]; // KEEP_L_MASK_A or B
JonFreeman 3:ecb00e0e8d68 324 *Motor_Port = p;
JonFreeman 1:0fabe6fdb55b 325 }
JonFreeman 1:0fabe6fdb55b 326
JonFreeman 8:93203f473f6e 327 void motor::low_side_on ()
JonFreeman 8:93203f473f6e 328 {
JonFreeman 8:93203f473f6e 329 // uint16_t p = *Motor_Port;
JonFreeman 8:93203f473f6e 330 // p &= lut[31]; // KEEP_L_MASK_A or B
JonFreeman 8:93203f473f6e 331 *Motor_Port = lut[31];
JonFreeman 8:93203f473f6e 332 }
JonFreeman 8:93203f473f6e 333
JonFreeman 0:435bf84ce48a 334 void motor::current_calc ()
JonFreeman 0:435bf84ce48a 335 {
JonFreeman 0:435bf84ce48a 336 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 0:435bf84ce48a 337 I.max = 0;
JonFreeman 0:435bf84ce48a 338 I.ave = 0;
JonFreeman 0:435bf84ce48a 339 uint16_t sample;
JonFreeman 0:435bf84ce48a 340 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 0:435bf84ce48a 341 sample = current_samples[i];
JonFreeman 0:435bf84ce48a 342 I.ave += sample;
JonFreeman 0:435bf84ce48a 343 if (I.min > sample)
JonFreeman 0:435bf84ce48a 344 I.min = sample;
JonFreeman 0:435bf84ce48a 345 if (I.max < sample)
JonFreeman 0:435bf84ce48a 346 I.max = sample;
JonFreeman 0:435bf84ce48a 347 }
JonFreeman 0:435bf84ce48a 348 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 0:435bf84ce48a 349 }
JonFreeman 0:435bf84ce48a 350
JonFreeman 8:93203f473f6e 351
JonFreeman 8:93203f473f6e 352 void motor::raw_V_pwm (int v)
JonFreeman 8:93203f473f6e 353 {
JonFreeman 8:93203f473f6e 354 if (v < 1) v = 1;
JonFreeman 8:93203f473f6e 355 if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
JonFreeman 8:93203f473f6e 356 maxV->pulsewidth_ticks (v);
JonFreeman 8:93203f473f6e 357 }
JonFreeman 8:93203f473f6e 358
JonFreeman 8:93203f473f6e 359 void motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
JonFreeman 0:435bf84ce48a 360 {
JonFreeman 0:435bf84ce48a 361 if (p < 0.0)
JonFreeman 0:435bf84ce48a 362 p = 0.0;
JonFreeman 0:435bf84ce48a 363 if (p > 1.0)
JonFreeman 0:435bf84ce48a 364 p = 1.0;
JonFreeman 5:ca86a7848d54 365 last_V = p; // for read by diagnostics
JonFreeman 0:435bf84ce48a 366 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:435bf84ce48a 367 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:435bf84ce48a 368 maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 0:435bf84ce48a 369 }
JonFreeman 0:435bf84ce48a 370
JonFreeman 0:435bf84ce48a 371 void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 0:435bf84ce48a 372 {
JonFreeman 0:435bf84ce48a 373 int a;
JonFreeman 0:435bf84ce48a 374 if (p < 0.0)
JonFreeman 0:435bf84ce48a 375 p = 0.0;
JonFreeman 0:435bf84ce48a 376 if (p > 1.0)
JonFreeman 0:435bf84ce48a 377 p = 1.0;
JonFreeman 5:ca86a7848d54 378 last_I = p;
JonFreeman 0:435bf84ce48a 379 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:435bf84ce48a 380 if (a > MAX_PWM_TICKS)
JonFreeman 0:435bf84ce48a 381 a = MAX_PWM_TICKS;
JonFreeman 0:435bf84ce48a 382 if (a < 0)
JonFreeman 0:435bf84ce48a 383 a = 0;
JonFreeman 0:435bf84ce48a 384 maxI->pulsewidth_ticks (a); // PWM
JonFreeman 0:435bf84ce48a 385 }
JonFreeman 0:435bf84ce48a 386
JonFreeman 3:ecb00e0e8d68 387 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 8:93203f473f6e 388 {
JonFreeman 8:93203f473f6e 389 // Can also test for motor running or not here
JonFreeman 8:93203f473f6e 390 if (dc_motor)
JonFreeman 8:93203f473f6e 391 return 0;
JonFreeman 3:ecb00e0e8d68 392 if (ppstmp == Hall_total) {
JonFreeman 8:93203f473f6e 393 // if (dc_motor || ppstmp == Hall_total) {
JonFreeman 3:ecb00e0e8d68 394 moving_flag = false; // Zero Hall transitions since previous call - motor not moving
JonFreeman 4:21d91465e4b1 395 tickleon = TICKLE_TIMES;
JonFreeman 8:93203f473f6e 396 } else {
JonFreeman 3:ecb00e0e8d68 397 moving_flag = true;
JonFreeman 3:ecb00e0e8d68 398 ppstmp = Hall_total;
JonFreeman 3:ecb00e0e8d68 399 }
JonFreeman 3:ecb00e0e8d68 400 latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
JonFreeman 3:ecb00e0e8d68 401 edge_count_table[Hall_tab_ptr] = ppstmp;
JonFreeman 0:435bf84ce48a 402 Hall_tab_ptr++;
JonFreeman 0:435bf84ce48a 403 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 0:435bf84ce48a 404 Hall_tab_ptr = 0;
JonFreeman 5:ca86a7848d54 405 PPS = latest_pulses_per_sec;
JonFreeman 5:ca86a7848d54 406 RPM = (latest_pulses_per_sec * 60) / 24;
JonFreeman 0:435bf84ce48a 407 return latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 408 }
JonFreeman 0:435bf84ce48a 409
JonFreeman 3:ecb00e0e8d68 410 bool motor::is_moving ()
JonFreeman 3:ecb00e0e8d68 411 {
JonFreeman 3:ecb00e0e8d68 412 return moving_flag;
JonFreeman 3:ecb00e0e8d68 413 }
JonFreeman 3:ecb00e0e8d68 414
JonFreeman 5:ca86a7848d54 415 /**
JonFreeman 5:ca86a7848d54 416 bool motor::set_mode (int m)
JonFreeman 5:ca86a7848d54 417 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
JonFreeman 5:ca86a7848d54 418 If this causes change of mode, also sets V and I to zero.
JonFreeman 5:ca86a7848d54 419 */
JonFreeman 0:435bf84ce48a 420 bool motor::set_mode (int m)
JonFreeman 0:435bf84ce48a 421 {
JonFreeman 2:04761b196473 422 if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
JonFreeman 2:04761b196473 423 pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 0:435bf84ce48a 424 return false;
JonFreeman 2:04761b196473 425 }
JonFreeman 5:ca86a7848d54 426 if (visible_mode != m) { // Mode change, kill volts and amps to be safe
JonFreeman 5:ca86a7848d54 427 set_V_limit (0.0);
JonFreeman 5:ca86a7848d54 428 set_I_limit (0.0);
JonFreeman 5:ca86a7848d54 429 visible_mode = m;
JonFreeman 5:ca86a7848d54 430 }
JonFreeman 3:ecb00e0e8d68 431 if (m == FORWARD || m == REVERSE)
JonFreeman 3:ecb00e0e8d68 432 m ^= direction;
JonFreeman 5:ca86a7848d54 433 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 434 return true;
JonFreeman 0:435bf84ce48a 435 }
JonFreeman 0:435bf84ce48a 436
JonFreeman 0:435bf84ce48a 437 void motor::Hall_change ()
JonFreeman 0:435bf84ce48a 438 {
JonFreeman 8:93203f473f6e 439 const int32_t delta_theta_lut[] = { // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
JonFreeman 8:93203f473f6e 440 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
JonFreeman 8:93203f473f6e 441 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
JonFreeman 8:93203f473f6e 442 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
JonFreeman 8:93203f473f6e 443 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
JonFreeman 8:93203f473f6e 444 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
JonFreeman 8:93203f473f6e 445 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
JonFreeman 8:93203f473f6e 446 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
JonFreeman 8:93203f473f6e 447 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
JonFreeman 8:93203f473f6e 448 } ;
JonFreeman 3:ecb00e0e8d68 449 int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
JonFreeman 3:ecb00e0e8d68 450 if (delta_theta == 0)
JonFreeman 3:ecb00e0e8d68 451 encoder_error_cnt++;
JonFreeman 3:ecb00e0e8d68 452 else
JonFreeman 3:ecb00e0e8d68 453 angle_cnt += delta_theta;
JonFreeman 5:ca86a7848d54 454 *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18
JonFreeman 0:435bf84ce48a 455 Hall_total++;
JonFreeman 3:ecb00e0e8d68 456 Hindex[1] = Hindex[0];
JonFreeman 0:435bf84ce48a 457 }
JonFreeman 2:04761b196473 458
JonFreeman 5:ca86a7848d54 459
JonFreeman 8:93203f473f6e 460 void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
JonFreeman 8:93203f473f6e 461 {
JonFreeman 8:93203f473f6e 462 pc.printf ("Mot A is %s, Mot B is %s\r\n", MotorA.motor_is_brushless() ? "Brushless":"DC", MotorB.motor_is_brushless() ? "Brushless":"DC");
JonFreeman 8:93203f473f6e 463 }
JonFreeman 8:93203f473f6e 464
JonFreeman 8:93203f473f6e 465 void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some
JonFreeman 8:93203f473f6e 466 {
JonFreeman 6:f289a49c1eae 467 int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
JonFreeman 6:f289a49c1eae 468 temperature_timer.reset ();
JonFreeman 5:ca86a7848d54 469 temp_sensor_count++;
JonFreeman 6:f289a49c1eae 470 if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
JonFreeman 6:f289a49c1eae 471 temp_sensor_count++;
JonFreeman 6:f289a49c1eae 472 // T2 = !T2; // scope hanger
JonFreeman 5:ca86a7848d54 473 }
JonFreeman 5:ca86a7848d54 474
JonFreeman 8:93203f473f6e 475
JonFreeman 3:ecb00e0e8d68 476 void MAH_isr ()
JonFreeman 0:435bf84ce48a 477 {
JonFreeman 3:ecb00e0e8d68 478 uint32_t x = 0;
JonFreeman 3:ecb00e0e8d68 479 MotorA.high_side_off ();
JonFreeman 8:93203f473f6e 480 // T3 = !T3;
JonFreeman 3:ecb00e0e8d68 481 if (MAH1 != 0) x |= 1;
JonFreeman 3:ecb00e0e8d68 482 if (MAH2 != 0) x |= 2;
JonFreeman 3:ecb00e0e8d68 483 if (MAH3 != 0) x |= 4;
JonFreeman 3:ecb00e0e8d68 484 MotorA.Hindex[0] = x; // New in [0], old in [1]
JonFreeman 0:435bf84ce48a 485 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 486 }
JonFreeman 0:435bf84ce48a 487
JonFreeman 3:ecb00e0e8d68 488 void MBH_isr ()
JonFreeman 0:435bf84ce48a 489 {
JonFreeman 3:ecb00e0e8d68 490 uint32_t x = 0;
JonFreeman 3:ecb00e0e8d68 491 MotorB.high_side_off ();
JonFreeman 3:ecb00e0e8d68 492 if (MBH1 != 0) x |= 1;
JonFreeman 3:ecb00e0e8d68 493 if (MBH2 != 0) x |= 2;
JonFreeman 3:ecb00e0e8d68 494 if (MBH3 != 0) x |= 4;
JonFreeman 3:ecb00e0e8d68 495 MotorB.Hindex[0] = x;
JonFreeman 0:435bf84ce48a 496 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 497 }
JonFreeman 0:435bf84ce48a 498
JonFreeman 0:435bf84ce48a 499
JonFreeman 0:435bf84ce48a 500 // End of Interrupt Service Routines
JonFreeman 0:435bf84ce48a 501
JonFreeman 3:ecb00e0e8d68 502 void motor::motor_set ()
JonFreeman 3:ecb00e0e8d68 503 {
JonFreeman 3:ecb00e0e8d68 504 Hindex[0] = read_Halls ();
JonFreeman 5:ca86a7848d54 505 *Motor_Port = lut[inner_mode | Hindex[0]];
JonFreeman 3:ecb00e0e8d68 506 }
JonFreeman 3:ecb00e0e8d68 507
JonFreeman 8:93203f473f6e 508 void setVI (double v, double i)
JonFreeman 8:93203f473f6e 509 {
JonFreeman 4:21d91465e4b1 510 MotorA.set_V_limit (v); // Sets max motor voltage
JonFreeman 4:21d91465e4b1 511 MotorA.set_I_limit (i); // Sets max motor current
JonFreeman 4:21d91465e4b1 512 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 513 MotorB.set_I_limit (i);
JonFreeman 4:21d91465e4b1 514 }
JonFreeman 8:93203f473f6e 515 void setV (double v)
JonFreeman 8:93203f473f6e 516 {
JonFreeman 2:04761b196473 517 MotorA.set_V_limit (v);
JonFreeman 4:21d91465e4b1 518 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 519 }
JonFreeman 8:93203f473f6e 520 void setI (double i)
JonFreeman 8:93203f473f6e 521 {
JonFreeman 2:04761b196473 522 MotorA.set_I_limit (i);
JonFreeman 2:04761b196473 523 MotorB.set_I_limit (i);
JonFreeman 0:435bf84ce48a 524 }
JonFreeman 2:04761b196473 525
JonFreeman 8:93203f473f6e 526 void read_RPM (uint32_t * dest)
JonFreeman 8:93203f473f6e 527 {
JonFreeman 5:ca86a7848d54 528 dest[0] = MotorA.RPM;
JonFreeman 5:ca86a7848d54 529 dest[1] = MotorB.RPM;
JonFreeman 5:ca86a7848d54 530 }
JonFreeman 5:ca86a7848d54 531
JonFreeman 8:93203f473f6e 532 void read_PPS (uint32_t * dest)
JonFreeman 8:93203f473f6e 533 {
JonFreeman 5:ca86a7848d54 534 dest[0] = MotorA.PPS;
JonFreeman 5:ca86a7848d54 535 dest[1] = MotorB.PPS;
JonFreeman 5:ca86a7848d54 536 }
JonFreeman 5:ca86a7848d54 537
JonFreeman 8:93203f473f6e 538 void read_last_VI (double * d) // only for test from cli
JonFreeman 8:93203f473f6e 539 {
JonFreeman 5:ca86a7848d54 540 d[0] = MotorA.last_V;
JonFreeman 5:ca86a7848d54 541 d[1] = MotorA.last_I;
JonFreeman 5:ca86a7848d54 542 d[2] = MotorB.last_V;
JonFreeman 5:ca86a7848d54 543 d[3] = MotorB.last_I;
JonFreeman 5:ca86a7848d54 544 }
JonFreeman 5:ca86a7848d54 545
JonFreeman 3:ecb00e0e8d68 546
JonFreeman 5:ca86a7848d54 547 /**
JonFreeman 5:ca86a7848d54 548 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 5:ca86a7848d54 549 Not part of ISR
JonFreeman 5:ca86a7848d54 550 */
JonFreeman 3:ecb00e0e8d68 551 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 0:435bf84ce48a 552 {
JonFreeman 6:f289a49c1eae 553 static uint32_t i = 0, tab_ptr = 0;
JonFreeman 8:93203f473f6e 554 // if (MotorA.dc_motor) {
JonFreeman 8:93203f473f6e 555 // MotorA.low_side_on ();
JonFreeman 8:93203f473f6e 556 // }
JonFreeman 8:93203f473f6e 557 // else {
JonFreeman 3:ecb00e0e8d68 558 if (MotorA.tickleon)
JonFreeman 3:ecb00e0e8d68 559 MotorA.high_side_off ();
JonFreeman 8:93203f473f6e 560 // }
JonFreeman 8:93203f473f6e 561 // if (MotorB.dc_motor) {
JonFreeman 8:93203f473f6e 562 // MotorB.low_side_on ();
JonFreeman 8:93203f473f6e 563 // }
JonFreeman 8:93203f473f6e 564 // else {
JonFreeman 3:ecb00e0e8d68 565 if (MotorB.tickleon)
JonFreeman 3:ecb00e0e8d68 566 MotorB.high_side_off ();
JonFreeman 8:93203f473f6e 567 // }
JonFreeman 0:435bf84ce48a 568 if (AtoD_Semaphore > 20) {
JonFreeman 8:93203f473f6e 569 pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
JonFreeman 0:435bf84ce48a 570 AtoD_Semaphore = 20;
JonFreeman 0:435bf84ce48a 571 }
JonFreeman 0:435bf84ce48a 572 while (AtoD_Semaphore > 0) {
JonFreeman 0:435bf84ce48a 573 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 574 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 575 switch (i) { //
JonFreeman 0:435bf84ce48a 576 case 0:
JonFreeman 0:435bf84ce48a 577 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 578 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 579 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 580 case 1:
JonFreeman 0:435bf84ce48a 581 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 582 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 583 break;
JonFreeman 0:435bf84ce48a 584 case 2:
JonFreeman 0:435bf84ce48a 585 MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 586 break;
JonFreeman 0:435bf84ce48a 587 case 3:
JonFreeman 0:435bf84ce48a 588 MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 589 if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter
JonFreeman 0:435bf84ce48a 590 tab_ptr = 0;
JonFreeman 0:435bf84ce48a 591 break;
JonFreeman 0:435bf84ce48a 592 }
JonFreeman 0:435bf84ce48a 593 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 594 if (i > 3)
JonFreeman 0:435bf84ce48a 595 i = 0;
JonFreeman 3:ecb00e0e8d68 596 } // end of while (AtoD_Semaphore > 0) {
JonFreeman 3:ecb00e0e8d68 597 if (MotorA.tickleon) {
JonFreeman 8:93203f473f6e 598 // if (MotorA.dc_motor || MotorA.tickleon) {
JonFreeman 3:ecb00e0e8d68 599 MotorA.tickleon--;
JonFreeman 5:ca86a7848d54 600 MotorA.motor_set (); // Reactivate any high side switches turned off above
JonFreeman 3:ecb00e0e8d68 601 }
JonFreeman 3:ecb00e0e8d68 602 if (MotorB.tickleon) {
JonFreeman 8:93203f473f6e 603 // if (MotorB.dc_motor || MotorB.tickleon) {
JonFreeman 3:ecb00e0e8d68 604 MotorB.tickleon--;
JonFreeman 3:ecb00e0e8d68 605 MotorB.motor_set ();
JonFreeman 0:435bf84ce48a 606 }
JonFreeman 0:435bf84ce48a 607 }
JonFreeman 0:435bf84ce48a 608
JonFreeman 0:435bf84ce48a 609 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 610 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 0:435bf84ce48a 611 * ISR also filters signal
JonFreeman 0:435bf84ce48a 612 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 613 */
JonFreeman 0:435bf84ce48a 614 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 615 {
JonFreeman 5:ca86a7848d54 616 return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 617 }
JonFreeman 0:435bf84ce48a 618
JonFreeman 0:435bf84ce48a 619 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 620 {
JonFreeman 5:ca86a7848d54 621 return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 622 }
JonFreeman 0:435bf84ce48a 623
JonFreeman 8:93203f473f6e 624 void read_supply_vi (double * val) // called from cli
JonFreeman 8:93203f473f6e 625 {
JonFreeman 5:ca86a7848d54 626 val[0] = MotorA.I.ave;
JonFreeman 5:ca86a7848d54 627 val[1] = MotorB.I.ave;
JonFreeman 5:ca86a7848d54 628 val[2] = Read_BatteryVolts ();
JonFreeman 0:435bf84ce48a 629 }
JonFreeman 0:435bf84ce48a 630
JonFreeman 8:93203f473f6e 631 void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb
JonFreeman 8:93203f473f6e 632 {
JonFreeman 2:04761b196473 633 MotorA.set_mode (mode);
JonFreeman 2:04761b196473 634 MotorB.set_mode (mode);
JonFreeman 2:04761b196473 635 if (mode == REGENBRAKE) {
JonFreeman 5:ca86a7848d54 636 if (val > 1.0)
JonFreeman 5:ca86a7848d54 637 val = 1.0;
JonFreeman 5:ca86a7848d54 638 if (val < 0.0)
JonFreeman 5:ca86a7848d54 639 val = 0.0;
JonFreeman 5:ca86a7848d54 640 val *= 0.9; // set upper limit, this is essential
JonFreeman 5:ca86a7848d54 641 val = sqrt (val); // to linearise effect
JonFreeman 5:ca86a7848d54 642 setVI (val, 1.0);
JonFreeman 2:04761b196473 643 }
JonFreeman 2:04761b196473 644 }
JonFreeman 0:435bf84ce48a 645
JonFreeman 7:6deaeace9a3e 646 extern void setup_comms () ;
JonFreeman 7:6deaeace9a3e 647 extern void command_line_interpreter_pc () ;
JonFreeman 7:6deaeace9a3e 648 extern void command_line_interpreter_loco () ;
JonFreeman 0:435bf84ce48a 649 extern int check_24LC64 () ; // Call from near top of main() to init i2c bus
JonFreeman 0:435bf84ce48a 650 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 0:435bf84ce48a 651 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 0:435bf84ce48a 652
JonFreeman 5:ca86a7848d54 653 /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes
JonFreeman 3:ecb00e0e8d68 654 uint8_t MotA_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 655 MotB_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 656 gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode
JonFreeman 3:ecb00e0e8d68 657 serv1, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 658 serv2, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 659 cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
JonFreeman 3:ecb00e0e8d68 660 last;
JonFreeman 3:ecb00e0e8d68 661 } ;
JonFreeman 5:ca86a7848d54 662 */
JonFreeman 8:93203f473f6e 663 int I_Am () // Returns boards id number as ASCII char
JonFreeman 8:93203f473f6e 664 {
JonFreeman 5:ca86a7848d54 665 return IAm;
JonFreeman 3:ecb00e0e8d68 666 }
JonFreeman 3:ecb00e0e8d68 667
JonFreeman 8:93203f473f6e 668 double mph (int rpm)
JonFreeman 8:93203f473f6e 669 {
JonFreeman 7:6deaeace9a3e 670 if (mode_good_flag) {
JonFreeman 7:6deaeace9a3e 671 return rpm2mph * (double) rpm;
JonFreeman 7:6deaeace9a3e 672 }
JonFreeman 7:6deaeace9a3e 673 return -1.0;
JonFreeman 7:6deaeace9a3e 674 }
JonFreeman 4:21d91465e4b1 675
JonFreeman 8:93203f473f6e 676 void restorer ()
JonFreeman 8:93203f473f6e 677 {
JonFreeman 8:93203f473f6e 678 motors_restore.detach ();
JonFreeman 8:93203f473f6e 679 if (MotorA.dc_motor) {
JonFreeman 8:93203f473f6e 680 MotorA.set_V_limit (MotorA.last_V);
JonFreeman 8:93203f473f6e 681 MotorA.set_I_limit (MotorA.last_I);
JonFreeman 8:93203f473f6e 682 MotorA.motor_set ();
JonFreeman 8:93203f473f6e 683 }
JonFreeman 8:93203f473f6e 684 if (MotorB.dc_motor) {
JonFreeman 8:93203f473f6e 685 MotorB.set_V_limit (MotorB.last_V);
JonFreeman 8:93203f473f6e 686 MotorB.set_I_limit (MotorB.last_I);
JonFreeman 8:93203f473f6e 687 MotorB.motor_set ();
JonFreeman 8:93203f473f6e 688 }
JonFreeman 8:93203f473f6e 689 }
JonFreeman 8:93203f473f6e 690
JonFreeman 8:93203f473f6e 691 //int ttime = 3; // resettable via cli 'stt', used to determine suitable force low on period for driving dc motor
JonFreeman 8:93203f473f6e 692 bool worth_the_bother (double a, double b) {
JonFreeman 8:93203f473f6e 693 double c = a - b;
JonFreeman 8:93203f473f6e 694 if (c < 0.0)
JonFreeman 8:93203f473f6e 695 c = 0.0 - c;
JonFreeman 8:93203f473f6e 696 if (c > 0.02) {
JonFreeman 8:93203f473f6e 697 // pc.printf ("%.3f\r\n", c);
JonFreeman 8:93203f473f6e 698 return true;
JonFreeman 8:93203f473f6e 699 }
JonFreeman 8:93203f473f6e 700 return false;
JonFreeman 8:93203f473f6e 701 }
JonFreeman 8:93203f473f6e 702
JonFreeman 8:93203f473f6e 703 void prscfuck (int v) {
JonFreeman 8:93203f473f6e 704 /*
JonFreeman 8:93203f473f6e 705 int prescaler ( int value )
JonFreeman 8:93203f473f6e 706
JonFreeman 8:93203f473f6e 707 Set the PWM prescaler.
JonFreeman 8:93203f473f6e 708
JonFreeman 8:93203f473f6e 709 The period of all PWM pins on the same PWM unit have to be reset after using this!
JonFreeman 8:93203f473f6e 710
JonFreeman 8:93203f473f6e 711 Parameters:
JonFreeman 8:93203f473f6e 712 value - The required prescaler. Special values: 0 = lock current prescaler, -1 = use dynamic prescaler
JonFreeman 8:93203f473f6e 713 return - The prescaler which was set (can differ from requested prescaler if not possible)
JonFreeman 8:93203f473f6e 714
JonFreeman 8:93203f473f6e 715 Definition at line 82 of file FastPWM_common.cpp.
JonFreeman 8:93203f473f6e 716 */
JonFreeman 8:93203f473f6e 717 if (v < 1)
JonFreeman 8:93203f473f6e 718 v = 1;
JonFreeman 8:93203f473f6e 719 int w = A_MAX_V_PWM.prescaler (v);
JonFreeman 8:93203f473f6e 720 pc.printf ("Attempt setting prescaler %d returned %d\r\n", v, w);
JonFreeman 8:93203f473f6e 721 }
JonFreeman 8:93203f473f6e 722
JonFreeman 8:93203f473f6e 723 enum { HAND_CONT_STATE_BEGIN,
JonFreeman 8:93203f473f6e 724 HAND_CONT_STATE_POWER_CYCLE_TO_EXIT,
JonFreeman 8:93203f473f6e 725 HAND_CONT_STATE_INTO_BRAKING,
JonFreeman 8:93203f473f6e 726 HAND_CONT_STATE_INTO_DRIVING,
JonFreeman 8:93203f473f6e 727 HAND_CONT_STATE_BRAKING,
JonFreeman 8:93203f473f6e 728 HAND_CONT_STATE_DRIVING
JonFreeman 8:93203f473f6e 729 } ;
JonFreeman 8:93203f473f6e 730
JonFreeman 8:93203f473f6e 731 void hand_control_state_machine () {
JonFreeman 8:93203f473f6e 732 static int new_hand_controller_state = HAND_CONT_STATE_BEGIN;
JonFreeman 8:93203f473f6e 733 // static int old_hand_controller_state = HAND_CONT_STATE_BEGIN;
JonFreeman 8:93203f473f6e 734 static int old_hand_controller_direction = T5, t = 0;
JonFreeman 8:93203f473f6e 735 static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
JonFreeman 8:93203f473f6e 736 if (T5 != old_hand_controller_direction) { // 1 Forward, 0 Reverse
JonFreeman 8:93203f473f6e 737 pc.printf ("Direction change! Power off then on again to resume\r\n");
JonFreeman 8:93203f473f6e 738 mode_set_both_motors (REGENBRAKE, 1.0);
JonFreeman 8:93203f473f6e 739 // old_hand_controller_state = new_hand_controller_state;
JonFreeman 8:93203f473f6e 740 new_hand_controller_state = HAND_CONT_STATE_POWER_CYCLE_TO_EXIT;
JonFreeman 8:93203f473f6e 741 }
JonFreeman 8:93203f473f6e 742 pot_position = Read_DriverPot (); // gets to here on first pass before pot has been read
JonFreeman 8:93203f473f6e 743 switch (new_hand_controller_state) {
JonFreeman 8:93203f473f6e 744 case HAND_CONT_STATE_BEGIN:
JonFreeman 8:93203f473f6e 745 pot_position = Read_DriverPot ();
JonFreeman 8:93203f473f6e 746 if (t++ > 2 && pot_position < 0.02) {
JonFreeman 8:93203f473f6e 747 pc.printf ("In BEGIN, pot %.2f\r\n", pot_position);
JonFreeman 8:93203f473f6e 748 new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
JonFreeman 8:93203f473f6e 749 }
JonFreeman 8:93203f473f6e 750 break;
JonFreeman 8:93203f473f6e 751 case HAND_CONT_STATE_POWER_CYCLE_TO_EXIT:
JonFreeman 8:93203f473f6e 752 break;
JonFreeman 8:93203f473f6e 753 case HAND_CONT_STATE_INTO_BRAKING:
JonFreeman 8:93203f473f6e 754 brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
JonFreeman 8:93203f473f6e 755 mode_set_both_motors (REGENBRAKE, brake_effort);
JonFreeman 8:93203f473f6e 756 old_pot_position = pot_position;
JonFreeman 8:93203f473f6e 757 new_hand_controller_state = HAND_CONT_STATE_BRAKING;
JonFreeman 8:93203f473f6e 758 pc.printf ("Brake\r\n");
JonFreeman 8:93203f473f6e 759 break;
JonFreeman 8:93203f473f6e 760 case HAND_CONT_STATE_INTO_DRIVING:
JonFreeman 8:93203f473f6e 761 new_hand_controller_state = HAND_CONT_STATE_DRIVING;
JonFreeman 8:93203f473f6e 762 pc.printf ("Drive\r\n");
JonFreeman 8:93203f473f6e 763 if (T5)
JonFreeman 8:93203f473f6e 764 mode_set_both_motors (FORWARD, 0.0); // sets both motors
JonFreeman 8:93203f473f6e 765 else
JonFreeman 8:93203f473f6e 766 mode_set_both_motors (REVERSE, 0.0);
JonFreeman 8:93203f473f6e 767 break;
JonFreeman 8:93203f473f6e 768 case HAND_CONT_STATE_BRAKING:
JonFreeman 8:93203f473f6e 769 if (pot_position > Pot_Brake_Range) // escape braking into drive
JonFreeman 8:93203f473f6e 770 new_hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
JonFreeman 8:93203f473f6e 771 else {
JonFreeman 8:93203f473f6e 772 if (worth_the_bother(pot_position, old_pot_position)) {
JonFreeman 8:93203f473f6e 773 old_pot_position = pot_position;
JonFreeman 8:93203f473f6e 774 brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
JonFreeman 8:93203f473f6e 775 mode_set_both_motors (REGENBRAKE, brake_effort);
JonFreeman 8:93203f473f6e 776 // pc.printf ("Brake %.2f\r\n", brake_effort);
JonFreeman 8:93203f473f6e 777 }
JonFreeman 8:93203f473f6e 778 }
JonFreeman 8:93203f473f6e 779 break;
JonFreeman 8:93203f473f6e 780 case HAND_CONT_STATE_DRIVING:
JonFreeman 8:93203f473f6e 781 if (pot_position < Pot_Brake_Range) // escape braking into drive
JonFreeman 8:93203f473f6e 782 new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
JonFreeman 8:93203f473f6e 783 else {
JonFreeman 8:93203f473f6e 784 if (worth_the_bother(pot_position, old_pot_position)) {
JonFreeman 8:93203f473f6e 785 old_pot_position = pot_position;
JonFreeman 8:93203f473f6e 786 drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range);
JonFreeman 8:93203f473f6e 787 setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ???
JonFreeman 8:93203f473f6e 788 pc.printf ("Drive %.2f\r\n", drive_effort);
JonFreeman 8:93203f473f6e 789 }
JonFreeman 8:93203f473f6e 790 }
JonFreeman 8:93203f473f6e 791 break;
JonFreeman 8:93203f473f6e 792 default:
JonFreeman 8:93203f473f6e 793 break;
JonFreeman 8:93203f473f6e 794 } // endof switch (hand_controller_state) {
JonFreeman 8:93203f473f6e 795 }
JonFreeman 8:93203f473f6e 796
JonFreeman 0:435bf84ce48a 797 int main()
JonFreeman 0:435bf84ce48a 798 {
JonFreeman 4:21d91465e4b1 799 int eighth_sec_count = 0;
JonFreeman 0:435bf84ce48a 800
JonFreeman 4:21d91465e4b1 801 MotA = 0; // Output all 0s to Motor drive ports A and B
JonFreeman 0:435bf84ce48a 802 MotB = 0;
JonFreeman 7:6deaeace9a3e 803 // MotPtr[0] = &MotorA; // Pointers to motor class objects
JonFreeman 7:6deaeace9a3e 804 // MotPtr[1] = &MotorB;
JonFreeman 8:93203f473f6e 805
JonFreeman 6:f289a49c1eae 806 Temperature_pin.fall (&temp_sensor_isr);
JonFreeman 6:f289a49c1eae 807 Temperature_pin.mode (PullUp);
JonFreeman 4:21d91465e4b1 808
JonFreeman 4:21d91465e4b1 809 MAH1.rise (& MAH_isr); // Set up interrupt vectors
JonFreeman 3:ecb00e0e8d68 810 MAH1.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 811 MAH2.rise (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 812 MAH2.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 813 MAH3.rise (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 814 MAH3.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 815
JonFreeman 3:ecb00e0e8d68 816 MBH1.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 817 MBH1.fall (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 818 MBH2.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 819 MBH2.fall (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 820 MBH3.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 821 MBH3.fall (& MBH_isr);
JonFreeman 4:21d91465e4b1 822
JonFreeman 0:435bf84ce48a 823 MAH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 824 MAH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 825 MAH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 826 MBH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 827 MBH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 828 MBH3.mode (PullUp);
JonFreeman 4:21d91465e4b1 829 Servo1_i.mode (PullUp);
JonFreeman 4:21d91465e4b1 830 Servo2_i.mode (PullUp);
JonFreeman 4:21d91465e4b1 831
JonFreeman 0:435bf84ce48a 832 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 0:435bf84ce48a 833 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 834 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 6:f289a49c1eae 835 temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960);
JonFreeman 0:435bf84ce48a 836 // Done setting up system interrupt timers
JonFreeman 6:f289a49c1eae 837 temperature_timer.start ();
JonFreeman 0:435bf84ce48a 838
JonFreeman 6:f289a49c1eae 839 // const int TXTBUFSIZ = 36;
JonFreeman 6:f289a49c1eae 840 // char buff[TXTBUFSIZ];
JonFreeman 3:ecb00e0e8d68 841 pc.baud (9600);
JonFreeman 4:21d91465e4b1 842 com3.baud (1200);
JonFreeman 4:21d91465e4b1 843 com2.baud (19200);
JonFreeman 7:6deaeace9a3e 844 setup_comms ();
JonFreeman 6:f289a49c1eae 845
JonFreeman 7:6deaeace9a3e 846 IAm = '0';
JonFreeman 5:ca86a7848d54 847 if (check_24LC64() != 0xa0) { // searches for i2c devices, returns address of highest found
JonFreeman 0:435bf84ce48a 848 pc.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 5:ca86a7848d54 849 com2.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 7:6deaeace9a3e 850 eeprom_flag = false;
JonFreeman 8:93203f473f6e 851 } else { // Found 24LC64 memory on I2C
JonFreeman 7:6deaeace9a3e 852 eeprom_flag = true;
JonFreeman 5:ca86a7848d54 853 bool k;
JonFreeman 6:f289a49c1eae 854 k = rd_24LC64 (0, mode_bytes, 32);
JonFreeman 5:ca86a7848d54 855 if (!k)
JonFreeman 5:ca86a7848d54 856 com2.printf ("Error reading from eeprom\r\n");
JonFreeman 5:ca86a7848d54 857
JonFreeman 8:93203f473f6e 858 // int err = 0;
JonFreeman 6:f289a49c1eae 859 for (int i = 0; i < numof_eeprom_options; i++) {
JonFreeman 6:f289a49c1eae 860 if ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max)) {
JonFreeman 5:ca86a7848d54 861 com2.printf ("EEROM error with %s\r\n", option_list[i].t);
JonFreeman 8:93203f473f6e 862 pc.printf ("EEROM error with %s\r\n", option_list[i].t);
JonFreeman 8:93203f473f6e 863 if (i == ID) { // need to force id to '0'
JonFreeman 8:93203f473f6e 864 pc.printf ("Bad board ID value %d, forcing to \'0\'\r\n");
JonFreeman 8:93203f473f6e 865 mode_bytes[ID] = '0';
JonFreeman 8:93203f473f6e 866 }
JonFreeman 8:93203f473f6e 867 // err++;
JonFreeman 5:ca86a7848d54 868 }
JonFreeman 5:ca86a7848d54 869 // else
JonFreeman 5:ca86a7848d54 870 // com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
JonFreeman 5:ca86a7848d54 871 }
JonFreeman 7:6deaeace9a3e 872 rpm2mph = 0.0;
JonFreeman 8:93203f473f6e 873 if (mode_bytes[WHEELGEAR] == 0) // avoid making div0 error
JonFreeman 8:93203f473f6e 874 mode_bytes[WHEELGEAR]++;
JonFreeman 8:93203f473f6e 875 // if (err == 0) {
JonFreeman 8:93203f473f6e 876 mode_good_flag = true;
JonFreeman 8:93203f473f6e 877 MotorA.direction_set (mode_bytes[MOTADIR]);
JonFreeman 8:93203f473f6e 878 MotorB.direction_set (mode_bytes[MOTBDIR]);
JonFreeman 8:93203f473f6e 879 IAm = mode_bytes[ID];
JonFreeman 8:93203f473f6e 880 rpm2mph = 60.0 // to Motor Revs per hour;
JonFreeman 8:93203f473f6e 881 * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR]) // Wheel revs per hour
JonFreeman 8:93203f473f6e 882 * PI * ((double)mode_bytes[WHEELDIA] / 1000.0) // metres per hour
JonFreeman 8:93203f473f6e 883 * 39.37 / (1760.0 * 36.0); // miles per hour
JonFreeman 8:93203f473f6e 884 // }
JonFreeman 8:93203f473f6e 885 // Alternative ID 1 to 9
JonFreeman 5:ca86a7848d54 886 // com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
JonFreeman 5:ca86a7848d54 887 }
JonFreeman 6:f289a49c1eae 888 // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
JonFreeman 5:ca86a7848d54 889 T2 = 0; // T2, T3, T4 As yet unused pins
JonFreeman 8:93203f473f6e 890 // T3 = 0;
JonFreeman 8:93203f473f6e 891 // T4 = 0;
JonFreeman 5:ca86a7848d54 892 // T5 = 0; now input from fw/re on remote control box
JonFreeman 0:435bf84ce48a 893 T6 = 0;
JonFreeman 3:ecb00e0e8d68 894 // MotPtr[0]->set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 895 MotorA.set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 896 MotorB.set_mode (REGENBRAKE);
JonFreeman 5:ca86a7848d54 897 setVI (0.9, 0.5);
JonFreeman 5:ca86a7848d54 898
JonFreeman 8:93203f473f6e 899 // prscfuck (PWM_PRESECALER_DEFAULT); // Test fucking with fastpwm prescaler
JonFreeman 8:93203f473f6e 900
JonFreeman 4:21d91465e4b1 901 Servos[0] = Servos[1] = NULL;
JonFreeman 4:21d91465e4b1 902 // NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
JonFreeman 4:21d91465e4b1 903 // Only works with unconditional inline code
JonFreeman 4:21d91465e4b1 904 // However, setup code for Input by default,
JonFreeman 4:21d91465e4b1 905 // This can be used to monitor Servo output also !
JonFreeman 4:21d91465e4b1 906 Servo Servo1 (PB_8) ;
JonFreeman 4:21d91465e4b1 907 Servos[0] = & Servo1;
JonFreeman 4:21d91465e4b1 908 Servo Servo2 (PB_9) ;
JonFreeman 4:21d91465e4b1 909 Servos[1] = & Servo2;
JonFreeman 8:93203f473f6e 910
JonFreeman 7:6deaeace9a3e 911 // pc.printf ("last_temp_count = %d\r\n", last_temp_count); // Has had time to do at least 1 conversion
JonFreeman 6:f289a49c1eae 912 if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C
JonFreeman 6:f289a49c1eae 913 temp_sensor_exists = true;
JonFreeman 8:93203f473f6e 914 /*
JonFreeman 8:93203f473f6e 915 // Setup Complete ! Can now start main control forever loop.
JonFreeman 8:93203f473f6e 916 // March 16th 2018 thoughts !!!
JonFreeman 8:93203f473f6e 917 // Control from one of several sources and types as selected in options bytes in eeprom.
JonFreeman 8:93203f473f6e 918 // Control could be from e.g. Pot, Com2, Servos etc.
JonFreeman 8:93203f473f6e 919 // Suggest e.g.
JonFreeman 8:93203f473f6e 920 */ /*
JonFreeman 3:ecb00e0e8d68 921 switch (mode_byte) { // executed once only upon startup
JonFreeman 3:ecb00e0e8d68 922 case POT:
JonFreeman 3:ecb00e0e8d68 923 while (1) { // forever loop
JonFreeman 3:ecb00e0e8d68 924 call common_stuff ();
JonFreeman 3:ecb00e0e8d68 925 ...
JonFreeman 3:ecb00e0e8d68 926 }
JonFreeman 3:ecb00e0e8d68 927 break;
JonFreeman 3:ecb00e0e8d68 928 case COM2:
JonFreeman 3:ecb00e0e8d68 929 while (1) { // forever loop
JonFreeman 3:ecb00e0e8d68 930 call common_stuff ();
JonFreeman 3:ecb00e0e8d68 931 ...
JonFreeman 3:ecb00e0e8d68 932 }
JonFreeman 3:ecb00e0e8d68 933 break;
JonFreeman 3:ecb00e0e8d68 934 }
JonFreeman 3:ecb00e0e8d68 935 */
JonFreeman 7:6deaeace9a3e 936 // pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
JonFreeman 8:93203f473f6e 937 dc_motor_kicker_timer.start ();
JonFreeman 8:93203f473f6e 938 int old_hand_controller_direction = T5;
JonFreeman 8:93203f473f6e 939 if (mode_bytes[COMM_SRC] == 3) { // Read fwd/rev switch 'T5', PA15 on 401
JonFreeman 8:93203f473f6e 940 pc.printf ("Pot control\r\n");
JonFreeman 8:93203f473f6e 941 if (T5)
JonFreeman 8:93203f473f6e 942 mode_set_both_motors (FORWARD, 0.0); // sets both motors
JonFreeman 8:93203f473f6e 943 else
JonFreeman 8:93203f473f6e 944 mode_set_both_motors (REVERSE, 0.0);
JonFreeman 8:93203f473f6e 945 }
JonFreeman 8:93203f473f6e 946
JonFreeman 8:93203f473f6e 947 pc.printf ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]);
JonFreeman 8:93203f473f6e 948
JonFreeman 0:435bf84ce48a 949 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 950 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 7:6deaeace9a3e 951 command_line_interpreter_pc () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 7:6deaeace9a3e 952 command_line_interpreter_loco () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 953 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 0:435bf84ce48a 954 }
JonFreeman 0:435bf84ce48a 955 loop_flag = false; // Clear flag set by ticker interrupt handler
JonFreeman 8:93203f473f6e 956 switch (mode_bytes[COMM_SRC]) { // Look to selected source of driving command, act on commands from wherever
JonFreeman 8:93203f473f6e 957 case 0: // Invalid
JonFreeman 8:93203f473f6e 958 break;
JonFreeman 8:93203f473f6e 959 case 1: // COM1 - Primarily for testing, bypassed by command line interpreter
JonFreeman 8:93203f473f6e 960 break;
JonFreeman 8:93203f473f6e 961 case 2: // COM2 - Primarily for testing, bypassed by command line interpreter
JonFreeman 8:93203f473f6e 962 break;
JonFreeman 8:93203f473f6e 963 case 3: // Put all hand controller input stuff here
JonFreeman 8:93203f473f6e 964 hand_control_state_machine ();
JonFreeman 8:93203f473f6e 965 break; // endof hand controller stuff
JonFreeman 8:93203f473f6e 966 case 4: // Servo1
JonFreeman 8:93203f473f6e 967 break;
JonFreeman 8:93203f473f6e 968 case 5: // Servo2
JonFreeman 8:93203f473f6e 969 break;
JonFreeman 8:93203f473f6e 970 default:
JonFreeman 8:93203f473f6e 971 pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]);
JonFreeman 8:93203f473f6e 972 break;
JonFreeman 8:93203f473f6e 973 } // endof switch (mode_bytes[COMM_SRC]) {
JonFreeman 8:93203f473f6e 974
JonFreeman 8:93203f473f6e 975 dc_motor_kicker_timer.reset ();
JonFreeman 5:ca86a7848d54 976 MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 5:ca86a7848d54 977 MotorB.pulses_per_sec (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
JonFreeman 8:93203f473f6e 978 // T4 = !T4; // toggle to hang scope on to verify loop execution
JonFreeman 0:435bf84ce48a 979 // do stuff
JonFreeman 8:93203f473f6e 980 if (MotorA.dc_motor) {
JonFreeman 8:93203f473f6e 981 MotorA.raw_V_pwm (1);
JonFreeman 8:93203f473f6e 982 MotorA.low_side_on ();
JonFreeman 8:93203f473f6e 983 }
JonFreeman 8:93203f473f6e 984 if (MotorB.dc_motor) {
JonFreeman 8:93203f473f6e 985 MotorB.raw_V_pwm (1);
JonFreeman 8:93203f473f6e 986 MotorB.low_side_on ();
JonFreeman 8:93203f473f6e 987 }
JonFreeman 8:93203f473f6e 988 if (MotorA.dc_motor || MotorB.dc_motor) {
JonFreeman 8:93203f473f6e 989 // motors_restore.attach_us (&restorer, ttime);
JonFreeman 8:93203f473f6e 990 motors_restore.attach_us (&restorer, 25);
JonFreeman 8:93203f473f6e 991 }
JonFreeman 8:93203f473f6e 992
JonFreeman 0:435bf84ce48a 993 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 994 flag_8Hz = false;
JonFreeman 3:ecb00e0e8d68 995 LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 8:93203f473f6e 996 if (WatchDogEnable) {
JonFreeman 8:93203f473f6e 997 WatchDog--;
JonFreeman 8:93203f473f6e 998 if (WatchDog == 0) { // Deal with WatchDog timer timeout here
JonFreeman 8:93203f473f6e 999 setVI (0.0, 0.0); // set motor volts and amps to zero
JonFreeman 8:93203f473f6e 1000 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 8:93203f473f6e 1001 } // End of dealing with WatchDog timer timeout
JonFreeman 8:93203f473f6e 1002 if (WatchDog < 0)
JonFreeman 8:93203f473f6e 1003 WatchDog = 0;
JonFreeman 8:93203f473f6e 1004 }
JonFreeman 4:21d91465e4b1 1005 eighth_sec_count++;
JonFreeman 4:21d91465e4b1 1006 if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 4:21d91465e4b1 1007 eighth_sec_count = 0;
JonFreeman 6:f289a49c1eae 1008 MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
JonFreeman 6:f289a49c1eae 1009 MotorB.current_calc ();
JonFreeman 8:93203f473f6e 1010 /* if (temp_sensor_exists) {
JonFreeman 8:93203f473f6e 1011 double tmprt = (double) last_temp_count;
JonFreeman 8:93203f473f6e 1012 tmprt /= 16.0;
JonFreeman 8:93203f473f6e 1013 tmprt -= 50.0;
JonFreeman 8:93203f473f6e 1014 pc.printf ("Temp %.2f\r\n", tmprt);
JonFreeman 8:93203f473f6e 1015 }*/
JonFreeman 5:ca86a7848d54 1016 // 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 1017 }
JonFreeman 0:435bf84ce48a 1018 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 1019 } // End of main programme loop
JonFreeman 0:435bf84ce48a 1020 }