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@10:e40d8724268a, 2019-01-15 (annotated)

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

Who changed what in which revision?

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