STM3 ESC dual brushless motor controller. 10-60v, motor power rating tiny to kW. Ganged or independent motor control As used in 'The Brute' locomotive - www.jons-workshop.com
Dependencies: mbed BufferedSerial Servo FastPWM
main.cpp@11:bfb73f083009, 2019-01-19 (annotated)
- Committer:
- JonFreeman
- Date:
- Sat Jan 19 11:45:01 2019 +0000
- Revision:
- 11:bfb73f083009
- Parent:
- 10:e40d8724268a
- Child:
- 12:d1d21a2941ef
Tidied class parameter passing, serial problem fixed (was hardware)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JonFreeman | 10:e40d8724268a | 1 | // Cloned from 'DualBLS2018_06' on 23 November 2018 |
JonFreeman | 0:435bf84ce48a | 2 | #include "mbed.h" |
JonFreeman | 8:93203f473f6e | 3 | //#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h" |
JonFreeman | 11:bfb73f083009 | 4 | #include "DualBLS.h" |
JonFreeman | 11:bfb73f083009 | 5 | |
JonFreeman | 8:93203f473f6e | 6 | #include "stm32f401xe.h" |
JonFreeman | 0:435bf84ce48a | 7 | #include "BufferedSerial.h" |
JonFreeman | 0:435bf84ce48a | 8 | #include "FastPWM.h" |
JonFreeman | 4:21d91465e4b1 | 9 | #include "Servo.h" |
JonFreeman | 10:e40d8724268a | 10 | #include "brushless_motor.h" |
JonFreeman | 11:bfb73f083009 | 11 | #include "Radio_Control_In.h" |
JonFreeman | 5:ca86a7848d54 | 12 | |
JonFreeman | 5:ca86a7848d54 | 13 | /* |
JonFreeman | 10:e40d8724268a | 14 | Brushless_STM3 board |
JonFreeman | 10:e40d8724268a | 15 | |
JonFreeman | 10:e40d8724268a | 16 | New 29th May 2018 ** |
JonFreeman | 10:e40d8724268a | 17 | 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 | 18 | */ |
JonFreeman | 5:ca86a7848d54 | 19 | |
JonFreeman | 5:ca86a7848d54 | 20 | |
JonFreeman | 0:435bf84ce48a | 21 | /* STM32F401RE - compile using NUCLEO-F401RE |
JonFreeman | 6:f289a49c1eae | 22 | // PROJECT - Dual Brushless Motor Controller - Jon Freeman June 2018. |
JonFreeman | 0:435bf84ce48a | 23 | |
JonFreeman | 0:435bf84ce48a | 24 | AnalogIn to read each motor current |
JonFreeman | 0:435bf84ce48a | 25 | |
JonFreeman | 0:435bf84ce48a | 26 | ____________________________________________________________________________________ |
JonFreeman | 0:435bf84ce48a | 27 | CONTROL PHILOSOPHY |
JonFreeman | 0:435bf84ce48a | 28 | This Bogie drive board software should ensure sensible control when commands supplied are not sensible ! |
JonFreeman | 0:435bf84ce48a | 29 | |
JonFreeman | 0:435bf84ce48a | 30 | That is, smooth transition between Drive, Coast and Brake to be implemented here. |
JonFreeman | 0:435bf84ce48a | 31 | The remote controller must not be relied upon to deliver sensible command sequences. |
JonFreeman | 0:435bf84ce48a | 32 | |
JonFreeman | 0:435bf84ce48a | 33 | So much the better if the remote controller does issue sensible commands, but ... |
JonFreeman | 0:435bf84ce48a | 34 | |
JonFreeman | 0:435bf84ce48a | 35 | ____________________________________________________________________________________ |
JonFreeman | 0:435bf84ce48a | 36 | |
JonFreeman | 0:435bf84ce48a | 37 | |
JonFreeman | 0:435bf84ce48a | 38 | */ |
JonFreeman | 8:93203f473f6e | 39 | |
JonFreeman | 7:6deaeace9a3e | 40 | #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP |
JonFreeman | 9:ac2412df01be | 41 | #include "F401RE.h" // See here for warnings about Servo InterruptIn not working |
JonFreeman | 6:f289a49c1eae | 42 | #endif |
JonFreeman | 7:6deaeace9a3e | 43 | #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP |
JonFreeman | 10:e40d8724268a | 44 | #include "F446ZE.h" // A thought for future version |
JonFreeman | 6:f289a49c1eae | 45 | #endif |
JonFreeman | 0:435bf84ce48a | 46 | /* Global variable declarations */ |
JonFreeman | 3:ecb00e0e8d68 | 47 | volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US |
JonFreeman | 5:ca86a7848d54 | 48 | int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup |
JonFreeman | 8:93203f473f6e | 49 | bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable |
JonFreeman | 0:435bf84ce48a | 50 | uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts |
JonFreeman | 0:435bf84ce48a | 51 | driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot |
JonFreeman | 0:435bf84ce48a | 52 | sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US |
JonFreeman | 5:ca86a7848d54 | 53 | AtoD_Semaphore = 0; |
JonFreeman | 5:ca86a7848d54 | 54 | int IAm; |
JonFreeman | 11:bfb73f083009 | 55 | bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop |
JonFreeman | 11:bfb73f083009 | 56 | bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec |
JonFreeman | 6:f289a49c1eae | 57 | bool temp_sensor_exists = false; |
JonFreeman | 11:bfb73f083009 | 58 | bool eeprom_flag; // gets set according to 24LC674 being found or not |
JonFreeman | 7:6deaeace9a3e | 59 | bool mode_good_flag = false; |
JonFreeman | 6:f289a49c1eae | 60 | char mode_bytes[36]; |
JonFreeman | 3:ecb00e0e8d68 | 61 | |
JonFreeman | 6:f289a49c1eae | 62 | uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01 |
JonFreeman | 6:f289a49c1eae | 63 | last_temp_count = 0; // global updated approx every 100ms after each LMT01 conversion completes |
JonFreeman | 8:93203f473f6e | 64 | // struct single_bogie_options bogie; |
JonFreeman | 8:93203f473f6e | 65 | double rpm2mph = 0.0; // gets calculated from eeprom mode entries if present |
JonFreeman | 0:435bf84ce48a | 66 | /* End of Global variable declarations */ |
JonFreeman | 0:435bf84ce48a | 67 | |
JonFreeman | 0:435bf84ce48a | 68 | Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc |
JonFreeman | 0:435bf84ce48a | 69 | Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop |
JonFreeman | 6:f289a49c1eae | 70 | Ticker temperature_find_ticker; |
JonFreeman | 6:f289a49c1eae | 71 | Timer temperature_timer; |
JonFreeman | 8:93203f473f6e | 72 | Timer dc_motor_kicker_timer; |
JonFreeman | 8:93203f473f6e | 73 | Timeout motors_restore; |
JonFreeman | 11:bfb73f083009 | 74 | RControl_In RC_chan_1 (PC_14); |
JonFreeman | 11:bfb73f083009 | 75 | RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin |
JonFreeman | 11:bfb73f083009 | 76 | |
JonFreeman | 0:435bf84ce48a | 77 | |
JonFreeman | 0:435bf84ce48a | 78 | // Interrupt Service Routines |
JonFreeman | 8:93203f473f6e | 79 | void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec |
JonFreeman | 8:93203f473f6e | 80 | { |
JonFreeman | 6:f289a49c1eae | 81 | static bool readflag = false; |
JonFreeman | 6:f289a49c1eae | 82 | int t = temperature_timer.read_ms (); |
JonFreeman | 6:f289a49c1eae | 83 | if ((t == 5) && (!readflag)) { |
JonFreeman | 6:f289a49c1eae | 84 | last_temp_count = temp_sensor_count; |
JonFreeman | 6:f289a49c1eae | 85 | temp_sensor_count = 0; |
JonFreeman | 6:f289a49c1eae | 86 | readflag = true; |
JonFreeman | 6:f289a49c1eae | 87 | } |
JonFreeman | 6:f289a49c1eae | 88 | if (t == 6) |
JonFreeman | 6:f289a49c1eae | 89 | readflag = false; |
JonFreeman | 6:f289a49c1eae | 90 | } |
JonFreeman | 0:435bf84ce48a | 91 | |
JonFreeman | 0:435bf84ce48a | 92 | /** void ISR_loop_timer () |
JonFreeman | 0:435bf84ce48a | 93 | * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above) |
JonFreeman | 0:435bf84ce48a | 94 | * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop. |
JonFreeman | 0:435bf84ce48a | 95 | * Increments global 'sys_timer', usable anywhere as general measure of elapsed time |
JonFreeman | 0:435bf84ce48a | 96 | */ |
JonFreeman | 0:435bf84ce48a | 97 | void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US |
JonFreeman | 0:435bf84ce48a | 98 | { |
JonFreeman | 0:435bf84ce48a | 99 | loop_flag = true; // set flag to allow main programme loop to proceed |
JonFreeman | 0:435bf84ce48a | 100 | sys_timer++; // Just a handy measure of elapsed time for anything to use |
JonFreeman | 0:435bf84ce48a | 101 | if ((sys_timer & 0x03) == 0) |
JonFreeman | 0:435bf84ce48a | 102 | flag_8Hz = true; |
JonFreeman | 0:435bf84ce48a | 103 | } |
JonFreeman | 0:435bf84ce48a | 104 | |
JonFreeman | 0:435bf84ce48a | 105 | /** void ISR_voltage_reader () |
JonFreeman | 0:435bf84ce48a | 106 | * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds |
JonFreeman | 0:435bf84ce48a | 107 | * |
JonFreeman | 0:435bf84ce48a | 108 | * AtoD_reader() called from convenient point in code to take readings outside of ISRs |
JonFreeman | 0:435bf84ce48a | 109 | */ |
JonFreeman | 0:435bf84ce48a | 110 | |
JonFreeman | 5:ca86a7848d54 | 111 | void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50 |
JonFreeman | 0:435bf84ce48a | 112 | { |
JonFreeman | 0:435bf84ce48a | 113 | AtoD_Semaphore++; |
JonFreeman | 2:04761b196473 | 114 | fast_sys_timer++; // Just a handy measure of elapsed time for anything to use |
JonFreeman | 0:435bf84ce48a | 115 | } |
JonFreeman | 0:435bf84ce48a | 116 | |
JonFreeman | 11:bfb73f083009 | 117 | void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some |
JonFreeman | 8:93203f473f6e | 118 | { |
JonFreeman | 11:bfb73f083009 | 119 | int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ? |
JonFreeman | 11:bfb73f083009 | 120 | temperature_timer.reset (); |
JonFreeman | 11:bfb73f083009 | 121 | temp_sensor_count++; |
JonFreeman | 11:bfb73f083009 | 122 | if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading |
JonFreeman | 11:bfb73f083009 | 123 | temp_sensor_count++; |
JonFreeman | 11:bfb73f083009 | 124 | // T2 = !T2; // scope hanger |
JonFreeman | 4:21d91465e4b1 | 125 | } |
JonFreeman | 4:21d91465e4b1 | 126 | |
JonFreeman | 11:bfb73f083009 | 127 | // End of Interrupt Service Routines |
JonFreeman | 4:21d91465e4b1 | 128 | |
JonFreeman | 0:435bf84ce48a | 129 | //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; |
JonFreeman | 0:435bf84ce48a | 130 | /* |
JonFreeman | 0:435bf84ce48a | 131 | 5 1 3 2 6 4 SENSOR SEQUENCE |
JonFreeman | 0:435bf84ce48a | 132 | |
JonFreeman | 3:ecb00e0e8d68 | 133 | 1 1 1 1 0 0 0 ---___---___ Hall1 |
JonFreeman | 3:ecb00e0e8d68 | 134 | 2 0 0 1 1 1 0 __---___---_ Hall2 |
JonFreeman | 3:ecb00e0e8d68 | 135 | 4 1 0 0 0 1 1 -___---___-- Hall3 |
JonFreeman | 0:435bf84ce48a | 136 | |
JonFreeman | 0:435bf84ce48a | 137 | UV WV WU VU VW UW OUTPUT SEQUENCE |
JonFreeman | 8:93203f473f6e | 138 | |
JonFreeman | 8:93203f473f6e | 139 | 8th July 2018 |
JonFreeman | 8:93203f473f6e | 140 | Added drive to DC brushed motors. |
JonFreeman | 8:93203f473f6e | 141 | Connect U and W to dc motor, leave V open. |
JonFreeman | 8:93203f473f6e | 142 | |
JonFreeman | 8:93203f473f6e | 143 | 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 | 144 | |
JonFreeman | 0:435bf84ce48a | 145 | */ |
JonFreeman | 3:ecb00e0e8d68 | 146 | const uint16_t A_tabl[] = { // Origial table |
JonFreeman | 0:435bf84ce48a | 147 | 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake |
JonFreeman | 10:e40d8724268a | 148 | 0, AWHVL,AVHUL,AWHUL,AUHWL,AUHVL,AVHWL,AUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 |
JonFreeman | 10:e40d8724268a | 149 | 0, AVHWL,AUHVL,AUHWL,AWHUL,AVHUL,AWHVL,AWHUL, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1 |
JonFreeman | 8:93203f473f6e | 150 | 0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking |
JonFreeman | 4:21d91465e4b1 | 151 | KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33] |
JonFreeman | 3:ecb00e0e8d68 | 152 | } ; |
JonFreeman | 11:bfb73f083009 | 153 | |
JonFreeman | 0:435bf84ce48a | 154 | const uint16_t B_tabl[] = { |
JonFreeman | 0:435bf84ce48a | 155 | 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake |
JonFreeman | 10:e40d8724268a | 156 | 0, BWHVL,BVHUL,BWHUL,BUHWL,BUHVL,BVHWL,BUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 |
JonFreeman | 10:e40d8724268a | 157 | 0, BVHWL,BUHVL,BUHWL,BWHUL,BVHUL,BWHVL,BWHUL, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1 |
JonFreeman | 8:93203f473f6e | 158 | 0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking |
JonFreeman | 4:21d91465e4b1 | 159 | KEEP_L_MASK_B, KEEP_H_MASK_B |
JonFreeman | 3:ecb00e0e8d68 | 160 | } ; |
JonFreeman | 0:435bf84ce48a | 161 | |
JonFreeman | 11:bfb73f083009 | 162 | brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK); |
JonFreeman | 11:bfb73f083009 | 163 | brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK); |
JonFreeman | 2:04761b196473 | 164 | |
JonFreeman | 5:ca86a7848d54 | 165 | |
JonFreeman | 8:93203f473f6e | 166 | void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise |
JonFreeman | 8:93203f473f6e | 167 | { |
JonFreeman | 8:93203f473f6e | 168 | 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 | 169 | } |
JonFreeman | 8:93203f473f6e | 170 | |
JonFreeman | 8:93203f473f6e | 171 | void setVI (double v, double i) |
JonFreeman | 8:93203f473f6e | 172 | { |
JonFreeman | 4:21d91465e4b1 | 173 | MotorA.set_V_limit (v); // Sets max motor voltage |
JonFreeman | 4:21d91465e4b1 | 174 | MotorA.set_I_limit (i); // Sets max motor current |
JonFreeman | 4:21d91465e4b1 | 175 | MotorB.set_V_limit (v); |
JonFreeman | 4:21d91465e4b1 | 176 | MotorB.set_I_limit (i); |
JonFreeman | 4:21d91465e4b1 | 177 | } |
JonFreeman | 11:bfb73f083009 | 178 | |
JonFreeman | 8:93203f473f6e | 179 | void setV (double v) |
JonFreeman | 8:93203f473f6e | 180 | { |
JonFreeman | 2:04761b196473 | 181 | MotorA.set_V_limit (v); |
JonFreeman | 4:21d91465e4b1 | 182 | MotorB.set_V_limit (v); |
JonFreeman | 4:21d91465e4b1 | 183 | } |
JonFreeman | 11:bfb73f083009 | 184 | |
JonFreeman | 8:93203f473f6e | 185 | void setI (double i) |
JonFreeman | 8:93203f473f6e | 186 | { |
JonFreeman | 2:04761b196473 | 187 | MotorA.set_I_limit (i); |
JonFreeman | 2:04761b196473 | 188 | MotorB.set_I_limit (i); |
JonFreeman | 0:435bf84ce48a | 189 | } |
JonFreeman | 2:04761b196473 | 190 | |
JonFreeman | 3:ecb00e0e8d68 | 191 | |
JonFreeman | 5:ca86a7848d54 | 192 | /** |
JonFreeman | 5:ca86a7848d54 | 193 | void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr |
JonFreeman | 5:ca86a7848d54 | 194 | Not part of ISR |
JonFreeman | 5:ca86a7848d54 | 195 | */ |
JonFreeman | 3:ecb00e0e8d68 | 196 | void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr |
JonFreeman | 0:435bf84ce48a | 197 | { |
JonFreeman | 11:bfb73f083009 | 198 | static uint32_t i = 0; |
JonFreeman | 8:93203f473f6e | 199 | // if (MotorA.dc_motor) { |
JonFreeman | 8:93203f473f6e | 200 | // MotorA.low_side_on (); |
JonFreeman | 8:93203f473f6e | 201 | // } |
JonFreeman | 8:93203f473f6e | 202 | // else { |
JonFreeman | 3:ecb00e0e8d68 | 203 | if (MotorA.tickleon) |
JonFreeman | 3:ecb00e0e8d68 | 204 | MotorA.high_side_off (); |
JonFreeman | 8:93203f473f6e | 205 | // } |
JonFreeman | 8:93203f473f6e | 206 | // if (MotorB.dc_motor) { |
JonFreeman | 8:93203f473f6e | 207 | // MotorB.low_side_on (); |
JonFreeman | 8:93203f473f6e | 208 | // } |
JonFreeman | 8:93203f473f6e | 209 | // else { |
JonFreeman | 3:ecb00e0e8d68 | 210 | if (MotorB.tickleon) |
JonFreeman | 3:ecb00e0e8d68 | 211 | MotorB.high_side_off (); |
JonFreeman | 8:93203f473f6e | 212 | // } |
JonFreeman | 0:435bf84ce48a | 213 | if (AtoD_Semaphore > 20) { |
JonFreeman | 8:93203f473f6e | 214 | pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore); |
JonFreeman | 0:435bf84ce48a | 215 | AtoD_Semaphore = 20; |
JonFreeman | 0:435bf84ce48a | 216 | } |
JonFreeman | 0:435bf84ce48a | 217 | while (AtoD_Semaphore > 0) { |
JonFreeman | 0:435bf84ce48a | 218 | AtoD_Semaphore--; |
JonFreeman | 0:435bf84ce48a | 219 | // Code here to sequence through reading voltmeter, demand pot, ammeters |
JonFreeman | 0:435bf84ce48a | 220 | switch (i) { // |
JonFreeman | 0:435bf84ce48a | 221 | case 0: |
JonFreeman | 0:435bf84ce48a | 222 | volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading |
JonFreeman | 0:435bf84ce48a | 223 | volt_reading >>= 1; // Result = Result / 2 |
JonFreeman | 0:435bf84ce48a | 224 | break; // i.e. Very simple digital low pass filter |
JonFreeman | 0:435bf84ce48a | 225 | case 1: |
JonFreeman | 0:435bf84ce48a | 226 | driverpot_reading += Ain_DriverPot.read_u16 (); |
JonFreeman | 0:435bf84ce48a | 227 | driverpot_reading >>= 1; |
JonFreeman | 0:435bf84ce48a | 228 | break; |
JonFreeman | 0:435bf84ce48a | 229 | case 2: |
JonFreeman | 11:bfb73f083009 | 230 | MotorA.sniff_current (); // Initiate ADC reading into averaging table |
JonFreeman | 0:435bf84ce48a | 231 | break; |
JonFreeman | 0:435bf84ce48a | 232 | case 3: |
JonFreeman | 11:bfb73f083009 | 233 | MotorB.sniff_current (); |
JonFreeman | 0:435bf84ce48a | 234 | break; |
JonFreeman | 0:435bf84ce48a | 235 | } |
JonFreeman | 0:435bf84ce48a | 236 | i++; // prepare to read the next input in response to the next interrupt |
JonFreeman | 0:435bf84ce48a | 237 | if (i > 3) |
JonFreeman | 0:435bf84ce48a | 238 | i = 0; |
JonFreeman | 3:ecb00e0e8d68 | 239 | } // end of while (AtoD_Semaphore > 0) { |
JonFreeman | 3:ecb00e0e8d68 | 240 | if (MotorA.tickleon) { |
JonFreeman | 8:93203f473f6e | 241 | // if (MotorA.dc_motor || MotorA.tickleon) { |
JonFreeman | 3:ecb00e0e8d68 | 242 | MotorA.tickleon--; |
JonFreeman | 5:ca86a7848d54 | 243 | MotorA.motor_set (); // Reactivate any high side switches turned off above |
JonFreeman | 3:ecb00e0e8d68 | 244 | } |
JonFreeman | 3:ecb00e0e8d68 | 245 | if (MotorB.tickleon) { |
JonFreeman | 8:93203f473f6e | 246 | // if (MotorB.dc_motor || MotorB.tickleon) { |
JonFreeman | 3:ecb00e0e8d68 | 247 | MotorB.tickleon--; |
JonFreeman | 3:ecb00e0e8d68 | 248 | MotorB.motor_set (); |
JonFreeman | 0:435bf84ce48a | 249 | } |
JonFreeman | 0:435bf84ce48a | 250 | } |
JonFreeman | 0:435bf84ce48a | 251 | |
JonFreeman | 10:e40d8724268a | 252 | /** double Read_Servo1_In () |
JonFreeman | 10:e40d8724268a | 253 | * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine |
JonFreeman | 10:e40d8724268a | 254 | * ISR also filters signal |
JonFreeman | 10:e40d8724268a | 255 | * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position |
JonFreeman | 10:e40d8724268a | 256 | */ |
JonFreeman | 10:e40d8724268a | 257 | double Read_Servo1_In () |
JonFreeman | 10:e40d8724268a | 258 | { |
JonFreeman | 10:e40d8724268a | 259 | const double xjoin = 0.5, |
JonFreeman | 10:e40d8724268a | 260 | yjoin = 0.35, |
JonFreeman | 10:e40d8724268a | 261 | slope_a = yjoin / xjoin, |
JonFreeman | 10:e40d8724268a | 262 | slope_b = (1.0 - yjoin)/(1.0 - xjoin); |
JonFreeman | 10:e40d8724268a | 263 | // centre = 0.35, // For pot, first (1/3)ish in braking area, position = 1/3 drift, > (1/3)ish drive |
JonFreeman | 10:e40d8724268a | 264 | // halfish = 0.5; // RC stick in the centre value |
JonFreeman | 10:e40d8724268a | 265 | // Bottom half of RC stick movement maps to lowest (1/3)ish pot movement |
JonFreeman | 10:e40d8724268a | 266 | // Higher half of RC stick movement maps to highest (2/3)ish pot movement |
JonFreeman | 10:e40d8724268a | 267 | double t; |
JonFreeman | 10:e40d8724268a | 268 | double demand = RC_chan_1.normalised(); |
JonFreeman | 10:e40d8724268a | 269 | // apply demand = 1.0 - demand here to swap stick move polarity |
JonFreeman | 10:e40d8724268a | 270 | // return pow (demand, SERVOIN_PWR_BENDER); |
JonFreeman | 10:e40d8724268a | 271 | if (demand < 0.0) demand = 0.0; |
JonFreeman | 10:e40d8724268a | 272 | if (demand > 1.0) demand = 1.0; |
JonFreeman | 10:e40d8724268a | 273 | if (demand < xjoin) { |
JonFreeman | 10:e40d8724268a | 274 | demand *= slope_a; |
JonFreeman | 10:e40d8724268a | 275 | } |
JonFreeman | 10:e40d8724268a | 276 | else { |
JonFreeman | 10:e40d8724268a | 277 | t = yjoin + ((demand - xjoin) * slope_b); |
JonFreeman | 10:e40d8724268a | 278 | demand = t; |
JonFreeman | 10:e40d8724268a | 279 | } |
JonFreeman | 10:e40d8724268a | 280 | return demand; |
JonFreeman | 10:e40d8724268a | 281 | } |
JonFreeman | 10:e40d8724268a | 282 | |
JonFreeman | 0:435bf84ce48a | 283 | /** double Read_DriverPot () |
JonFreeman | 0:435bf84ce48a | 284 | * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine |
JonFreeman | 10:e40d8724268a | 285 | * ISR also filters signal by returning average of most recent two readings |
JonFreeman | 0:435bf84ce48a | 286 | * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position |
JonFreeman | 0:435bf84ce48a | 287 | */ |
JonFreeman | 0:435bf84ce48a | 288 | double Read_DriverPot () |
JonFreeman | 0:435bf84ce48a | 289 | { |
JonFreeman | 5:ca86a7848d54 | 290 | return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0 |
JonFreeman | 0:435bf84ce48a | 291 | } |
JonFreeman | 0:435bf84ce48a | 292 | |
JonFreeman | 0:435bf84ce48a | 293 | double Read_BatteryVolts () |
JonFreeman | 0:435bf84ce48a | 294 | { |
JonFreeman | 5:ca86a7848d54 | 295 | return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct ! |
JonFreeman | 0:435bf84ce48a | 296 | } |
JonFreeman | 0:435bf84ce48a | 297 | |
JonFreeman | 8:93203f473f6e | 298 | void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb |
JonFreeman | 8:93203f473f6e | 299 | { |
JonFreeman | 2:04761b196473 | 300 | MotorA.set_mode (mode); |
JonFreeman | 2:04761b196473 | 301 | MotorB.set_mode (mode); |
JonFreeman | 2:04761b196473 | 302 | if (mode == REGENBRAKE) { |
JonFreeman | 5:ca86a7848d54 | 303 | if (val > 1.0) |
JonFreeman | 5:ca86a7848d54 | 304 | val = 1.0; |
JonFreeman | 5:ca86a7848d54 | 305 | if (val < 0.0) |
JonFreeman | 5:ca86a7848d54 | 306 | val = 0.0; |
JonFreeman | 5:ca86a7848d54 | 307 | val *= 0.9; // set upper limit, this is essential |
JonFreeman | 5:ca86a7848d54 | 308 | val = sqrt (val); // to linearise effect |
JonFreeman | 5:ca86a7848d54 | 309 | setVI (val, 1.0); |
JonFreeman | 2:04761b196473 | 310 | } |
JonFreeman | 2:04761b196473 | 311 | } |
JonFreeman | 0:435bf84ce48a | 312 | |
JonFreeman | 7:6deaeace9a3e | 313 | extern void setup_comms () ; |
JonFreeman | 7:6deaeace9a3e | 314 | extern void command_line_interpreter_pc () ; |
JonFreeman | 7:6deaeace9a3e | 315 | extern void command_line_interpreter_loco () ; |
JonFreeman | 0:435bf84ce48a | 316 | extern int check_24LC64 () ; // Call from near top of main() to init i2c bus |
JonFreeman | 0:435bf84ce48a | 317 | extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ; |
JonFreeman | 0:435bf84ce48a | 318 | extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ; |
JonFreeman | 0:435bf84ce48a | 319 | |
JonFreeman | 5:ca86a7848d54 | 320 | /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes |
JonFreeman | 3:ecb00e0e8d68 | 321 | uint8_t MotA_dir, // 0 or 1 |
JonFreeman | 3:ecb00e0e8d68 | 322 | MotB_dir, // 0 or 1 |
JonFreeman | 3:ecb00e0e8d68 | 323 | gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode |
JonFreeman | 3:ecb00e0e8d68 | 324 | serv1, // 0, 1, 2 = Not used, Input, Output |
JonFreeman | 3:ecb00e0e8d68 | 325 | serv2, // 0, 1, 2 = Not used, Input, Output |
JonFreeman | 3:ecb00e0e8d68 | 326 | cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2 |
JonFreeman | 3:ecb00e0e8d68 | 327 | last; |
JonFreeman | 3:ecb00e0e8d68 | 328 | } ; |
JonFreeman | 5:ca86a7848d54 | 329 | */ |
JonFreeman | 8:93203f473f6e | 330 | int I_Am () // Returns boards id number as ASCII char |
JonFreeman | 8:93203f473f6e | 331 | { |
JonFreeman | 5:ca86a7848d54 | 332 | return IAm; |
JonFreeman | 3:ecb00e0e8d68 | 333 | } |
JonFreeman | 3:ecb00e0e8d68 | 334 | |
JonFreeman | 8:93203f473f6e | 335 | double mph (int rpm) |
JonFreeman | 8:93203f473f6e | 336 | { |
JonFreeman | 7:6deaeace9a3e | 337 | if (mode_good_flag) { |
JonFreeman | 7:6deaeace9a3e | 338 | return rpm2mph * (double) rpm; |
JonFreeman | 7:6deaeace9a3e | 339 | } |
JonFreeman | 7:6deaeace9a3e | 340 | return -1.0; |
JonFreeman | 7:6deaeace9a3e | 341 | } |
JonFreeman | 4:21d91465e4b1 | 342 | |
JonFreeman | 8:93203f473f6e | 343 | void restorer () |
JonFreeman | 8:93203f473f6e | 344 | { |
JonFreeman | 8:93203f473f6e | 345 | motors_restore.detach (); |
JonFreeman | 8:93203f473f6e | 346 | if (MotorA.dc_motor) { |
JonFreeman | 8:93203f473f6e | 347 | MotorA.set_V_limit (MotorA.last_V); |
JonFreeman | 8:93203f473f6e | 348 | MotorA.set_I_limit (MotorA.last_I); |
JonFreeman | 8:93203f473f6e | 349 | MotorA.motor_set (); |
JonFreeman | 8:93203f473f6e | 350 | } |
JonFreeman | 8:93203f473f6e | 351 | if (MotorB.dc_motor) { |
JonFreeman | 8:93203f473f6e | 352 | MotorB.set_V_limit (MotorB.last_V); |
JonFreeman | 8:93203f473f6e | 353 | MotorB.set_I_limit (MotorB.last_I); |
JonFreeman | 8:93203f473f6e | 354 | MotorB.motor_set (); |
JonFreeman | 8:93203f473f6e | 355 | } |
JonFreeman | 8:93203f473f6e | 356 | } |
JonFreeman | 8:93203f473f6e | 357 | |
JonFreeman | 10:e40d8724268a | 358 | void rcin_report () { |
JonFreeman | 10:e40d8724268a | 359 | double c1 = RC_chan_1.normalised(); |
JonFreeman | 10:e40d8724268a | 360 | double c2 = RC_chan_2.normalised(); |
JonFreeman | 10:e40d8724268a | 361 | uint32_t pc1 = RC_chan_1.pulsecount(); |
JonFreeman | 10:e40d8724268a | 362 | uint32_t pc2 = RC_chan_2.pulsecount(); |
JonFreeman | 10:e40d8724268a | 363 | pc.printf ("At rcin_report, Ch1=%.3f, Ch2=%.3f, cnt1 %d, cnt2 %d\r\n", c1, c2, pc1, pc2); |
JonFreeman | 10:e40d8724268a | 364 | // if (c1 < -0.0001) |
JonFreeman | 10:e40d8724268a | 365 | pc.printf ("\t1 period %d, pulsewidth %d\r\n", RC_chan_1.period(), RC_chan_1.pulsewidth()); |
JonFreeman | 10:e40d8724268a | 366 | // if (c2 < -0.0001) |
JonFreeman | 10:e40d8724268a | 367 | pc.printf ("\t2 period %d, pulsewidth %d\r\n", RC_chan_2.period(), RC_chan_2.pulsewidth()); |
JonFreeman | 10:e40d8724268a | 368 | } |
JonFreeman | 8:93203f473f6e | 369 | |
JonFreeman | 11:bfb73f083009 | 370 | bool worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes |
JonFreeman | 10:e40d8724268a | 371 | double c = a - b; |
JonFreeman | 10:e40d8724268a | 372 | if (c < 0.0) |
JonFreeman | 10:e40d8724268a | 373 | c = 0.0 - c; |
JonFreeman | 10:e40d8724268a | 374 | if (c > 0.02) |
JonFreeman | 10:e40d8724268a | 375 | return true; |
JonFreeman | 10:e40d8724268a | 376 | return false; |
JonFreeman | 10:e40d8724268a | 377 | } |
JonFreeman | 10:e40d8724268a | 378 | |
JonFreeman | 10:e40d8724268a | 379 | void hand_control_state_machine (int source) { // if hand control mode '3', get here @ approx 30Hz to read drivers control pot |
JonFreeman | 10:e40d8724268a | 380 | // if servo1 mode '4', reads input from servo1 instead |
JonFreeman | 10:e40d8724268a | 381 | enum { // states used in hand_control_state_machine() |
JonFreeman | 10:e40d8724268a | 382 | HAND_CONT_IDLE, |
JonFreeman | 10:e40d8724268a | 383 | HAND_CONT_BRAKE_WAIT, |
JonFreeman | 10:e40d8724268a | 384 | HAND_CONT_BRAKE_POT, |
JonFreeman | 10:e40d8724268a | 385 | HAND_CONT_STATE_INTO_BRAKING, |
JonFreeman | 10:e40d8724268a | 386 | HAND_CONT_STATE_BRAKING, |
JonFreeman | 10:e40d8724268a | 387 | HAND_CONT_STATE_INTO_DRIVING, |
JonFreeman | 10:e40d8724268a | 388 | HAND_CONT_STATE_DRIVING |
JonFreeman | 10:e40d8724268a | 389 | } ; |
JonFreeman | 10:e40d8724268a | 390 | |
JonFreeman | 10:e40d8724268a | 391 | static int hand_controller_state = HAND_CONT_IDLE; |
JonFreeman | 10:e40d8724268a | 392 | // static int old_hand_controller_direction = T5; // Nov 2018 reworked thanks to feedback from Rob and Quentin |
JonFreeman | 8:93203f473f6e | 393 | static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0; |
JonFreeman | 10:e40d8724268a | 394 | static int dirbuf[5] = {100,100,100,100,100}; // Initialised with invalid direction such that 'change' is read from switch |
JonFreeman | 10:e40d8724268a | 395 | static int dirbufptr = 0, direction_old = -1, direction_new = -1; |
JonFreeman | 10:e40d8724268a | 396 | const int buflen = sizeof(dirbuf) / sizeof(int); |
JonFreeman | 10:e40d8724268a | 397 | const double Pot_Brake_Range = 0.35; //pow (0.5, SERVOIN_PWR_BENDER); //0.353553 for SERVOIN_PWR_BENDER = 1.5; |
JonFreeman | 10:e40d8724268a | 398 | |
JonFreeman | 10:e40d8724268a | 399 | direction_old = direction_new; |
JonFreeman | 10:e40d8724268a | 400 | |
JonFreeman | 10:e40d8724268a | 401 | // Test for change in direction switch setting. |
JonFreeman | 10:e40d8724268a | 402 | // If whole buffer NEWLY filled with all Fwd or all Rev, state = brake_wait |
JonFreeman | 10:e40d8724268a | 403 | int diracc = 0; |
JonFreeman | 10:e40d8724268a | 404 | if (dirbufptr >= buflen) |
JonFreeman | 10:e40d8724268a | 405 | dirbufptr = 0; |
JonFreeman | 10:e40d8724268a | 406 | dirbuf[dirbufptr++] = T5; // Read direction switch into circular debounce buffer |
JonFreeman | 10:e40d8724268a | 407 | for (int i = 0; i < buflen; i++) |
JonFreeman | 10:e40d8724268a | 408 | diracc += dirbuf[i]; // will = 0 or buflen if direction switch stable |
JonFreeman | 10:e40d8724268a | 409 | if (diracc == buflen || diracc == 0) // if was all 0 or all 1 |
JonFreeman | 10:e40d8724268a | 410 | direction_new = diracc / buflen; |
JonFreeman | 10:e40d8724268a | 411 | if (direction_new != direction_old) |
JonFreeman | 10:e40d8724268a | 412 | hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch |
JonFreeman | 10:e40d8724268a | 413 | |
JonFreeman | 10:e40d8724268a | 414 | switch (source) { |
JonFreeman | 10:e40d8724268a | 415 | case 3: // driver pot is control input |
JonFreeman | 10:e40d8724268a | 416 | pot_position = Read_DriverPot (); // Only place read in the loop, rteads normalised 0.0 to 1.0 |
JonFreeman | 10:e40d8724268a | 417 | break; |
JonFreeman | 10:e40d8724268a | 418 | case 4: // servo 1 input is control input |
JonFreeman | 10:e40d8724268a | 419 | break; |
JonFreeman | 10:e40d8724268a | 420 | default: |
JonFreeman | 10:e40d8724268a | 421 | pot_position = 0.0; |
JonFreeman | 10:e40d8724268a | 422 | break; |
JonFreeman | 8:93203f473f6e | 423 | } |
JonFreeman | 10:e40d8724268a | 424 | |
JonFreeman | 10:e40d8724268a | 425 | switch (hand_controller_state) { |
JonFreeman | 10:e40d8724268a | 426 | case HAND_CONT_IDLE: // Here for first few passes until validated direction switch reading |
JonFreeman | 10:e40d8724268a | 427 | break; |
JonFreeman | 10:e40d8724268a | 428 | |
JonFreeman | 11:bfb73f083009 | 429 | case HAND_CONT_BRAKE_WAIT: // Only get here after direction input changed or newly validated at power on |
JonFreeman | 10:e40d8724268a | 430 | pc.printf ("At HAND_CONT_BRAKE_WAIT\r\n"); |
JonFreeman | 10:e40d8724268a | 431 | brake_effort = 0.05; // Apply braking not too fiercely |
JonFreeman | 10:e40d8724268a | 432 | mode_set_both_motors (REGENBRAKE, brake_effort); // Direction change |
JonFreeman | 10:e40d8724268a | 433 | hand_controller_state = HAND_CONT_BRAKE_POT; |
JonFreeman | 10:e40d8724268a | 434 | break; |
JonFreeman | 10:e40d8724268a | 435 | |
JonFreeman | 11:bfb73f083009 | 436 | case HAND_CONT_BRAKE_POT: // Only get here after one pass through HAND_CONT_BRAKE_WAIT but |
JonFreeman | 11:bfb73f083009 | 437 | if (brake_effort < 0.9) { // remain in this state until driver has turned pott fully anticlockwise |
JonFreeman | 11:bfb73f083009 | 438 | brake_effort += 0.05; // ramp braking up to max over about one sec after direction change or validation |
JonFreeman | 11:bfb73f083009 | 439 | mode_set_both_motors (REGENBRAKE, brake_effort); // Direction change or testing at power on |
JonFreeman | 10:e40d8724268a | 440 | pc.printf ("Brake effort %.2f\r\n", brake_effort); |
JonFreeman | 10:e40d8724268a | 441 | } |
JonFreeman | 10:e40d8724268a | 442 | else { // once braking up to quite hard |
JonFreeman | 10:e40d8724268a | 443 | if (pot_position < 0.02) { // Driver has turned pot fully anticlock |
JonFreeman | 10:e40d8724268a | 444 | hand_controller_state = HAND_CONT_STATE_BRAKING; |
JonFreeman | 10:e40d8724268a | 445 | } |
JonFreeman | 8:93203f473f6e | 446 | } |
JonFreeman | 8:93203f473f6e | 447 | break; |
JonFreeman | 10:e40d8724268a | 448 | |
JonFreeman | 8:93203f473f6e | 449 | case HAND_CONT_STATE_INTO_BRAKING: |
JonFreeman | 8:93203f473f6e | 450 | brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; |
JonFreeman | 10:e40d8724268a | 451 | if (brake_effort < 0.0) |
JonFreeman | 10:e40d8724268a | 452 | brake_effort = 0.5; |
JonFreeman | 8:93203f473f6e | 453 | mode_set_both_motors (REGENBRAKE, brake_effort); |
JonFreeman | 8:93203f473f6e | 454 | old_pot_position = pot_position; |
JonFreeman | 10:e40d8724268a | 455 | hand_controller_state = HAND_CONT_STATE_BRAKING; |
JonFreeman | 8:93203f473f6e | 456 | pc.printf ("Brake\r\n"); |
JonFreeman | 8:93203f473f6e | 457 | break; |
JonFreeman | 10:e40d8724268a | 458 | |
JonFreeman | 8:93203f473f6e | 459 | case HAND_CONT_STATE_BRAKING: |
JonFreeman | 8:93203f473f6e | 460 | if (pot_position > Pot_Brake_Range) // escape braking into drive |
JonFreeman | 10:e40d8724268a | 461 | hand_controller_state = HAND_CONT_STATE_INTO_DRIVING; |
JonFreeman | 8:93203f473f6e | 462 | else { |
JonFreeman | 8:93203f473f6e | 463 | if (worth_the_bother(pot_position, old_pot_position)) { |
JonFreeman | 8:93203f473f6e | 464 | old_pot_position = pot_position; |
JonFreeman | 8:93203f473f6e | 465 | brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; |
JonFreeman | 8:93203f473f6e | 466 | mode_set_both_motors (REGENBRAKE, brake_effort); |
JonFreeman | 8:93203f473f6e | 467 | // pc.printf ("Brake %.2f\r\n", brake_effort); |
JonFreeman | 8:93203f473f6e | 468 | } |
JonFreeman | 8:93203f473f6e | 469 | } |
JonFreeman | 8:93203f473f6e | 470 | break; |
JonFreeman | 10:e40d8724268a | 471 | |
JonFreeman | 11:bfb73f083009 | 472 | case HAND_CONT_STATE_INTO_DRIVING: // Only get here after HAND_CONT_STATE_BRAKING |
JonFreeman | 10:e40d8724268a | 473 | pc.printf ("Drive\r\n"); |
JonFreeman | 10:e40d8724268a | 474 | if (direction_new == 1) |
JonFreeman | 10:e40d8724268a | 475 | mode_set_both_motors (FORWARD, 0.0); // sets both motors |
JonFreeman | 10:e40d8724268a | 476 | else |
JonFreeman | 10:e40d8724268a | 477 | mode_set_both_motors (REVERSE, 0.0); |
JonFreeman | 10:e40d8724268a | 478 | hand_controller_state = HAND_CONT_STATE_DRIVING; |
JonFreeman | 10:e40d8724268a | 479 | break; |
JonFreeman | 10:e40d8724268a | 480 | |
JonFreeman | 8:93203f473f6e | 481 | case HAND_CONT_STATE_DRIVING: |
JonFreeman | 8:93203f473f6e | 482 | if (pot_position < Pot_Brake_Range) // escape braking into drive |
JonFreeman | 10:e40d8724268a | 483 | hand_controller_state = HAND_CONT_STATE_INTO_BRAKING; |
JonFreeman | 8:93203f473f6e | 484 | else { |
JonFreeman | 8:93203f473f6e | 485 | if (worth_the_bother(pot_position, old_pot_position)) { |
JonFreeman | 8:93203f473f6e | 486 | old_pot_position = pot_position; |
JonFreeman | 8:93203f473f6e | 487 | drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range); |
JonFreeman | 8:93203f473f6e | 488 | setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ??? |
JonFreeman | 8:93203f473f6e | 489 | pc.printf ("Drive %.2f\r\n", drive_effort); |
JonFreeman | 8:93203f473f6e | 490 | } |
JonFreeman | 8:93203f473f6e | 491 | } |
JonFreeman | 8:93203f473f6e | 492 | break; |
JonFreeman | 10:e40d8724268a | 493 | |
JonFreeman | 8:93203f473f6e | 494 | default: |
JonFreeman | 10:e40d8724268a | 495 | pc.printf ("Unhandled Hand Controller state %d\r\n", hand_controller_state); |
JonFreeman | 8:93203f473f6e | 496 | break; |
JonFreeman | 8:93203f473f6e | 497 | } // endof switch (hand_controller_state) { |
JonFreeman | 8:93203f473f6e | 498 | } |
JonFreeman | 8:93203f473f6e | 499 | |
JonFreeman | 0:435bf84ce48a | 500 | int main() |
JonFreeman | 0:435bf84ce48a | 501 | { |
JonFreeman | 4:21d91465e4b1 | 502 | int eighth_sec_count = 0; |
JonFreeman | 10:e40d8724268a | 503 | double servo_angle = 0.0; // For testing servo outs |
JonFreeman | 0:435bf84ce48a | 504 | |
JonFreeman | 8:93203f473f6e | 505 | |
JonFreeman | 6:f289a49c1eae | 506 | Temperature_pin.fall (&temp_sensor_isr); |
JonFreeman | 6:f289a49c1eae | 507 | Temperature_pin.mode (PullUp); |
JonFreeman | 0:435bf84ce48a | 508 | // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc |
JonFreeman | 0:435bf84ce48a | 509 | tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator |
JonFreeman | 0:435bf84ce48a | 510 | loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator |
JonFreeman | 6:f289a49c1eae | 511 | temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960); |
JonFreeman | 0:435bf84ce48a | 512 | // Done setting up system interrupt timers |
JonFreeman | 6:f289a49c1eae | 513 | temperature_timer.start (); |
JonFreeman | 0:435bf84ce48a | 514 | |
JonFreeman | 3:ecb00e0e8d68 | 515 | pc.baud (9600); |
JonFreeman | 4:21d91465e4b1 | 516 | com3.baud (1200); |
JonFreeman | 4:21d91465e4b1 | 517 | com2.baud (19200); |
JonFreeman | 7:6deaeace9a3e | 518 | setup_comms (); |
JonFreeman | 6:f289a49c1eae | 519 | |
JonFreeman | 7:6deaeace9a3e | 520 | IAm = '0'; |
JonFreeman | 5:ca86a7848d54 | 521 | if (check_24LC64() != 0xa0) { // searches for i2c devices, returns address of highest found |
JonFreeman | 0:435bf84ce48a | 522 | pc.printf ("Check for 24LC64 eeprom FAILED\r\n"); |
JonFreeman | 5:ca86a7848d54 | 523 | com2.printf ("Check for 24LC64 eeprom FAILED\r\n"); |
JonFreeman | 7:6deaeace9a3e | 524 | eeprom_flag = false; |
JonFreeman | 8:93203f473f6e | 525 | } else { // Found 24LC64 memory on I2C |
JonFreeman | 7:6deaeace9a3e | 526 | eeprom_flag = true; |
JonFreeman | 5:ca86a7848d54 | 527 | bool k; |
JonFreeman | 6:f289a49c1eae | 528 | k = rd_24LC64 (0, mode_bytes, 32); |
JonFreeman | 5:ca86a7848d54 | 529 | if (!k) |
JonFreeman | 5:ca86a7848d54 | 530 | com2.printf ("Error reading from eeprom\r\n"); |
JonFreeman | 5:ca86a7848d54 | 531 | |
JonFreeman | 8:93203f473f6e | 532 | // int err = 0; |
JonFreeman | 6:f289a49c1eae | 533 | for (int i = 0; i < numof_eeprom_options; i++) { |
JonFreeman | 6:f289a49c1eae | 534 | if ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max)) { |
JonFreeman | 5:ca86a7848d54 | 535 | com2.printf ("EEROM error with %s\r\n", option_list[i].t); |
JonFreeman | 8:93203f473f6e | 536 | pc.printf ("EEROM error with %s\r\n", option_list[i].t); |
JonFreeman | 8:93203f473f6e | 537 | if (i == ID) { // need to force id to '0' |
JonFreeman | 8:93203f473f6e | 538 | pc.printf ("Bad board ID value %d, forcing to \'0\'\r\n"); |
JonFreeman | 8:93203f473f6e | 539 | mode_bytes[ID] = '0'; |
JonFreeman | 8:93203f473f6e | 540 | } |
JonFreeman | 8:93203f473f6e | 541 | // err++; |
JonFreeman | 5:ca86a7848d54 | 542 | } |
JonFreeman | 5:ca86a7848d54 | 543 | // else |
JonFreeman | 5:ca86a7848d54 | 544 | // com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t); |
JonFreeman | 5:ca86a7848d54 | 545 | } |
JonFreeman | 7:6deaeace9a3e | 546 | rpm2mph = 0.0; |
JonFreeman | 8:93203f473f6e | 547 | if (mode_bytes[WHEELGEAR] == 0) // avoid making div0 error |
JonFreeman | 8:93203f473f6e | 548 | mode_bytes[WHEELGEAR]++; |
JonFreeman | 8:93203f473f6e | 549 | // if (err == 0) { |
JonFreeman | 8:93203f473f6e | 550 | mode_good_flag = true; |
JonFreeman | 8:93203f473f6e | 551 | MotorA.direction_set (mode_bytes[MOTADIR]); |
JonFreeman | 8:93203f473f6e | 552 | MotorB.direction_set (mode_bytes[MOTBDIR]); |
JonFreeman | 8:93203f473f6e | 553 | IAm = mode_bytes[ID]; |
JonFreeman | 8:93203f473f6e | 554 | rpm2mph = 60.0 // to Motor Revs per hour; |
JonFreeman | 8:93203f473f6e | 555 | * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR]) // Wheel revs per hour |
JonFreeman | 8:93203f473f6e | 556 | * PI * ((double)mode_bytes[WHEELDIA] / 1000.0) // metres per hour |
JonFreeman | 8:93203f473f6e | 557 | * 39.37 / (1760.0 * 36.0); // miles per hour |
JonFreeman | 8:93203f473f6e | 558 | // } |
JonFreeman | 8:93203f473f6e | 559 | // Alternative ID 1 to 9 |
JonFreeman | 5:ca86a7848d54 | 560 | // com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]); |
JonFreeman | 5:ca86a7848d54 | 561 | } |
JonFreeman | 6:f289a49c1eae | 562 | // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor |
JonFreeman | 5:ca86a7848d54 | 563 | T2 = 0; // T2, T3, T4 As yet unused pins |
JonFreeman | 8:93203f473f6e | 564 | // T3 = 0; |
JonFreeman | 8:93203f473f6e | 565 | // T4 = 0; |
JonFreeman | 5:ca86a7848d54 | 566 | // T5 = 0; now input from fw/re on remote control box |
JonFreeman | 0:435bf84ce48a | 567 | T6 = 0; |
JonFreeman | 3:ecb00e0e8d68 | 568 | // MotPtr[0]->set_mode (REGENBRAKE); |
JonFreeman | 2:04761b196473 | 569 | MotorA.set_mode (REGENBRAKE); |
JonFreeman | 2:04761b196473 | 570 | MotorB.set_mode (REGENBRAKE); |
JonFreeman | 5:ca86a7848d54 | 571 | setVI (0.9, 0.5); |
JonFreeman | 5:ca86a7848d54 | 572 | |
JonFreeman | 6:f289a49c1eae | 573 | if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C |
JonFreeman | 6:f289a49c1eae | 574 | temp_sensor_exists = true; |
JonFreeman | 7:6deaeace9a3e | 575 | // pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR); |
JonFreeman | 8:93203f473f6e | 576 | dc_motor_kicker_timer.start (); |
JonFreeman | 8:93203f473f6e | 577 | int old_hand_controller_direction = T5; |
JonFreeman | 8:93203f473f6e | 578 | if (mode_bytes[COMM_SRC] == 3) { // Read fwd/rev switch 'T5', PA15 on 401 |
JonFreeman | 8:93203f473f6e | 579 | pc.printf ("Pot control\r\n"); |
JonFreeman | 8:93203f473f6e | 580 | if (T5) |
JonFreeman | 8:93203f473f6e | 581 | mode_set_both_motors (FORWARD, 0.0); // sets both motors |
JonFreeman | 8:93203f473f6e | 582 | else |
JonFreeman | 8:93203f473f6e | 583 | mode_set_both_motors (REVERSE, 0.0); |
JonFreeman | 8:93203f473f6e | 584 | } |
JonFreeman | 8:93203f473f6e | 585 | |
JonFreeman | 8:93203f473f6e | 586 | pc.printf ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]); |
JonFreeman | 8:93203f473f6e | 587 | |
JonFreeman | 0:435bf84ce48a | 588 | while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true |
JonFreeman | 0:435bf84ce48a | 589 | while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port |
JonFreeman | 7:6deaeace9a3e | 590 | command_line_interpreter_pc () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true |
JonFreeman | 7:6deaeace9a3e | 591 | command_line_interpreter_loco () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true |
JonFreeman | 0:435bf84ce48a | 592 | AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts |
JonFreeman | 11:bfb73f083009 | 593 | } // 32Hz original setting for loop repeat rate |
JonFreeman | 10:e40d8724268a | 594 | loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms |
JonFreeman | 10:e40d8724268a | 595 | |
JonFreeman | 10:e40d8724268a | 596 | RC_chan_1.validate_rx(); |
JonFreeman | 10:e40d8724268a | 597 | RC_chan_2.validate_rx(); |
JonFreeman | 10:e40d8724268a | 598 | |
JonFreeman | 8:93203f473f6e | 599 | switch (mode_bytes[COMM_SRC]) { // Look to selected source of driving command, act on commands from wherever |
JonFreeman | 8:93203f473f6e | 600 | case 0: // Invalid |
JonFreeman | 8:93203f473f6e | 601 | break; |
JonFreeman | 11:bfb73f083009 | 602 | case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter |
JonFreeman | 8:93203f473f6e | 603 | break; |
JonFreeman | 11:bfb73f083009 | 604 | case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter |
JonFreeman | 8:93203f473f6e | 605 | break; |
JonFreeman | 11:bfb73f083009 | 606 | case HAND: // Put all hand controller input stuff here |
JonFreeman | 10:e40d8724268a | 607 | hand_control_state_machine (3); |
JonFreeman | 8:93203f473f6e | 608 | break; // endof hand controller stuff |
JonFreeman | 11:bfb73f083009 | 609 | case RC_IN1: // RC_chan_1 |
JonFreeman | 10:e40d8724268a | 610 | hand_control_state_machine (4); |
JonFreeman | 8:93203f473f6e | 611 | break; |
JonFreeman | 11:bfb73f083009 | 612 | case RC_IN2: // RC_chan_2 |
JonFreeman | 8:93203f473f6e | 613 | break; |
JonFreeman | 8:93203f473f6e | 614 | default: |
JonFreeman | 11:bfb73f083009 | 615 | pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]); // set error flag instead here |
JonFreeman | 8:93203f473f6e | 616 | break; |
JonFreeman | 8:93203f473f6e | 617 | } // endof switch (mode_bytes[COMM_SRC]) { |
JonFreeman | 8:93203f473f6e | 618 | |
JonFreeman | 8:93203f473f6e | 619 | dc_motor_kicker_timer.reset (); |
JonFreeman | 5:ca86a7848d54 | 620 | MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second |
JonFreeman | 5:ca86a7848d54 | 621 | MotorB.pulses_per_sec (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM |
JonFreeman | 8:93203f473f6e | 622 | // T4 = !T4; // toggle to hang scope on to verify loop execution |
JonFreeman | 0:435bf84ce48a | 623 | // do stuff |
JonFreeman | 8:93203f473f6e | 624 | if (MotorA.dc_motor) { |
JonFreeman | 8:93203f473f6e | 625 | MotorA.raw_V_pwm (1); |
JonFreeman | 8:93203f473f6e | 626 | MotorA.low_side_on (); |
JonFreeman | 8:93203f473f6e | 627 | } |
JonFreeman | 8:93203f473f6e | 628 | if (MotorB.dc_motor) { |
JonFreeman | 8:93203f473f6e | 629 | MotorB.raw_V_pwm (1); |
JonFreeman | 8:93203f473f6e | 630 | MotorB.low_side_on (); |
JonFreeman | 8:93203f473f6e | 631 | } |
JonFreeman | 8:93203f473f6e | 632 | if (MotorA.dc_motor || MotorB.dc_motor) { |
JonFreeman | 8:93203f473f6e | 633 | // motors_restore.attach_us (&restorer, ttime); |
JonFreeman | 8:93203f473f6e | 634 | motors_restore.attach_us (&restorer, 25); |
JonFreeman | 8:93203f473f6e | 635 | } |
JonFreeman | 8:93203f473f6e | 636 | |
JonFreeman | 0:435bf84ce48a | 637 | if (flag_8Hz) { // do slower stuff |
JonFreeman | 0:435bf84ce48a | 638 | flag_8Hz = false; |
JonFreeman | 3:ecb00e0e8d68 | 639 | LED = !LED; // Toggle LED on board, should be seen to fast flash |
JonFreeman | 8:93203f473f6e | 640 | if (WatchDogEnable) { |
JonFreeman | 8:93203f473f6e | 641 | WatchDog--; |
JonFreeman | 8:93203f473f6e | 642 | if (WatchDog == 0) { // Deal with WatchDog timer timeout here |
JonFreeman | 8:93203f473f6e | 643 | setVI (0.0, 0.0); // set motor volts and amps to zero |
JonFreeman | 8:93203f473f6e | 644 | 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 | 645 | } // End of dealing with WatchDog timer timeout |
JonFreeman | 8:93203f473f6e | 646 | if (WatchDog < 0) |
JonFreeman | 8:93203f473f6e | 647 | WatchDog = 0; |
JonFreeman | 8:93203f473f6e | 648 | } |
JonFreeman | 4:21d91465e4b1 | 649 | eighth_sec_count++; |
JonFreeman | 4:21d91465e4b1 | 650 | if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts |
JonFreeman | 4:21d91465e4b1 | 651 | eighth_sec_count = 0; |
JonFreeman | 6:f289a49c1eae | 652 | MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max |
JonFreeman | 6:f289a49c1eae | 653 | MotorB.current_calc (); |
JonFreeman | 8:93203f473f6e | 654 | /* if (temp_sensor_exists) { |
JonFreeman | 8:93203f473f6e | 655 | double tmprt = (double) last_temp_count; |
JonFreeman | 8:93203f473f6e | 656 | tmprt /= 16.0; |
JonFreeman | 8:93203f473f6e | 657 | tmprt -= 50.0; |
JonFreeman | 8:93203f473f6e | 658 | pc.printf ("Temp %.2f\r\n", tmprt); |
JonFreeman | 8:93203f473f6e | 659 | }*/ |
JonFreeman | 5:ca86a7848d54 | 660 | // 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 | 11:bfb73f083009 | 661 | // com2.printf ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IAmin %d, IAave %d, IAmax %d, IB %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); |
JonFreeman | 0:435bf84ce48a | 662 | } |
JonFreeman | 11:bfb73f083009 | 663 | #define SERVO_OUT_TEST |
JonFreeman | 11:bfb73f083009 | 664 | #ifdef SERVO_OUT_TEST |
JonFreeman | 10:e40d8724268a | 665 | // servo out test here December 2018 |
JonFreeman | 10:e40d8724268a | 666 | servo_angle += 0.01; |
JonFreeman | 10:e40d8724268a | 667 | if (servo_angle > TWOPI) |
JonFreeman | 10:e40d8724268a | 668 | servo_angle -= TWOPI; |
JonFreeman | 10:e40d8724268a | 669 | Servo1 = ((sin(servo_angle)+1.0) / 2.0); |
JonFreeman | 10:e40d8724268a | 670 | Servo2 = ((cos(servo_angle)+1.0) / 2.0); |
JonFreeman | 10:e40d8724268a | 671 | // Yep! Both servo outs work lovely December 2018 |
JonFreeman | 11:bfb73f083009 | 672 | #endif |
JonFreeman | 0:435bf84ce48a | 673 | } // End of if(flag_8Hz) |
JonFreeman | 0:435bf84ce48a | 674 | } // End of main programme loop |
JonFreeman | 0:435bf84ce48a | 675 | } |
JonFreeman | 11:bfb73f083009 | 676 |