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@13:ef7a06fa11de, 2019-09-29 (annotated)

Committer:
JonFreeman
Date:
Sun Sep 29 16:34:37 2019 +0000
Revision:
13:ef7a06fa11de
Parent:
12:d1d21a2941ef
Child:
15:2591e2008323
Stable code as at end of 2019 running season

Who changed what in which revision?

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