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

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