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@12:d1d21a2941ef, 2019-03-04 (annotated)

Committer:
JonFreeman
Date:
Mon Mar 04 17:51:08 2019 +0000
Revision:
12:d1d21a2941ef
Parent:
11:bfb73f083009
Child:
13:ef7a06fa11de
STM3 ESC dual motor controller boards. Always 'Work In Progress', working snapshot March 2019

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 12:d1d21a2941ef 15 Jan 2019 * WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads
JonFreeman 12:d1d21a2941ef 16 * Tidied brushless_motor class, parameter passing now done properly
JonFreeman 12:d1d21a2941ef 17 * class RControl_In created. Inputs now routed to new pins, can now use two chans both class RControl_In and Servo out
JonFreeman 12:d1d21a2941ef 18 (buggery board required for new inputs)
JonFreeman 12:d1d21a2941ef 19 * Added version string
JonFreeman 12:d1d21a2941ef 20 * Error handler written and included
JonFreeman 12:d1d21a2941ef 21 * Realised Nanotec motors are 6 pole, all others are 8 pole. Modified 'mode' to include 'motor poles' in EEPROM data, now speed reading correct for all
JonFreeman 12:d1d21a2941ef 22 * Reorganised EEPROM data - mode setting now much easier, less error prone
JonFreeman 12:d1d21a2941ef 23 * Maximum speed now one EEPROM option, range 1.0 to 25.0 MPH
JonFreeman 12:d1d21a2941ef 24
JonFreeman 10:e40d8724268a 25 New 29th May 2018 **
JonFreeman 10:e40d8724268a 26 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 27 */
JonFreeman 5:ca86a7848d54 28
JonFreeman 5:ca86a7848d54 29
JonFreeman 0:435bf84ce48a 30 /* STM32F401RE - compile using NUCLEO-F401RE
JonFreeman 12:d1d21a2941ef 31 // PROJECT - STM3_ESC Dual Brushless Motor Controller - Jon Freeman June 2018.
JonFreeman 0:435bf84ce48a 32
JonFreeman 0:435bf84ce48a 33 AnalogIn to read each motor current
JonFreeman 0:435bf84ce48a 34
JonFreeman 0:435bf84ce48a 35 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 36 CONTROL PHILOSOPHY
JonFreeman 0:435bf84ce48a 37 This Bogie drive board software should ensure sensible control when commands supplied are not sensible !
JonFreeman 0:435bf84ce48a 38
JonFreeman 0:435bf84ce48a 39 That is, smooth transition between Drive, Coast and Brake to be implemented here.
JonFreeman 0:435bf84ce48a 40 The remote controller must not be relied upon to deliver sensible command sequences.
JonFreeman 0:435bf84ce48a 41
JonFreeman 0:435bf84ce48a 42 So much the better if the remote controller does issue sensible commands, but ...
JonFreeman 0:435bf84ce48a 43
JonFreeman 0:435bf84ce48a 44 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 45
JonFreeman 0:435bf84ce48a 46
JonFreeman 0:435bf84ce48a 47 */
JonFreeman 8:93203f473f6e 48
JonFreeman 7:6deaeace9a3e 49 #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP
JonFreeman 9:ac2412df01be 50 #include "F401RE.h" // See here for warnings about Servo InterruptIn not working
JonFreeman 6:f289a49c1eae 51 #endif
JonFreeman 7:6deaeace9a3e 52 #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP
JonFreeman 10:e40d8724268a 53 #include "F446ZE.h" // A thought for future version
JonFreeman 6:f289a49c1eae 54 #endif
JonFreeman 0:435bf84ce48a 55 /* Global variable declarations */
JonFreeman 12:d1d21a2941ef 56 char const const_version_string[] = {"1.0.0\0"}; // Version string, readable from serial ports
JonFreeman 3:ecb00e0e8d68 57 volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
JonFreeman 5:ca86a7848d54 58 int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
JonFreeman 8:93203f473f6e 59 bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable
JonFreeman 0:435bf84ce48a 60 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 61 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 62 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 5:ca86a7848d54 63 AtoD_Semaphore = 0;
JonFreeman 12:d1d21a2941ef 64
JonFreeman 11:bfb73f083009 65 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 11:bfb73f083009 66 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 6:f289a49c1eae 67 bool temp_sensor_exists = false;
JonFreeman 12:d1d21a2941ef 68 double rpm2mph;
JonFreeman 3:ecb00e0e8d68 69
JonFreeman 6:f289a49c1eae 70 uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
JonFreeman 12:d1d21a2941ef 71 last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
JonFreeman 0:435bf84ce48a 72 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 73
JonFreeman 0:435bf84ce48a 74 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 75 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 6:f289a49c1eae 76 Ticker temperature_find_ticker;
JonFreeman 6:f289a49c1eae 77 Timer temperature_timer;
JonFreeman 12:d1d21a2941ef 78
JonFreeman 12:d1d21a2941ef 79 #ifdef USING_DC_MOTORS
JonFreeman 8:93203f473f6e 80 Timer dc_motor_kicker_timer;
JonFreeman 8:93203f473f6e 81 Timeout motors_restore;
JonFreeman 12:d1d21a2941ef 82 #endif
JonFreeman 12:d1d21a2941ef 83
JonFreeman 11:bfb73f083009 84 RControl_In RC_chan_1 (PC_14);
JonFreeman 11:bfb73f083009 85 RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin
JonFreeman 12:d1d21a2941ef 86 error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes.
JonFreeman 0:435bf84ce48a 87
JonFreeman 12:d1d21a2941ef 88 eeprom_settings mode (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup
JonFreeman 4:21d91465e4b1 89
JonFreeman 0:435bf84ce48a 90 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 91 /*
JonFreeman 0:435bf84ce48a 92 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 93
JonFreeman 3:ecb00e0e8d68 94 1 1 1 1 0 0 0 ---___---___ Hall1
JonFreeman 3:ecb00e0e8d68 95 2 0 0 1 1 1 0 __---___---_ Hall2
JonFreeman 3:ecb00e0e8d68 96 4 1 0 0 0 1 1 -___---___-- Hall3
JonFreeman 0:435bf84ce48a 97
JonFreeman 0:435bf84ce48a 98 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 8:93203f473f6e 99
JonFreeman 8:93203f473f6e 100 8th July 2018
JonFreeman 8:93203f473f6e 101 Added drive to DC brushed motors.
JonFreeman 8:93203f473f6e 102 Connect U and W to dc motor, leave V open.
JonFreeman 8:93203f473f6e 103
JonFreeman 8:93203f473f6e 104 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 105
JonFreeman 0:435bf84ce48a 106 */
JonFreeman 3:ecb00e0e8d68 107 const uint16_t A_tabl[] = { // Origial table
JonFreeman 0:435bf84ce48a 108 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 10:e40d8724268a 109 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 110 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 111 0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking
JonFreeman 4:21d91465e4b1 112 KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
JonFreeman 3:ecb00e0e8d68 113 } ;
JonFreeman 11:bfb73f083009 114
JonFreeman 0:435bf84ce48a 115 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 116 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 10:e40d8724268a 117 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 118 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 119 0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking
JonFreeman 4:21d91465e4b1 120 KEEP_L_MASK_B, KEEP_H_MASK_B
JonFreeman 3:ecb00e0e8d68 121 } ;
JonFreeman 0:435bf84ce48a 122
JonFreeman 12:d1d21a2941ef 123 brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA);
JonFreeman 12:d1d21a2941ef 124 brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB);
JonFreeman 2:04761b196473 125
JonFreeman 5:ca86a7848d54 126
JonFreeman 12:d1d21a2941ef 127 extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen
JonFreeman 12:d1d21a2941ef 128
JonFreeman 12:d1d21a2941ef 129 // Interrupt Service Routines
JonFreeman 12:d1d21a2941ef 130 void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec
JonFreeman 12:d1d21a2941ef 131 {
JonFreeman 12:d1d21a2941ef 132 static bool readflag = false;
JonFreeman 12:d1d21a2941ef 133 int t = temperature_timer.read_ms ();
JonFreeman 12:d1d21a2941ef 134 if ((t == 5) && (!readflag)) {
JonFreeman 12:d1d21a2941ef 135 last_temperature_count = temp_sensor_count;
JonFreeman 12:d1d21a2941ef 136 temp_sensor_count = 0;
JonFreeman 12:d1d21a2941ef 137 readflag = true;
JonFreeman 12:d1d21a2941ef 138 }
JonFreeman 12:d1d21a2941ef 139 if (t == 6)
JonFreeman 12:d1d21a2941ef 140 readflag = false;
JonFreeman 12:d1d21a2941ef 141 }
JonFreeman 12:d1d21a2941ef 142
JonFreeman 12:d1d21a2941ef 143 /** void ISR_loop_timer ()
JonFreeman 12:d1d21a2941ef 144 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 12:d1d21a2941ef 145 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 12:d1d21a2941ef 146 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 12:d1d21a2941ef 147 */
JonFreeman 12:d1d21a2941ef 148 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 8:93203f473f6e 149 {
JonFreeman 12:d1d21a2941ef 150 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 12:d1d21a2941ef 151 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 12:d1d21a2941ef 152 if ((sys_timer & 0x03) == 0)
JonFreeman 12:d1d21a2941ef 153 flag_8Hz = true;
JonFreeman 8:93203f473f6e 154 }
JonFreeman 8:93203f473f6e 155
JonFreeman 12:d1d21a2941ef 156 /** void ISR_voltage_reader ()
JonFreeman 12:d1d21a2941ef 157 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 12:d1d21a2941ef 158 *
JonFreeman 12:d1d21a2941ef 159 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 12:d1d21a2941ef 160 */
JonFreeman 12:d1d21a2941ef 161 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
JonFreeman 12:d1d21a2941ef 162 {
JonFreeman 12:d1d21a2941ef 163 AtoD_Semaphore++;
JonFreeman 12:d1d21a2941ef 164 fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 12:d1d21a2941ef 165 }
JonFreeman 12:d1d21a2941ef 166
JonFreeman 12:d1d21a2941ef 167 void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some
JonFreeman 12:d1d21a2941ef 168 {
JonFreeman 12:d1d21a2941ef 169 int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
JonFreeman 12:d1d21a2941ef 170 temperature_timer.reset ();
JonFreeman 12:d1d21a2941ef 171 temp_sensor_count++;
JonFreeman 12:d1d21a2941ef 172 if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
JonFreeman 12:d1d21a2941ef 173 temp_sensor_count++;
JonFreeman 12:d1d21a2941ef 174 // T2 = !T2; // scope hanger
JonFreeman 12:d1d21a2941ef 175 }
JonFreeman 12:d1d21a2941ef 176
JonFreeman 12:d1d21a2941ef 177 // End of Interrupt Service Routines
JonFreeman 12:d1d21a2941ef 178
JonFreeman 12:d1d21a2941ef 179
JonFreeman 8:93203f473f6e 180 void setVI (double v, double i)
JonFreeman 8:93203f473f6e 181 {
JonFreeman 4:21d91465e4b1 182 MotorA.set_V_limit (v); // Sets max motor voltage
JonFreeman 4:21d91465e4b1 183 MotorA.set_I_limit (i); // Sets max motor current
JonFreeman 4:21d91465e4b1 184 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 185 MotorB.set_I_limit (i);
JonFreeman 4:21d91465e4b1 186 }
JonFreeman 11:bfb73f083009 187
JonFreeman 3:ecb00e0e8d68 188
JonFreeman 5:ca86a7848d54 189 /**
JonFreeman 12:d1d21a2941ef 190 * void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 12:d1d21a2941ef 191 * Not part of ISR
JonFreeman 5:ca86a7848d54 192 */
JonFreeman 3:ecb00e0e8d68 193 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 0:435bf84ce48a 194 {
JonFreeman 11:bfb73f083009 195 static uint32_t i = 0;
JonFreeman 3:ecb00e0e8d68 196 if (MotorA.tickleon)
JonFreeman 3:ecb00e0e8d68 197 MotorA.high_side_off ();
JonFreeman 3:ecb00e0e8d68 198 if (MotorB.tickleon)
JonFreeman 3:ecb00e0e8d68 199 MotorB.high_side_off ();
JonFreeman 0:435bf84ce48a 200 if (AtoD_Semaphore > 20) {
JonFreeman 8:93203f473f6e 201 pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
JonFreeman 0:435bf84ce48a 202 AtoD_Semaphore = 20;
JonFreeman 0:435bf84ce48a 203 }
JonFreeman 0:435bf84ce48a 204 while (AtoD_Semaphore > 0) {
JonFreeman 0:435bf84ce48a 205 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 206 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 207 switch (i) { //
JonFreeman 0:435bf84ce48a 208 case 0:
JonFreeman 0:435bf84ce48a 209 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 210 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 211 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 212 case 1:
JonFreeman 0:435bf84ce48a 213 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 214 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 215 break;
JonFreeman 0:435bf84ce48a 216 case 2:
JonFreeman 12:d1d21a2941ef 217 MotorA.sniff_current (); // Initiate ADC current reading
JonFreeman 0:435bf84ce48a 218 break;
JonFreeman 0:435bf84ce48a 219 case 3:
JonFreeman 11:bfb73f083009 220 MotorB.sniff_current ();
JonFreeman 0:435bf84ce48a 221 break;
JonFreeman 0:435bf84ce48a 222 }
JonFreeman 0:435bf84ce48a 223 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 224 if (i > 3)
JonFreeman 0:435bf84ce48a 225 i = 0;
JonFreeman 3:ecb00e0e8d68 226 } // end of while (AtoD_Semaphore > 0) {
JonFreeman 3:ecb00e0e8d68 227 if (MotorA.tickleon) {
JonFreeman 3:ecb00e0e8d68 228 MotorA.tickleon--;
JonFreeman 5:ca86a7848d54 229 MotorA.motor_set (); // Reactivate any high side switches turned off above
JonFreeman 3:ecb00e0e8d68 230 }
JonFreeman 3:ecb00e0e8d68 231 if (MotorB.tickleon) {
JonFreeman 3:ecb00e0e8d68 232 MotorB.tickleon--;
JonFreeman 3:ecb00e0e8d68 233 MotorB.motor_set ();
JonFreeman 0:435bf84ce48a 234 }
JonFreeman 0:435bf84ce48a 235 }
JonFreeman 0:435bf84ce48a 236
JonFreeman 10:e40d8724268a 237 /** double Read_Servo1_In ()
JonFreeman 10:e40d8724268a 238 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 10:e40d8724268a 239 * ISR also filters signal
JonFreeman 10:e40d8724268a 240 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 10:e40d8724268a 241 */
JonFreeman 10:e40d8724268a 242 double Read_Servo1_In ()
JonFreeman 10:e40d8724268a 243 {
JonFreeman 10:e40d8724268a 244 const double xjoin = 0.5,
JonFreeman 10:e40d8724268a 245 yjoin = 0.35,
JonFreeman 10:e40d8724268a 246 slope_a = yjoin / xjoin,
JonFreeman 10:e40d8724268a 247 slope_b = (1.0 - yjoin)/(1.0 - xjoin);
JonFreeman 10:e40d8724268a 248 // centre = 0.35, // For pot, first (1/3)ish in braking area, position = 1/3 drift, > (1/3)ish drive
JonFreeman 10:e40d8724268a 249 // halfish = 0.5; // RC stick in the centre value
JonFreeman 10:e40d8724268a 250 // Bottom half of RC stick movement maps to lowest (1/3)ish pot movement
JonFreeman 10:e40d8724268a 251 // Higher half of RC stick movement maps to highest (2/3)ish pot movement
JonFreeman 10:e40d8724268a 252 double t;
JonFreeman 10:e40d8724268a 253 double demand = RC_chan_1.normalised();
JonFreeman 10:e40d8724268a 254 // apply demand = 1.0 - demand here to swap stick move polarity
JonFreeman 10:e40d8724268a 255 // return pow (demand, SERVOIN_PWR_BENDER);
JonFreeman 10:e40d8724268a 256 if (demand < 0.0) demand = 0.0;
JonFreeman 10:e40d8724268a 257 if (demand > 1.0) demand = 1.0;
JonFreeman 10:e40d8724268a 258 if (demand < xjoin) {
JonFreeman 10:e40d8724268a 259 demand *= slope_a;
JonFreeman 10:e40d8724268a 260 }
JonFreeman 10:e40d8724268a 261 else {
JonFreeman 10:e40d8724268a 262 t = yjoin + ((demand - xjoin) * slope_b);
JonFreeman 10:e40d8724268a 263 demand = t;
JonFreeman 10:e40d8724268a 264 }
JonFreeman 10:e40d8724268a 265 return demand;
JonFreeman 10:e40d8724268a 266 }
JonFreeman 10:e40d8724268a 267
JonFreeman 0:435bf84ce48a 268 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 269 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 10:e40d8724268a 270 * ISR also filters signal by returning average of most recent two readings
JonFreeman 0:435bf84ce48a 271 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 272 */
JonFreeman 0:435bf84ce48a 273 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 274 {
JonFreeman 5:ca86a7848d54 275 return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 276 }
JonFreeman 0:435bf84ce48a 277
JonFreeman 0:435bf84ce48a 278 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 279 {
JonFreeman 5:ca86a7848d54 280 return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 281 }
JonFreeman 0:435bf84ce48a 282
JonFreeman 12:d1d21a2941ef 283
JonFreeman 8:93203f473f6e 284 void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb
JonFreeman 8:93203f473f6e 285 {
JonFreeman 2:04761b196473 286 MotorA.set_mode (mode);
JonFreeman 2:04761b196473 287 MotorB.set_mode (mode);
JonFreeman 12:d1d21a2941ef 288 if (mode == MOTOR_REGENBRAKE) {
JonFreeman 5:ca86a7848d54 289 if (val > 1.0)
JonFreeman 5:ca86a7848d54 290 val = 1.0;
JonFreeman 5:ca86a7848d54 291 if (val < 0.0)
JonFreeman 5:ca86a7848d54 292 val = 0.0;
JonFreeman 5:ca86a7848d54 293 val *= 0.9; // set upper limit, this is essential
JonFreeman 5:ca86a7848d54 294 val = sqrt (val); // to linearise effect
JonFreeman 5:ca86a7848d54 295 setVI (val, 1.0);
JonFreeman 2:04761b196473 296 }
JonFreeman 2:04761b196473 297 }
JonFreeman 0:435bf84ce48a 298
JonFreeman 12:d1d21a2941ef 299
JonFreeman 0:435bf84ce48a 300
JonFreeman 12:d1d21a2941ef 301 #ifdef USING_DC_MOTORS
JonFreeman 8:93203f473f6e 302 void restorer ()
JonFreeman 8:93203f473f6e 303 {
JonFreeman 8:93203f473f6e 304 motors_restore.detach ();
JonFreeman 8:93203f473f6e 305 if (MotorA.dc_motor) {
JonFreeman 8:93203f473f6e 306 MotorA.set_V_limit (MotorA.last_V);
JonFreeman 8:93203f473f6e 307 MotorA.set_I_limit (MotorA.last_I);
JonFreeman 8:93203f473f6e 308 MotorA.motor_set ();
JonFreeman 8:93203f473f6e 309 }
JonFreeman 8:93203f473f6e 310 if (MotorB.dc_motor) {
JonFreeman 8:93203f473f6e 311 MotorB.set_V_limit (MotorB.last_V);
JonFreeman 8:93203f473f6e 312 MotorB.set_I_limit (MotorB.last_I);
JonFreeman 8:93203f473f6e 313 MotorB.motor_set ();
JonFreeman 8:93203f473f6e 314 }
JonFreeman 8:93203f473f6e 315 }
JonFreeman 12:d1d21a2941ef 316 #endif
JonFreeman 8:93203f473f6e 317
JonFreeman 10:e40d8724268a 318 void rcin_report () {
JonFreeman 10:e40d8724268a 319 double c1 = RC_chan_1.normalised();
JonFreeman 10:e40d8724268a 320 double c2 = RC_chan_2.normalised();
JonFreeman 10:e40d8724268a 321 uint32_t pc1 = RC_chan_1.pulsecount();
JonFreeman 10:e40d8724268a 322 uint32_t pc2 = RC_chan_2.pulsecount();
JonFreeman 10:e40d8724268a 323 pc.printf ("At rcin_report, Ch1=%.3f, Ch2=%.3f, cnt1 %d, cnt2 %d\r\n", c1, c2, pc1, pc2);
JonFreeman 10:e40d8724268a 324 // if (c1 < -0.0001)
JonFreeman 10:e40d8724268a 325 pc.printf ("\t1 period %d, pulsewidth %d\r\n", RC_chan_1.period(), RC_chan_1.pulsewidth());
JonFreeman 10:e40d8724268a 326 // if (c2 < -0.0001)
JonFreeman 10:e40d8724268a 327 pc.printf ("\t2 period %d, pulsewidth %d\r\n", RC_chan_2.period(), RC_chan_2.pulsewidth());
JonFreeman 10:e40d8724268a 328 }
JonFreeman 8:93203f473f6e 329
JonFreeman 11:bfb73f083009 330 bool worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes
JonFreeman 10:e40d8724268a 331 double c = a - b;
JonFreeman 10:e40d8724268a 332 if (c < 0.0)
JonFreeman 10:e40d8724268a 333 c = 0.0 - c;
JonFreeman 10:e40d8724268a 334 if (c > 0.02)
JonFreeman 10:e40d8724268a 335 return true;
JonFreeman 10:e40d8724268a 336 return false;
JonFreeman 10:e40d8724268a 337 }
JonFreeman 10:e40d8724268a 338
JonFreeman 10:e40d8724268a 339 void hand_control_state_machine (int source) { // if hand control mode '3', get here @ approx 30Hz to read drivers control pot
JonFreeman 10:e40d8724268a 340 // if servo1 mode '4', reads input from servo1 instead
JonFreeman 10:e40d8724268a 341 enum { // states used in hand_control_state_machine()
JonFreeman 10:e40d8724268a 342 HAND_CONT_IDLE,
JonFreeman 10:e40d8724268a 343 HAND_CONT_BRAKE_WAIT,
JonFreeman 10:e40d8724268a 344 HAND_CONT_BRAKE_POT,
JonFreeman 10:e40d8724268a 345 HAND_CONT_STATE_INTO_BRAKING,
JonFreeman 10:e40d8724268a 346 HAND_CONT_STATE_BRAKING,
JonFreeman 10:e40d8724268a 347 HAND_CONT_STATE_INTO_DRIVING,
JonFreeman 10:e40d8724268a 348 HAND_CONT_STATE_DRIVING
JonFreeman 10:e40d8724268a 349 } ;
JonFreeman 10:e40d8724268a 350
JonFreeman 10:e40d8724268a 351 static int hand_controller_state = HAND_CONT_IDLE;
JonFreeman 10:e40d8724268a 352 // static int old_hand_controller_direction = T5; // Nov 2018 reworked thanks to feedback from Rob and Quentin
JonFreeman 8:93203f473f6e 353 static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
JonFreeman 10:e40d8724268a 354 static int dirbuf[5] = {100,100,100,100,100}; // Initialised with invalid direction such that 'change' is read from switch
JonFreeman 10:e40d8724268a 355 static int dirbufptr = 0, direction_old = -1, direction_new = -1;
JonFreeman 10:e40d8724268a 356 const int buflen = sizeof(dirbuf) / sizeof(int);
JonFreeman 10:e40d8724268a 357 const double Pot_Brake_Range = 0.35; //pow (0.5, SERVOIN_PWR_BENDER); //0.353553 for SERVOIN_PWR_BENDER = 1.5;
JonFreeman 10:e40d8724268a 358
JonFreeman 10:e40d8724268a 359 direction_old = direction_new;
JonFreeman 10:e40d8724268a 360
JonFreeman 10:e40d8724268a 361 // Test for change in direction switch setting.
JonFreeman 10:e40d8724268a 362 // If whole buffer NEWLY filled with all Fwd or all Rev, state = brake_wait
JonFreeman 10:e40d8724268a 363 int diracc = 0;
JonFreeman 10:e40d8724268a 364 if (dirbufptr >= buflen)
JonFreeman 10:e40d8724268a 365 dirbufptr = 0;
JonFreeman 10:e40d8724268a 366 dirbuf[dirbufptr++] = T5; // Read direction switch into circular debounce buffer
JonFreeman 10:e40d8724268a 367 for (int i = 0; i < buflen; i++)
JonFreeman 10:e40d8724268a 368 diracc += dirbuf[i]; // will = 0 or buflen if direction switch stable
JonFreeman 10:e40d8724268a 369 if (diracc == buflen || diracc == 0) // if was all 0 or all 1
JonFreeman 10:e40d8724268a 370 direction_new = diracc / buflen;
JonFreeman 10:e40d8724268a 371 if (direction_new != direction_old)
JonFreeman 10:e40d8724268a 372 hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch
JonFreeman 10:e40d8724268a 373
JonFreeman 10:e40d8724268a 374 switch (source) {
JonFreeman 10:e40d8724268a 375 case 3: // driver pot is control input
JonFreeman 10:e40d8724268a 376 pot_position = Read_DriverPot (); // Only place read in the loop, rteads normalised 0.0 to 1.0
JonFreeman 10:e40d8724268a 377 break;
JonFreeman 10:e40d8724268a 378 case 4: // servo 1 input is control input
JonFreeman 10:e40d8724268a 379 break;
JonFreeman 10:e40d8724268a 380 default:
JonFreeman 10:e40d8724268a 381 pot_position = 0.0;
JonFreeman 10:e40d8724268a 382 break;
JonFreeman 8:93203f473f6e 383 }
JonFreeman 10:e40d8724268a 384
JonFreeman 10:e40d8724268a 385 switch (hand_controller_state) {
JonFreeman 10:e40d8724268a 386 case HAND_CONT_IDLE: // Here for first few passes until validated direction switch reading
JonFreeman 10:e40d8724268a 387 break;
JonFreeman 10:e40d8724268a 388
JonFreeman 11:bfb73f083009 389 case HAND_CONT_BRAKE_WAIT: // Only get here after direction input changed or newly validated at power on
JonFreeman 10:e40d8724268a 390 pc.printf ("At HAND_CONT_BRAKE_WAIT\r\n");
JonFreeman 10:e40d8724268a 391 brake_effort = 0.05; // Apply braking not too fiercely
JonFreeman 12:d1d21a2941ef 392 mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change
JonFreeman 10:e40d8724268a 393 hand_controller_state = HAND_CONT_BRAKE_POT;
JonFreeman 10:e40d8724268a 394 break;
JonFreeman 10:e40d8724268a 395
JonFreeman 11:bfb73f083009 396 case HAND_CONT_BRAKE_POT: // Only get here after one pass through HAND_CONT_BRAKE_WAIT but
JonFreeman 11:bfb73f083009 397 if (brake_effort < 0.9) { // remain in this state until driver has turned pott fully anticlockwise
JonFreeman 11:bfb73f083009 398 brake_effort += 0.05; // ramp braking up to max over about one sec after direction change or validation
JonFreeman 12:d1d21a2941ef 399 mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change or testing at power on
JonFreeman 10:e40d8724268a 400 pc.printf ("Brake effort %.2f\r\n", brake_effort);
JonFreeman 10:e40d8724268a 401 }
JonFreeman 10:e40d8724268a 402 else { // once braking up to quite hard
JonFreeman 10:e40d8724268a 403 if (pot_position < 0.02) { // Driver has turned pot fully anticlock
JonFreeman 10:e40d8724268a 404 hand_controller_state = HAND_CONT_STATE_BRAKING;
JonFreeman 10:e40d8724268a 405 }
JonFreeman 8:93203f473f6e 406 }
JonFreeman 8:93203f473f6e 407 break;
JonFreeman 10:e40d8724268a 408
JonFreeman 8:93203f473f6e 409 case HAND_CONT_STATE_INTO_BRAKING:
JonFreeman 8:93203f473f6e 410 brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
JonFreeman 10:e40d8724268a 411 if (brake_effort < 0.0)
JonFreeman 10:e40d8724268a 412 brake_effort = 0.5;
JonFreeman 12:d1d21a2941ef 413 mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort);
JonFreeman 8:93203f473f6e 414 old_pot_position = pot_position;
JonFreeman 10:e40d8724268a 415 hand_controller_state = HAND_CONT_STATE_BRAKING;
JonFreeman 8:93203f473f6e 416 pc.printf ("Brake\r\n");
JonFreeman 8:93203f473f6e 417 break;
JonFreeman 10:e40d8724268a 418
JonFreeman 8:93203f473f6e 419 case HAND_CONT_STATE_BRAKING:
JonFreeman 8:93203f473f6e 420 if (pot_position > Pot_Brake_Range) // escape braking into drive
JonFreeman 10:e40d8724268a 421 hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
JonFreeman 8:93203f473f6e 422 else {
JonFreeman 8:93203f473f6e 423 if (worth_the_bother(pot_position, old_pot_position)) {
JonFreeman 8:93203f473f6e 424 old_pot_position = pot_position;
JonFreeman 8:93203f473f6e 425 brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
JonFreeman 12:d1d21a2941ef 426 mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort);
JonFreeman 8:93203f473f6e 427 // pc.printf ("Brake %.2f\r\n", brake_effort);
JonFreeman 8:93203f473f6e 428 }
JonFreeman 8:93203f473f6e 429 }
JonFreeman 8:93203f473f6e 430 break;
JonFreeman 10:e40d8724268a 431
JonFreeman 11:bfb73f083009 432 case HAND_CONT_STATE_INTO_DRIVING: // Only get here after HAND_CONT_STATE_BRAKING
JonFreeman 10:e40d8724268a 433 pc.printf ("Drive\r\n");
JonFreeman 10:e40d8724268a 434 if (direction_new == 1)
JonFreeman 12:d1d21a2941ef 435 mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors
JonFreeman 10:e40d8724268a 436 else
JonFreeman 12:d1d21a2941ef 437 mode_set_both_motors (MOTOR_REVERSE, 0.0);
JonFreeman 10:e40d8724268a 438 hand_controller_state = HAND_CONT_STATE_DRIVING;
JonFreeman 10:e40d8724268a 439 break;
JonFreeman 10:e40d8724268a 440
JonFreeman 8:93203f473f6e 441 case HAND_CONT_STATE_DRIVING:
JonFreeman 8:93203f473f6e 442 if (pot_position < Pot_Brake_Range) // escape braking into drive
JonFreeman 10:e40d8724268a 443 hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
JonFreeman 8:93203f473f6e 444 else {
JonFreeman 8:93203f473f6e 445 if (worth_the_bother(pot_position, old_pot_position)) {
JonFreeman 8:93203f473f6e 446 old_pot_position = pot_position;
JonFreeman 8:93203f473f6e 447 drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range);
JonFreeman 8:93203f473f6e 448 setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ???
JonFreeman 8:93203f473f6e 449 pc.printf ("Drive %.2f\r\n", drive_effort);
JonFreeman 8:93203f473f6e 450 }
JonFreeman 8:93203f473f6e 451 }
JonFreeman 8:93203f473f6e 452 break;
JonFreeman 10:e40d8724268a 453
JonFreeman 8:93203f473f6e 454 default:
JonFreeman 10:e40d8724268a 455 pc.printf ("Unhandled Hand Controller state %d\r\n", hand_controller_state);
JonFreeman 8:93203f473f6e 456 break;
JonFreeman 8:93203f473f6e 457 } // endof switch (hand_controller_state) {
JonFreeman 8:93203f473f6e 458 }
JonFreeman 8:93203f473f6e 459
JonFreeman 0:435bf84ce48a 460 int main()
JonFreeman 0:435bf84ce48a 461 {
JonFreeman 4:21d91465e4b1 462 int eighth_sec_count = 0;
JonFreeman 10:e40d8724268a 463 double servo_angle = 0.0; // For testing servo outs
JonFreeman 0:435bf84ce48a 464
JonFreeman 6:f289a49c1eae 465 Temperature_pin.fall (&temp_sensor_isr);
JonFreeman 6:f289a49c1eae 466 Temperature_pin.mode (PullUp);
JonFreeman 0:435bf84ce48a 467 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 0:435bf84ce48a 468 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 469 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 6:f289a49c1eae 470 temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960);
JonFreeman 0:435bf84ce48a 471 // Done setting up system interrupt timers
JonFreeman 6:f289a49c1eae 472 temperature_timer.start ();
JonFreeman 0:435bf84ce48a 473
JonFreeman 12:d1d21a2941ef 474 pc.baud (9600); // COM port to pc
JonFreeman 12:d1d21a2941ef 475 com3.baud (1200); // Once had an idea to use this for IR comms, never tried
JonFreeman 12:d1d21a2941ef 476 com2.baud (19200); // Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller
JonFreeman 6:f289a49c1eae 477
JonFreeman 12:d1d21a2941ef 478 rpm2mph = 60.0 // to Motor Revs per hour;
JonFreeman 12:d1d21a2941ef 479 * ((double)mode.rd(MOTPIN) / (double)mode.rd(WHEELGEAR)) // Wheel revs per hour
JonFreeman 12:d1d21a2941ef 480 * PI * ((double)mode.rd(WHEELDIA) / 1000.0) // metres per hour
JonFreeman 12:d1d21a2941ef 481 * 39.37 / (1760.0 * 36.0); // miles per hour
JonFreeman 5:ca86a7848d54 482
JonFreeman 12:d1d21a2941ef 483 MotorA.direction_set (mode.rd(MOTADIR)); // modes all setup in class eeprom_settings {} mode ; constructor
JonFreeman 12:d1d21a2941ef 484 MotorB.direction_set (mode.rd(MOTBDIR));
JonFreeman 12:d1d21a2941ef 485 MotorA.poles (mode.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise
JonFreeman 12:d1d21a2941ef 486 MotorB.poles (mode.rd(MOTBPOLES)); // Only two calls are here
JonFreeman 12:d1d21a2941ef 487 MotorA.set_mode (MOTOR_REGENBRAKE);
JonFreeman 12:d1d21a2941ef 488 MotorB.set_mode (MOTOR_REGENBRAKE);
JonFreeman 12:d1d21a2941ef 489 setVI (0.9, 0.5); // Power up with moderate regen braking applied
JonFreeman 12:d1d21a2941ef 490
JonFreeman 6:f289a49c1eae 491 // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
JonFreeman 5:ca86a7848d54 492 T2 = 0; // T2, T3, T4 As yet unused pins
JonFreeman 8:93203f473f6e 493 // T3 = 0;
JonFreeman 8:93203f473f6e 494 // T4 = 0;
JonFreeman 5:ca86a7848d54 495 // T5 = 0; now input from fw/re on remote control box
JonFreeman 0:435bf84ce48a 496 T6 = 0;
JonFreeman 5:ca86a7848d54 497
JonFreeman 12:d1d21a2941ef 498 if ((last_temperature_count > 160) && (last_temperature_count < 2400)) // in range -40 to +100 degree C
JonFreeman 6:f289a49c1eae 499 temp_sensor_exists = true;
JonFreeman 12:d1d21a2941ef 500 #ifdef USING_DC_MOTORS
JonFreeman 8:93203f473f6e 501 dc_motor_kicker_timer.start ();
JonFreeman 12:d1d21a2941ef 502 #endif
JonFreeman 8:93203f473f6e 503 int old_hand_controller_direction = T5;
JonFreeman 12:d1d21a2941ef 504 if (mode.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401
JonFreeman 8:93203f473f6e 505 pc.printf ("Pot control\r\n");
JonFreeman 8:93203f473f6e 506 if (T5)
JonFreeman 12:d1d21a2941ef 507 mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors
JonFreeman 8:93203f473f6e 508 else
JonFreeman 12:d1d21a2941ef 509 mode_set_both_motors (MOTOR_REVERSE, 0.0);
JonFreeman 8:93203f473f6e 510 }
JonFreeman 8:93203f473f6e 511
JonFreeman 12:d1d21a2941ef 512 pc.printf ("About to start %s!, mode_bytes[COMM_SRC]= %d\r\n", const_version_string, mode.rd(COMM_SRC));
JonFreeman 12:d1d21a2941ef 513 pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true" : "false");
JonFreeman 12:d1d21a2941ef 514 // pc.printf ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS);
JonFreeman 12:d1d21a2941ef 515 // pcc.test () ;
JonFreeman 12:d1d21a2941ef 516 // tsc.test () ;
JonFreeman 0:435bf84ce48a 517 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 518 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 12:d1d21a2941ef 519 pcc.core () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 12:d1d21a2941ef 520 tsc.core () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 521 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 11:bfb73f083009 522 } // 32Hz original setting for loop repeat rate
JonFreeman 10:e40d8724268a 523 loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms
JonFreeman 10:e40d8724268a 524
JonFreeman 10:e40d8724268a 525 RC_chan_1.validate_rx();
JonFreeman 10:e40d8724268a 526 RC_chan_2.validate_rx();
JonFreeman 10:e40d8724268a 527
JonFreeman 12:d1d21a2941ef 528 switch (mode.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever
JonFreeman 8:93203f473f6e 529 case 0: // Invalid
JonFreeman 8:93203f473f6e 530 break;
JonFreeman 11:bfb73f083009 531 case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter
JonFreeman 8:93203f473f6e 532 break;
JonFreeman 11:bfb73f083009 533 case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter
JonFreeman 8:93203f473f6e 534 break;
JonFreeman 11:bfb73f083009 535 case HAND: // Put all hand controller input stuff here
JonFreeman 10:e40d8724268a 536 hand_control_state_machine (3);
JonFreeman 8:93203f473f6e 537 break; // endof hand controller stuff
JonFreeman 11:bfb73f083009 538 case RC_IN1: // RC_chan_1
JonFreeman 10:e40d8724268a 539 hand_control_state_machine (4);
JonFreeman 8:93203f473f6e 540 break;
JonFreeman 11:bfb73f083009 541 case RC_IN2: // RC_chan_2
JonFreeman 8:93203f473f6e 542 break;
JonFreeman 8:93203f473f6e 543 default:
JonFreeman 12:d1d21a2941ef 544 if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) {
JonFreeman 12:d1d21a2941ef 545 pc.printf ("Unrecognised state %d\r\n", mode.rd(COMM_SRC)); // set error flag instead here
JonFreeman 12:d1d21a2941ef 546 ESC_Error.set (FAULT_UNRECOGNISED_STATE, 1);
JonFreeman 12:d1d21a2941ef 547 }
JonFreeman 8:93203f473f6e 548 break;
JonFreeman 8:93203f473f6e 549 } // endof switch (mode_bytes[COMM_SRC]) {
JonFreeman 8:93203f473f6e 550
JonFreeman 12:d1d21a2941ef 551 #ifdef USING_DC_MOTORS
JonFreeman 8:93203f473f6e 552 dc_motor_kicker_timer.reset ();
JonFreeman 12:d1d21a2941ef 553 #endif
JonFreeman 12:d1d21a2941ef 554 MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 12:d1d21a2941ef 555 MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
JonFreeman 12:d1d21a2941ef 556
JonFreeman 12:d1d21a2941ef 557 #ifdef USING_DC_MOTORS
JonFreeman 8:93203f473f6e 558 if (MotorA.dc_motor) {
JonFreeman 12:d1d21a2941ef 559 // MotorA.raw_V_pwm (1);
JonFreeman 8:93203f473f6e 560 MotorA.low_side_on ();
JonFreeman 8:93203f473f6e 561 }
JonFreeman 8:93203f473f6e 562 if (MotorB.dc_motor) {
JonFreeman 12:d1d21a2941ef 563 // MotorB.raw_V_pwm (1);
JonFreeman 8:93203f473f6e 564 MotorB.low_side_on ();
JonFreeman 8:93203f473f6e 565 }
JonFreeman 8:93203f473f6e 566 if (MotorA.dc_motor || MotorB.dc_motor) {
JonFreeman 8:93203f473f6e 567 // motors_restore.attach_us (&restorer, ttime);
JonFreeman 8:93203f473f6e 568 motors_restore.attach_us (&restorer, 25);
JonFreeman 8:93203f473f6e 569 }
JonFreeman 12:d1d21a2941ef 570 #endif
JonFreeman 8:93203f473f6e 571
JonFreeman 0:435bf84ce48a 572 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 573 flag_8Hz = false;
JonFreeman 3:ecb00e0e8d68 574 LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 8:93203f473f6e 575 if (WatchDogEnable) {
JonFreeman 8:93203f473f6e 576 WatchDog--;
JonFreeman 8:93203f473f6e 577 if (WatchDog == 0) { // Deal with WatchDog timer timeout here
JonFreeman 8:93203f473f6e 578 setVI (0.0, 0.0); // set motor volts and amps to zero
JonFreeman 12:d1d21a2941ef 579 // com2.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
JonFreeman 12:d1d21a2941ef 580 pc.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this
JonFreeman 8:93203f473f6e 581 } // End of dealing with WatchDog timer timeout
JonFreeman 8:93203f473f6e 582 if (WatchDog < 0)
JonFreeman 8:93203f473f6e 583 WatchDog = 0;
JonFreeman 8:93203f473f6e 584 }
JonFreeman 4:21d91465e4b1 585 eighth_sec_count++;
JonFreeman 4:21d91465e4b1 586 if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 4:21d91465e4b1 587 eighth_sec_count = 0;
JonFreeman 12:d1d21a2941ef 588 ESC_Error.report_any (false); // retain = false - reset error after reporting once
JonFreeman 8:93203f473f6e 589 /* if (temp_sensor_exists) {
JonFreeman 8:93203f473f6e 590 double tmprt = (double) last_temp_count;
JonFreeman 8:93203f473f6e 591 tmprt /= 16.0;
JonFreeman 8:93203f473f6e 592 tmprt -= 50.0;
JonFreeman 8:93203f473f6e 593 pc.printf ("Temp %.2f\r\n", tmprt);
JonFreeman 8:93203f473f6e 594 }*/
JonFreeman 0:435bf84ce48a 595 }
JonFreeman 11:bfb73f083009 596 #define SERVO_OUT_TEST
JonFreeman 11:bfb73f083009 597 #ifdef SERVO_OUT_TEST
JonFreeman 10:e40d8724268a 598 // servo out test here December 2018
JonFreeman 10:e40d8724268a 599 servo_angle += 0.01;
JonFreeman 10:e40d8724268a 600 if (servo_angle > TWOPI)
JonFreeman 10:e40d8724268a 601 servo_angle -= TWOPI;
JonFreeman 10:e40d8724268a 602 Servo1 = ((sin(servo_angle)+1.0) / 2.0);
JonFreeman 10:e40d8724268a 603 Servo2 = ((cos(servo_angle)+1.0) / 2.0);
JonFreeman 10:e40d8724268a 604 // Yep! Both servo outs work lovely December 2018
JonFreeman 11:bfb73f083009 605 #endif
JonFreeman 0:435bf84ce48a 606 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 607 } // End of main programme loop
JonFreeman 0:435bf84ce48a 608 }
JonFreeman 11:bfb73f083009 609