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@17:cc9b854295d6, 2020-08-16 (annotated)

Committer:
JonFreeman
Date:
Sun Aug 16 14:13:19 2020 +0000
Revision:
17:cc9b854295d6
Parent:
16:d1e4b9ad3b8b 
August 2020. Checked Radio Control input ops.

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 16:d1e4b9ad3b8b 4 2015 - 2020
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 16:d1e4b9ad3b8b 13 #include "LM75B.h" // New I2C temp sensor code March 2020 (to suit possible next board issue, harmless otherwise)
JonFreeman 16:d1e4b9ad3b8b 14 //#ifdef TARGET_NUCLEO_F401RE // Target is TARGET_NUCLEO_F401RE for all boards produced.
JonFreeman 13:ef7a06fa11de 15 //#endif
JonFreeman 5:ca86a7848d54 16 /*
JonFreeman 13:ef7a06fa11de 17 Brushless_STM3_ESC board
JonFreeman 16:d1e4b9ad3b8b 18 Apr 2020 * RC tested on 'Idle Halt' branch line - all good - also tested Inverter Gen power sorce. All good.
JonFreeman 16:d1e4b9ad3b8b 19 Dec 2019 * Radio control inputs now fully implemented, requires fitting tiny 'RC_in_fix' board.
JonFreeman 12:d1d21a2941ef 20 Jan 2019 * WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads
JonFreeman 12:d1d21a2941ef 21 * Tidied brushless_motor class, parameter passing now done properly
JonFreeman 12:d1d21a2941ef 22 * 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 23 (buggery board required for new inputs on June 2018 issue boards)
JonFreeman 12:d1d21a2941ef 24 * Added version string
JonFreeman 12:d1d21a2941ef 25 * Error handler written and included
JonFreeman 12:d1d21a2941ef 26 * 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 27 * Reorganised EEPROM data - mode setting now much easier, less error prone
JonFreeman 13:ef7a06fa11de 28 * Maximum speed now one EEPROM option, range 1.0 to 25.0 MPH in 0.1 MPH steps
JonFreeman 12:d1d21a2941ef 29
JonFreeman 10:e40d8724268a 30 New 29th May 2018 **
JonFreeman 10:e40d8724268a 31 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 32 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 33 significant loading of interrupts, threatening integrity of Real Time System
JonFreeman 16:d1e4b9ad3b8b 34 *** New sensor code for LM75B temp sensor added March 2020 ***
JonFreeman 5:ca86a7848d54 35 */
JonFreeman 5:ca86a7848d54 36
JonFreeman 5:ca86a7848d54 37
JonFreeman 0:435bf84ce48a 38 /* STM32F401RE - compile using NUCLEO-F401RE
JonFreeman 12:d1d21a2941ef 39 // PROJECT - STM3_ESC Dual Brushless Motor Controller - Jon Freeman June 2018.
JonFreeman 0:435bf84ce48a 40
JonFreeman 0:435bf84ce48a 41 AnalogIn to read each motor current
JonFreeman 0:435bf84ce48a 42
JonFreeman 0:435bf84ce48a 43 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 44 CONTROL PHILOSOPHY
JonFreeman 16:d1e4b9ad3b8b 45 This STM3_ESC Dual Brushless Motor drive board software should ensure sensible control when commands supplied are not sensible !
JonFreeman 0:435bf84ce48a 46
JonFreeman 0:435bf84ce48a 47 That is, smooth transition between Drive, Coast and Brake to be implemented here.
JonFreeman 0:435bf84ce48a 48 The remote controller must not be relied upon to deliver sensible command sequences.
JonFreeman 0:435bf84ce48a 49
JonFreeman 0:435bf84ce48a 50 So much the better if the remote controller does issue sensible commands, but ...
JonFreeman 0:435bf84ce48a 51
JonFreeman 0:435bf84ce48a 52 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 53
JonFreeman 0:435bf84ce48a 54
JonFreeman 0:435bf84ce48a 55 */
JonFreeman 8:93203f473f6e 56
JonFreeman 16:d1e4b9ad3b8b 57 //#if defined (TARGET_NUCLEO_L476RG) // CPU in 64 pin LQFP ** NOT PROVED ** No good, pinmap different
JonFreeman 16:d1e4b9ad3b8b 58 //#include "F401RE.h"
JonFreeman 16:d1e4b9ad3b8b 59 //#endif
JonFreeman 16:d1e4b9ad3b8b 60 #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP - This is what all produced boards have
JonFreeman 15:2591e2008323 61 #include "F401RE.h"
JonFreeman 6:f289a49c1eae 62 #endif
JonFreeman 16:d1e4b9ad3b8b 63 #if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP - Never tried, but probably would work as is
JonFreeman 15:2591e2008323 64 #include "F411RE.h"
JonFreeman 13:ef7a06fa11de 65 #endif
JonFreeman 7:6deaeace9a3e 66 #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP
JonFreeman 10:e40d8724268a 67 #include "F446ZE.h" // A thought for future version
JonFreeman 6:f289a49c1eae 68 #endif
JonFreeman 0:435bf84ce48a 69 /* Global variable declarations */
JonFreeman 16:d1e4b9ad3b8b 70
JonFreeman 16:d1e4b9ad3b8b 71 //volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
JonFreeman 16:d1e4b9ad3b8b 72 uint32_t WatchDog = 0, // Set this up in main once pre-flight checks done. Allow extra few seconds at powerup
JonFreeman 16:d1e4b9ad3b8b 73 volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 74 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 13:ef7a06fa11de 75 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US, 31250us at Sept 2019
JonFreeman 5:ca86a7848d54 76 AtoD_Semaphore = 0;
JonFreeman 12:d1d21a2941ef 77
JonFreeman 16:d1e4b9ad3b8b 78 bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable
JonFreeman 11:bfb73f083009 79 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 11:bfb73f083009 80 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 16:d1e4b9ad3b8b 81 bool temp_sensor_exists = false; // March 2020 - Now used to indicate presence or not of LM75B i2c temp sensor
JonFreeman 3:ecb00e0e8d68 82
JonFreeman 16:d1e4b9ad3b8b 83 /*
JonFreeman 16:d1e4b9ad3b8b 84 * Not used since LMT01 proved not a good choice. See LM75B code added March 2020
JonFreeman 16:d1e4b9ad3b8b 85 *
JonFreeman 13:ef7a06fa11de 86 #ifdef TEMP_SENSOR_ENABLE
JonFreeman 6:f289a49c1eae 87 uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
JonFreeman 12:d1d21a2941ef 88 last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
JonFreeman 6:f289a49c1eae 89 Ticker temperature_find_ticker;
JonFreeman 6:f289a49c1eae 90 Timer temperature_timer;
JonFreeman 13:ef7a06fa11de 91 #endif
JonFreeman 16:d1e4b9ad3b8b 92 */
JonFreeman 16:d1e4b9ad3b8b 93 /* End of Global variable declarations */
JonFreeman 12:d1d21a2941ef 94
JonFreeman 16:d1e4b9ad3b8b 95 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc - 50 us
JonFreeman 16:d1e4b9ad3b8b 96 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 16:d1e4b9ad3b8b 97
JonFreeman 16:d1e4b9ad3b8b 98 eeprom_settings user_settings (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup
JonFreeman 16:d1e4b9ad3b8b 99 RControl_In RC_chan_1 (PC_14);
JonFreeman 16:d1e4b9ad3b8b 100 RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin
JonFreeman 12:d1d21a2941ef 101 error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes.
JonFreeman 0:435bf84ce48a 102
JonFreeman 16:d1e4b9ad3b8b 103 //PCT2075 temp_sensor( i2c ); // or LM75B temp_sensor( p?, p? ); Added March 2020
JonFreeman 16:d1e4b9ad3b8b 104 PCT2075 temp_sensor( SDA_PIN, SCL_PIN ); // or LM75B temp_sensor( p?, p? ); Added March 2020
JonFreeman 4:21d91465e4b1 105
JonFreeman 16:d1e4b9ad3b8b 106
JonFreeman 0:435bf84ce48a 107 /*
JonFreeman 16:d1e4b9ad3b8b 108 5 1 3 2 6 4 Brushless Motor Hall SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 109
JonFreeman 3:ecb00e0e8d68 110 1 1 1 1 0 0 0 ---___---___ Hall1
JonFreeman 3:ecb00e0e8d68 111 2 0 0 1 1 1 0 __---___---_ Hall2
JonFreeman 3:ecb00e0e8d68 112 4 1 0 0 0 1 1 -___---___-- Hall3
JonFreeman 0:435bf84ce48a 113
JonFreeman 0:435bf84ce48a 114 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 8:93203f473f6e 115
JonFreeman 16:d1e4b9ad3b8b 116 Dec 2019 Support for DC motors deleted.
JonFreeman 16:d1e4b9ad3b8b 117 Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7.
JonFreeman 8:93203f473f6e 118
JonFreeman 0:435bf84ce48a 119 */
JonFreeman 16:d1e4b9ad3b8b 120 const uint16_t A_tabl[] = { // Table of motor energisation bit patterns. Rows are Handbrake, Forward, Reverse, Regen brake. Cols relate to Hall sensor outputs
JonFreeman 16:d1e4b9ad3b8b 121 // AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH
JonFreeman 16:d1e4b9ad3b8b 122 0, AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH, 0, // Handbrake
JonFreeman 16:d1e4b9ad3b8b 123 // 1->3 2->6 3->2 4->5 5->1 6->4
JonFreeman 16:d1e4b9ad3b8b 124 0, AWHVL, AVHUL, AWHUL, AUHWL, AUHVL, AVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,
JonFreeman 16:d1e4b9ad3b8b 125 // 1->5 2->3 3->1 4->6 5->4 6->2
JonFreeman 16:d1e4b9ad3b8b 126 0, AVHWL, AUHVL, AUHWL, AWHUL, AVHUL, AWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,
JonFreeman 16:d1e4b9ad3b8b 127 0, BRA, BRA, BRA, BRA, BRA, BRA, BRA, // Regenerative Braking
JonFreeman 4:21d91465e4b1 128 KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
JonFreeman 3:ecb00e0e8d68 129 } ;
JonFreeman 16:d1e4b9ad3b8b 130 // AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH
JonFreeman 16:d1e4b9ad3b8b 131 const uint16_t B_tabl[] = { // Different tables for Motors A and B as different ports and different port bits used.
JonFreeman 16:d1e4b9ad3b8b 132 // 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 16:d1e4b9ad3b8b 133 0, BUHVL|BWL, BWHUL|BVL, BWHVL|BUH, BVHWL|BUL, BUHWL|BVH, BVHUL|BWH, 0, // Handbrake
JonFreeman 16:d1e4b9ad3b8b 134 0, BWHVL, BVHUL, BWHUL, BUHWL, BUHVL, BVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,
JonFreeman 16:d1e4b9ad3b8b 135 0, BVHWL, BUHVL, BUHWL, BWHUL, BVHUL, BWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,
JonFreeman 16:d1e4b9ad3b8b 136 0, BRB, BRB, BRB, BRB, BRB, BRB, BRB, // Regenerative Braking
JonFreeman 4:21d91465e4b1 137 KEEP_L_MASK_B, KEEP_H_MASK_B
JonFreeman 3:ecb00e0e8d68 138 } ;
JonFreeman 0:435bf84ce48a 139
JonFreeman 12:d1d21a2941ef 140 brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA);
JonFreeman 16:d1e4b9ad3b8b 141 brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); // Two brushless motor instantiations
JonFreeman 5:ca86a7848d54 142
JonFreeman 12:d1d21a2941ef 143 extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen
JonFreeman 12:d1d21a2941ef 144
JonFreeman 16:d1e4b9ad3b8b 145 static const int LM75_I2C_ADDR = 0x90; // i2c temperature sensor
JonFreeman 16:d1e4b9ad3b8b 146
JonFreeman 12:d1d21a2941ef 147 // Interrupt Service Routines
JonFreeman 16:d1e4b9ad3b8b 148 /*
JonFreeman 13:ef7a06fa11de 149 #ifdef TEMP_SENSOR_ENABLE
JonFreeman 12:d1d21a2941ef 150 void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec
JonFreeman 12:d1d21a2941ef 151 {
JonFreeman 12:d1d21a2941ef 152 static bool readflag = false;
JonFreeman 12:d1d21a2941ef 153 int t = temperature_timer.read_ms ();
JonFreeman 12:d1d21a2941ef 154 if ((t == 5) && (!readflag)) {
JonFreeman 12:d1d21a2941ef 155 last_temperature_count = temp_sensor_count;
JonFreeman 12:d1d21a2941ef 156 temp_sensor_count = 0;
JonFreeman 12:d1d21a2941ef 157 readflag = true;
JonFreeman 12:d1d21a2941ef 158 }
JonFreeman 12:d1d21a2941ef 159 if (t == 6)
JonFreeman 12:d1d21a2941ef 160 readflag = false;
JonFreeman 12:d1d21a2941ef 161 }
JonFreeman 16:d1e4b9ad3b8b 162
JonFreeman 16:d1e4b9ad3b8b 163 void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some
JonFreeman 16:d1e4b9ad3b8b 164 {
JonFreeman 16:d1e4b9ad3b8b 165 int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
JonFreeman 16:d1e4b9ad3b8b 166 temperature_timer.reset ();
JonFreeman 16:d1e4b9ad3b8b 167 temp_sensor_count++;
JonFreeman 16:d1e4b9ad3b8b 168 if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
JonFreeman 16:d1e4b9ad3b8b 169 temp_sensor_count++;
JonFreeman 16:d1e4b9ad3b8b 170 // T2 = !T2; // scope hanger
JonFreeman 16:d1e4b9ad3b8b 171 }
JonFreeman 13:ef7a06fa11de 172 #endif
JonFreeman 16:d1e4b9ad3b8b 173 */
JonFreeman 16:d1e4b9ad3b8b 174
JonFreeman 12:d1d21a2941ef 175 /** void ISR_loop_timer ()
JonFreeman 12:d1d21a2941ef 176 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 12:d1d21a2941ef 177 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 12:d1d21a2941ef 178 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 12:d1d21a2941ef 179 */
JonFreeman 16:d1e4b9ad3b8b 180 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US (32Hz)
JonFreeman 8:93203f473f6e 181 {
JonFreeman 12:d1d21a2941ef 182 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 12:d1d21a2941ef 183 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 12:d1d21a2941ef 184 if ((sys_timer & 0x03) == 0)
JonFreeman 12:d1d21a2941ef 185 flag_8Hz = true;
JonFreeman 8:93203f473f6e 186 }
JonFreeman 8:93203f473f6e 187
JonFreeman 12:d1d21a2941ef 188 /** void ISR_voltage_reader ()
JonFreeman 12:d1d21a2941ef 189 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 12:d1d21a2941ef 190 *
JonFreeman 12:d1d21a2941ef 191 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 12:d1d21a2941ef 192 */
JonFreeman 12:d1d21a2941ef 193 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
JonFreeman 12:d1d21a2941ef 194 {
JonFreeman 12:d1d21a2941ef 195 AtoD_Semaphore++;
JonFreeman 16:d1e4b9ad3b8b 196 // fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 16:d1e4b9ad3b8b 197 }
JonFreeman 16:d1e4b9ad3b8b 198
JonFreeman 16:d1e4b9ad3b8b 199 // End of Interrupt Service Routines
JonFreeman 16:d1e4b9ad3b8b 200
JonFreeman 16:d1e4b9ad3b8b 201 const char * get_version () {
JonFreeman 16:d1e4b9ad3b8b 202 return "1.0.y2020.m05.d21\0"; // Version string, readable using 'ver' serial command
JonFreeman 12:d1d21a2941ef 203 }
JonFreeman 12:d1d21a2941ef 204
JonFreeman 17:cc9b854295d6 205 /*bool read_temperature (float & t) {
JonFreeman 16:d1e4b9ad3b8b 206 // pc.printf ("test param temp = %7.3f\r\n", t);
JonFreeman 16:d1e4b9ad3b8b 207 if (!temp_sensor_exists)
JonFreeman 16:d1e4b9ad3b8b 208 return false;
JonFreeman 16:d1e4b9ad3b8b 209 t = temp_sensor;
JonFreeman 16:d1e4b9ad3b8b 210 return true;
JonFreeman 17:cc9b854295d6 211 }*/
JonFreeman 12:d1d21a2941ef 212
JonFreeman 16:d1e4b9ad3b8b 213 void setVI_A (double v, double i)
JonFreeman 8:93203f473f6e 214 {
JonFreeman 4:21d91465e4b1 215 MotorA.set_V_limit (v); // Sets max motor voltage
JonFreeman 4:21d91465e4b1 216 MotorA.set_I_limit (i); // Sets max motor current
JonFreeman 16:d1e4b9ad3b8b 217 }
JonFreeman 16:d1e4b9ad3b8b 218
JonFreeman 16:d1e4b9ad3b8b 219 void setVI_B (double v, double i)
JonFreeman 16:d1e4b9ad3b8b 220 {
JonFreeman 4:21d91465e4b1 221 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 222 MotorB.set_I_limit (i);
JonFreeman 4:21d91465e4b1 223 }
JonFreeman 11:bfb73f083009 224
JonFreeman 16:d1e4b9ad3b8b 225 void setVI_both (double v, double i)
JonFreeman 16:d1e4b9ad3b8b 226 {
JonFreeman 16:d1e4b9ad3b8b 227 setVI_A (v, i);
JonFreeman 16:d1e4b9ad3b8b 228 setVI_B (v, i);
JonFreeman 16:d1e4b9ad3b8b 229 }
JonFreeman 16:d1e4b9ad3b8b 230
JonFreeman 3:ecb00e0e8d68 231
JonFreeman 5:ca86a7848d54 232 /**
JonFreeman 12:d1d21a2941ef 233 * void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 12:d1d21a2941ef 234 * Not part of ISR
JonFreeman 5:ca86a7848d54 235 */
JonFreeman 3:ecb00e0e8d68 236 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 0:435bf84ce48a 237 {
JonFreeman 11:bfb73f083009 238 static uint32_t i = 0;
JonFreeman 3:ecb00e0e8d68 239 if (MotorA.tickleon)
JonFreeman 3:ecb00e0e8d68 240 MotorA.high_side_off ();
JonFreeman 3:ecb00e0e8d68 241 if (MotorB.tickleon)
JonFreeman 3:ecb00e0e8d68 242 MotorB.high_side_off ();
JonFreeman 16:d1e4b9ad3b8b 243 if (AtoD_Semaphore > 10) {
JonFreeman 8:93203f473f6e 244 pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
JonFreeman 16:d1e4b9ad3b8b 245 AtoD_Semaphore = 10;
JonFreeman 0:435bf84ce48a 246 }
JonFreeman 16:d1e4b9ad3b8b 247 while (AtoD_Semaphore > 0) { // semaphore gets incremented in timer interrupt handler, t=50us
JonFreeman 0:435bf84ce48a 248 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 249 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 250 switch (i) { //
JonFreeman 0:435bf84ce48a 251 case 0:
JonFreeman 0:435bf84ce48a 252 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 253 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 254 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 255 case 1:
JonFreeman 0:435bf84ce48a 256 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 257 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 258 break;
JonFreeman 0:435bf84ce48a 259 case 2:
JonFreeman 12:d1d21a2941ef 260 MotorA.sniff_current (); // Initiate ADC current reading
JonFreeman 0:435bf84ce48a 261 break;
JonFreeman 0:435bf84ce48a 262 case 3:
JonFreeman 11:bfb73f083009 263 MotorB.sniff_current ();
JonFreeman 0:435bf84ce48a 264 break;
JonFreeman 0:435bf84ce48a 265 }
JonFreeman 0:435bf84ce48a 266 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 267 if (i > 3)
JonFreeman 0:435bf84ce48a 268 i = 0;
JonFreeman 3:ecb00e0e8d68 269 } // end of while (AtoD_Semaphore > 0) {
JonFreeman 3:ecb00e0e8d68 270 if (MotorA.tickleon) {
JonFreeman 3:ecb00e0e8d68 271 MotorA.tickleon--;
JonFreeman 5:ca86a7848d54 272 MotorA.motor_set (); // Reactivate any high side switches turned off above
JonFreeman 3:ecb00e0e8d68 273 }
JonFreeman 3:ecb00e0e8d68 274 if (MotorB.tickleon) {
JonFreeman 3:ecb00e0e8d68 275 MotorB.tickleon--;
JonFreeman 3:ecb00e0e8d68 276 MotorB.motor_set ();
JonFreeman 0:435bf84ce48a 277 }
JonFreeman 0:435bf84ce48a 278 }
JonFreeman 0:435bf84ce48a 279
JonFreeman 10:e40d8724268a 280
JonFreeman 0:435bf84ce48a 281 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 282 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 10:e40d8724268a 283 * ISR also filters signal by returning average of most recent two readings
JonFreeman 0:435bf84ce48a 284 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 285 */
JonFreeman 0:435bf84ce48a 286 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 287 {
JonFreeman 5:ca86a7848d54 288 return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 289 }
JonFreeman 0:435bf84ce48a 290
JonFreeman 0:435bf84ce48a 291 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 292 {
JonFreeman 5:ca86a7848d54 293 return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 294 }
JonFreeman 0:435bf84ce48a 295
JonFreeman 12:d1d21a2941ef 296
JonFreeman 16:d1e4b9ad3b8b 297 double trim (const double min, const double max, double input) {
JonFreeman 16:d1e4b9ad3b8b 298 if (input < min) input = min;
JonFreeman 16:d1e4b9ad3b8b 299 if (input > max) input = max;
JonFreeman 16:d1e4b9ad3b8b 300 return input;
JonFreeman 13:ef7a06fa11de 301 }
JonFreeman 13:ef7a06fa11de 302
JonFreeman 16:d1e4b9ad3b8b 303 void brake_motors_both (double brake_effort) {
JonFreeman 16:d1e4b9ad3b8b 304 MotorA.brake (brake_effort);
JonFreeman 16:d1e4b9ad3b8b 305 MotorB.brake (brake_effort);
JonFreeman 16:d1e4b9ad3b8b 306 }
JonFreeman 16:d1e4b9ad3b8b 307
JonFreeman 16:d1e4b9ad3b8b 308
JonFreeman 16:d1e4b9ad3b8b 309 void mode_set_motors_both (int mode) // called from cli to set fw, re, rb, hb
JonFreeman 8:93203f473f6e 310 {
JonFreeman 2:04761b196473 311 MotorA.set_mode (mode);
JonFreeman 2:04761b196473 312 MotorB.set_mode (mode);
JonFreeman 2:04761b196473 313 }
JonFreeman 0:435bf84ce48a 314
JonFreeman 12:d1d21a2941ef 315
JonFreeman 16:d1e4b9ad3b8b 316 bool is_it_worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes
JonFreeman 10:e40d8724268a 317 double c = a - b;
JonFreeman 10:e40d8724268a 318 if (c < 0.0)
JonFreeman 10:e40d8724268a 319 c = 0.0 - c;
JonFreeman 10:e40d8724268a 320 if (c > 0.02)
JonFreeman 10:e40d8724268a 321 return true;
JonFreeman 10:e40d8724268a 322 return false;
JonFreeman 10:e40d8724268a 323 }
JonFreeman 10:e40d8724268a 324
JonFreeman 16:d1e4b9ad3b8b 325 void hand_control_state_machine (int source) { // if hand control user_settings '3', get here @ approx 30Hz to read drivers control pot
JonFreeman 16:d1e4b9ad3b8b 326 // if servo1 user_settings '4', reads input from servo1 instead
JonFreeman 16:d1e4b9ad3b8b 327 enum { // states used in RC_or_hand_control_state_machine()
JonFreeman 10:e40d8724268a 328 HAND_CONT_IDLE,
JonFreeman 10:e40d8724268a 329 HAND_CONT_BRAKE_WAIT,
JonFreeman 10:e40d8724268a 330 HAND_CONT_BRAKE_POT,
JonFreeman 10:e40d8724268a 331 HAND_CONT_STATE_INTO_BRAKING,
JonFreeman 10:e40d8724268a 332 HAND_CONT_STATE_BRAKING,
JonFreeman 10:e40d8724268a 333 HAND_CONT_STATE_INTO_DRIVING,
JonFreeman 10:e40d8724268a 334 HAND_CONT_STATE_DRIVING
JonFreeman 10:e40d8724268a 335 } ;
JonFreeman 10:e40d8724268a 336
JonFreeman 10:e40d8724268a 337 static int hand_controller_state = HAND_CONT_IDLE;
JonFreeman 10:e40d8724268a 338 // static int old_hand_controller_direction = T5; // Nov 2018 reworked thanks to feedback from Rob and Quentin
JonFreeman 8:93203f473f6e 339 static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
JonFreeman 10:e40d8724268a 340 static int dirbuf[5] = {100,100,100,100,100}; // Initialised with invalid direction such that 'change' is read from switch
JonFreeman 10:e40d8724268a 341 static int dirbufptr = 0, direction_old = -1, direction_new = -1;
JonFreeman 10:e40d8724268a 342 const int buflen = sizeof(dirbuf) / sizeof(int);
JonFreeman 16:d1e4b9ad3b8b 343 double User_Brake_Range; //
JonFreeman 10:e40d8724268a 344
JonFreeman 13:ef7a06fa11de 345 direction_old = direction_new;
JonFreeman 10:e40d8724268a 346
JonFreeman 10:e40d8724268a 347 // Test for change in direction switch setting.
JonFreeman 10:e40d8724268a 348 // If whole buffer NEWLY filled with all Fwd or all Rev, state = brake_wait
JonFreeman 10:e40d8724268a 349 int diracc = 0;
JonFreeman 10:e40d8724268a 350 if (dirbufptr >= buflen)
JonFreeman 10:e40d8724268a 351 dirbufptr = 0;
JonFreeman 10:e40d8724268a 352 dirbuf[dirbufptr++] = T5; // Read direction switch into circular debounce buffer
JonFreeman 10:e40d8724268a 353 for (int i = 0; i < buflen; i++)
JonFreeman 10:e40d8724268a 354 diracc += dirbuf[i]; // will = 0 or buflen if direction switch stable
JonFreeman 10:e40d8724268a 355 if (diracc == buflen || diracc == 0) // if was all 0 or all 1
JonFreeman 10:e40d8724268a 356 direction_new = diracc / buflen;
JonFreeman 10:e40d8724268a 357 if (direction_new != direction_old)
JonFreeman 10:e40d8724268a 358 hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch
JonFreeman 10:e40d8724268a 359
JonFreeman 10:e40d8724268a 360 switch (source) {
JonFreeman 16:d1e4b9ad3b8b 361 case HAND: // driver pot is control input
JonFreeman 10:e40d8724268a 362 pot_position = Read_DriverPot (); // Only place read in the loop, rteads normalised 0.0 to 1.0
JonFreeman 16:d1e4b9ad3b8b 363 User_Brake_Range = user_settings.user_brake_range();
JonFreeman 10:e40d8724268a 364 break;
JonFreeman 10:e40d8724268a 365 default:
JonFreeman 10:e40d8724268a 366 pot_position = 0.0;
JonFreeman 10:e40d8724268a 367 break;
JonFreeman 8:93203f473f6e 368 }
JonFreeman 10:e40d8724268a 369
JonFreeman 10:e40d8724268a 370 switch (hand_controller_state) {
JonFreeman 10:e40d8724268a 371 case HAND_CONT_IDLE: // Here for first few passes until validated direction switch reading
JonFreeman 10:e40d8724268a 372 break;
JonFreeman 10:e40d8724268a 373
JonFreeman 11:bfb73f083009 374 case HAND_CONT_BRAKE_WAIT: // Only get here after direction input changed or newly validated at power on
JonFreeman 10:e40d8724268a 375 pc.printf ("At HAND_CONT_BRAKE_WAIT\r\n");
JonFreeman 10:e40d8724268a 376 brake_effort = 0.05; // Apply braking not too fiercely
JonFreeman 16:d1e4b9ad3b8b 377 brake_motors_both (brake_effort);
JonFreeman 10:e40d8724268a 378 hand_controller_state = HAND_CONT_BRAKE_POT;
JonFreeman 10:e40d8724268a 379 break;
JonFreeman 10:e40d8724268a 380
JonFreeman 11:bfb73f083009 381 case HAND_CONT_BRAKE_POT: // Only get here after one pass through HAND_CONT_BRAKE_WAIT but
JonFreeman 16:d1e4b9ad3b8b 382 if (brake_effort < 0.95) { // remain in this state until driver has turned pott fully anticlockwise
JonFreeman 11:bfb73f083009 383 brake_effort += 0.05; // ramp braking up to max over about one sec after direction change or validation
JonFreeman 16:d1e4b9ad3b8b 384 brake_motors_both (brake_effort); // Direction change or testing at power on
JonFreeman 10:e40d8724268a 385 pc.printf ("Brake effort %.2f\r\n", brake_effort);
JonFreeman 10:e40d8724268a 386 }
JonFreeman 10:e40d8724268a 387 else { // once braking up to quite hard
JonFreeman 10:e40d8724268a 388 if (pot_position < 0.02) { // Driver has turned pot fully anticlock
JonFreeman 10:e40d8724268a 389 hand_controller_state = HAND_CONT_STATE_BRAKING;
JonFreeman 10:e40d8724268a 390 }
JonFreeman 8:93203f473f6e 391 }
JonFreeman 8:93203f473f6e 392 break;
JonFreeman 10:e40d8724268a 393
JonFreeman 8:93203f473f6e 394 case HAND_CONT_STATE_INTO_BRAKING:
JonFreeman 16:d1e4b9ad3b8b 395 brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range;
JonFreeman 10:e40d8724268a 396 if (brake_effort < 0.0)
JonFreeman 10:e40d8724268a 397 brake_effort = 0.5;
JonFreeman 16:d1e4b9ad3b8b 398 brake_motors_both (brake_effort);
JonFreeman 8:93203f473f6e 399 old_pot_position = pot_position;
JonFreeman 10:e40d8724268a 400 hand_controller_state = HAND_CONT_STATE_BRAKING;
JonFreeman 8:93203f473f6e 401 pc.printf ("Brake\r\n");
JonFreeman 8:93203f473f6e 402 break;
JonFreeman 10:e40d8724268a 403
JonFreeman 8:93203f473f6e 404 case HAND_CONT_STATE_BRAKING:
JonFreeman 16:d1e4b9ad3b8b 405 if (pot_position > User_Brake_Range) // escape braking into drive
JonFreeman 10:e40d8724268a 406 hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
JonFreeman 8:93203f473f6e 407 else {
JonFreeman 16:d1e4b9ad3b8b 408 if (is_it_worth_the_bother(pot_position, old_pot_position)) {
JonFreeman 8:93203f473f6e 409 old_pot_position = pot_position;
JonFreeman 16:d1e4b9ad3b8b 410 brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range;
JonFreeman 16:d1e4b9ad3b8b 411 brake_motors_both (brake_effort);
JonFreeman 8:93203f473f6e 412 }
JonFreeman 8:93203f473f6e 413 }
JonFreeman 8:93203f473f6e 414 break;
JonFreeman 10:e40d8724268a 415
JonFreeman 11:bfb73f083009 416 case HAND_CONT_STATE_INTO_DRIVING: // Only get here after HAND_CONT_STATE_BRAKING
JonFreeman 10:e40d8724268a 417 pc.printf ("Drive\r\n");
JonFreeman 10:e40d8724268a 418 if (direction_new == 1)
JonFreeman 16:d1e4b9ad3b8b 419 mode_set_motors_both (MOTOR_FORWARD); // sets both motors
JonFreeman 10:e40d8724268a 420 else
JonFreeman 16:d1e4b9ad3b8b 421 mode_set_motors_both (MOTOR_REVERSE);
JonFreeman 10:e40d8724268a 422 hand_controller_state = HAND_CONT_STATE_DRIVING;
JonFreeman 10:e40d8724268a 423 break;
JonFreeman 10:e40d8724268a 424
JonFreeman 8:93203f473f6e 425 case HAND_CONT_STATE_DRIVING:
JonFreeman 16:d1e4b9ad3b8b 426 if (pot_position < User_Brake_Range) // escape braking into drive
JonFreeman 10:e40d8724268a 427 hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
JonFreeman 8:93203f473f6e 428 else {
JonFreeman 16:d1e4b9ad3b8b 429 if (is_it_worth_the_bother(pot_position, old_pot_position)) {
JonFreeman 8:93203f473f6e 430 old_pot_position = pot_position;
JonFreeman 16:d1e4b9ad3b8b 431 drive_effort = (pot_position - User_Brake_Range) / (1.0 - User_Brake_Range);
JonFreeman 16:d1e4b9ad3b8b 432 setVI_both (drive_effort, drive_effort); // Wind volts and amps up and down together ???
JonFreeman 8:93203f473f6e 433 pc.printf ("Drive %.2f\r\n", drive_effort);
JonFreeman 8:93203f473f6e 434 }
JonFreeman 8:93203f473f6e 435 }
JonFreeman 8:93203f473f6e 436 break;
JonFreeman 10:e40d8724268a 437
JonFreeman 8:93203f473f6e 438 default:
JonFreeman 10:e40d8724268a 439 pc.printf ("Unhandled Hand Controller state %d\r\n", hand_controller_state);
JonFreeman 8:93203f473f6e 440 break;
JonFreeman 8:93203f473f6e 441 } // endof switch (hand_controller_state) {
JonFreeman 8:93203f473f6e 442 }
JonFreeman 8:93203f473f6e 443
JonFreeman 16:d1e4b9ad3b8b 444 void set_RCIN_offsets () {
JonFreeman 16:d1e4b9ad3b8b 445 RC_chan_1.set_offset (user_settings.rd(RCI1_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK));
JonFreeman 16:d1e4b9ad3b8b 446 RC_chan_2.set_offset (user_settings.rd(RCI2_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK));
JonFreeman 16:d1e4b9ad3b8b 447 RC_chan_1.set_chanmode (user_settings.rd(RCIN1), user_settings.rd(RCIN1REVERSE)) ;
JonFreeman 16:d1e4b9ad3b8b 448 RC_chan_2.set_chanmode (user_settings.rd(RCIN2), user_settings.rd(RCIN2REVERSE)) ;
JonFreeman 16:d1e4b9ad3b8b 449 }
JonFreeman 16:d1e4b9ad3b8b 450
JonFreeman 17:cc9b854295d6 451 void rcins_report () {
JonFreeman 17:cc9b854295d6 452 pc.printf ("RC1 pulsewidth %d, period %d, pulsecount %d\r\n", RC_chan_1.pulsewidth(), RC_chan_1.period(), RC_chan_1.pulsecount());
JonFreeman 17:cc9b854295d6 453 pc.printf ("RC2 pulsewidth %d, period %d, pulsecount %d\r\n", RC_chan_2.pulsewidth(), RC_chan_2.period(), RC_chan_2.pulsecount());
JonFreeman 17:cc9b854295d6 454 pc.printf ("\r\n");
JonFreeman 17:cc9b854295d6 455 }
JonFreeman 16:d1e4b9ad3b8b 456
JonFreeman 16:d1e4b9ad3b8b 457 int main() // Programme entry point
JonFreeman 0:435bf84ce48a 458 {
JonFreeman 16:d1e4b9ad3b8b 459 uint32_t eighth_sec_count = 0;
JonFreeman 0:435bf84ce48a 460 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 16:d1e4b9ad3b8b 461 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator - 50 us
JonFreeman 0:435bf84ce48a 462 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 16:d1e4b9ad3b8b 463
JonFreeman 0:435bf84ce48a 464 // Done setting up system interrupt timers
JonFreeman 0:435bf84ce48a 465
JonFreeman 12:d1d21a2941ef 466 com3.baud (1200); // Once had an idea to use this for IR comms, never tried
JonFreeman 12:d1d21a2941ef 467 com2.baud (19200); // Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller
JonFreeman 16:d1e4b9ad3b8b 468 // pc.baud (9600); // COM port to pc
JonFreeman 16:d1e4b9ad3b8b 469 pc.baud (user_settings.baud()); // COM port to pc
JonFreeman 6:f289a49c1eae 470
JonFreeman 16:d1e4b9ad3b8b 471 // pc.printf ("Baud rate %d\r\n", user_settings.baud());
JonFreeman 5:ca86a7848d54 472
JonFreeman 16:d1e4b9ad3b8b 473 MotorA.set_direction (user_settings.rd(MOTADIR)); // user_settingss all setup in class eeprom_settings {} user_settings ; constructor
JonFreeman 16:d1e4b9ad3b8b 474 MotorB.set_direction (user_settings.rd(MOTBDIR));
JonFreeman 16:d1e4b9ad3b8b 475 MotorA.poles (user_settings.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise
JonFreeman 16:d1e4b9ad3b8b 476 MotorB.poles (user_settings.rd(MOTBPOLES)); // Only two calls are here
JonFreeman 16:d1e4b9ad3b8b 477 // MotorA.set_mode (MOTOR_REGENBRAKE); // Done in motor constructor
JonFreeman 16:d1e4b9ad3b8b 478 // MotorB.set_mode (MOTOR_REGENBRAKE);
JonFreeman 16:d1e4b9ad3b8b 479 // setVI_both (0.9, 0.5); // Power up with moderate regen braking applied
JonFreeman 16:d1e4b9ad3b8b 480 set_RCIN_offsets ();
JonFreeman 16:d1e4b9ad3b8b 481
JonFreeman 16:d1e4b9ad3b8b 482 class RC_stick_info RCstick1, RCstick2;
JonFreeman 12:d1d21a2941ef 483
JonFreeman 6:f289a49c1eae 484 // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
JonFreeman 5:ca86a7848d54 485 T2 = 0; // T2, T3, T4 As yet unused pins
JonFreeman 16:d1e4b9ad3b8b 486 // T3 = 0;// T4 = 0;// T5 = 0; now input from fw/re on remote control box
JonFreeman 0:435bf84ce48a 487 T6 = 0;
JonFreeman 16:d1e4b9ad3b8b 488 pc.printf ("\r\n\nSTM3_ESC Starting Ver %s, Command Source setting = %d\r\n", get_version(), user_settings.rd(COMM_SRC));
JonFreeman 16:d1e4b9ad3b8b 489 pc.printf ("Designed by Jon Freeman B. Eng. Hons - 2017-2020. e jon@jons-workshop.com\r\n");
JonFreeman 16:d1e4b9ad3b8b 490 if (user_settings.do_we_have_i2c (0xa0))
JonFreeman 16:d1e4b9ad3b8b 491 pc.printf ("Got EEPROM, i2c error count = %d\r\n", user_settings.errs());
JonFreeman 16:d1e4b9ad3b8b 492 else
JonFreeman 16:d1e4b9ad3b8b 493 pc.printf ("No eeprom found\r\n");
JonFreeman 16:d1e4b9ad3b8b 494 pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true - Ready to Roll !" : "false");
JonFreeman 5:ca86a7848d54 495
JonFreeman 16:d1e4b9ad3b8b 496 //bool eeprom_settings::do_we_have_i2c (uint32_t x)
JonFreeman 16:d1e4b9ad3b8b 497 pc.printf ("LM75 temp sensor ");
JonFreeman 16:d1e4b9ad3b8b 498 if (user_settings.do_we_have_i2c (LM75_I2C_ADDR)) {
JonFreeman 16:d1e4b9ad3b8b 499 pc.printf ("reports temperature %7.3f\r\n", (float)temp_sensor );
JonFreeman 6:f289a49c1eae 500 temp_sensor_exists = true;
JonFreeman 16:d1e4b9ad3b8b 501 }
JonFreeman 16:d1e4b9ad3b8b 502 else
JonFreeman 16:d1e4b9ad3b8b 503 pc.printf ("Not Fitted\r\n");
JonFreeman 16:d1e4b9ad3b8b 504
JonFreeman 16:d1e4b9ad3b8b 505 uint32_t old_hand_controller_direction = T5;
JonFreeman 16:d1e4b9ad3b8b 506 if (user_settings.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401
JonFreeman 8:93203f473f6e 507 pc.printf ("Pot control\r\n");
JonFreeman 8:93203f473f6e 508 if (T5)
JonFreeman 16:d1e4b9ad3b8b 509 mode_set_motors_both (MOTOR_FORWARD); // sets both motors
JonFreeman 8:93203f473f6e 510 else
JonFreeman 16:d1e4b9ad3b8b 511 mode_set_motors_both (MOTOR_REVERSE);
JonFreeman 8:93203f473f6e 512 }
JonFreeman 12:d1d21a2941ef 513 // pc.printf ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS);
JonFreeman 13:ef7a06fa11de 514
JonFreeman 13:ef7a06fa11de 515 WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
JonFreeman 16:d1e4b9ad3b8b 516 pcc.flush (); // Flush serial rx buffers
JonFreeman 16:d1e4b9ad3b8b 517 tsc.flush ();
JonFreeman 16:d1e4b9ad3b8b 518 // pc.printf ("sizeof int is %d\r\n", sizeof(int)); // sizeof int is 4
JonFreeman 16:d1e4b9ad3b8b 519 double Current_Scaler; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom.
JonFreeman 16:d1e4b9ad3b8b 520 // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I
JonFreeman 16:d1e4b9ad3b8b 521 // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage
JonFreeman 16:d1e4b9ad3b8b 522 // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning
JonFreeman 16:d1e4b9ad3b8b 523 // similar current flows for shorter times at higher voltages.
JonFreeman 13:ef7a06fa11de 524
JonFreeman 0:435bf84ce48a 525 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 526 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 12:d1d21a2941ef 527 pcc.core () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 12:d1d21a2941ef 528 tsc.core () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 529 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 11:bfb73f083009 530 } // 32Hz original setting for loop repeat rate
JonFreeman 10:e40d8724268a 531 loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms
JonFreeman 10:e40d8724268a 532
JonFreeman 16:d1e4b9ad3b8b 533 // double trim (const double min, const double max, double input) {
JonFreeman 16:d1e4b9ad3b8b 534 Current_Scaler = trim (0.1, 1.0, Read_BatteryVolts() / user_settings.Vnom());
JonFreeman 16:d1e4b9ad3b8b 535 MotorA.I_scale (Current_Scaler); // Reduces motor current limits when voltage below nominal.
JonFreeman 16:d1e4b9ad3b8b 536 MotorB.I_scale (Current_Scaler); // This goes some way towards preventing engine stalls perhaps
JonFreeman 16:d1e4b9ad3b8b 537
JonFreeman 16:d1e4b9ad3b8b 538 MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 16:d1e4b9ad3b8b 539 MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
JonFreeman 16:d1e4b9ad3b8b 540
JonFreeman 16:d1e4b9ad3b8b 541 RC_chan_1.read(RCstick1); // Get latest info from Radio Control inputs
JonFreeman 16:d1e4b9ad3b8b 542 RC_chan_2.read(RCstick2);
JonFreeman 16:d1e4b9ad3b8b 543
JonFreeman 16:d1e4b9ad3b8b 544 //#define SERVO_OUT_TEST
JonFreeman 16:d1e4b9ad3b8b 545 #ifdef SERVO_OUT_TEST
JonFreeman 16:d1e4b9ad3b8b 546 if (RCstick1.active) Servo1 = RCstick1.deflection;
JonFreeman 16:d1e4b9ad3b8b 547 if (RCstick2.active) Servo2 = RCstick2.deflection;
JonFreeman 16:d1e4b9ad3b8b 548 #endif
JonFreeman 16:d1e4b9ad3b8b 549
JonFreeman 16:d1e4b9ad3b8b 550 switch (user_settings.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever
JonFreeman 16:d1e4b9ad3b8b 551 // case 0: // Invalid
JonFreeman 16:d1e4b9ad3b8b 552 // break;
JonFreeman 16:d1e4b9ad3b8b 553 // case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter
JonFreeman 16:d1e4b9ad3b8b 554 // break;
JonFreeman 16:d1e4b9ad3b8b 555 case COM2: // COM2 - Nothing done here, all serial instructions handled in command line interpreter
JonFreeman 8:93203f473f6e 556 break;
JonFreeman 11:bfb73f083009 557 case HAND: // Put all hand controller input stuff here
JonFreeman 16:d1e4b9ad3b8b 558 hand_control_state_machine (HAND);
JonFreeman 8:93203f473f6e 559 break; // endof hand controller stuff
JonFreeman 11:bfb73f083009 560 case RC_IN1: // RC_chan_1
JonFreeman 16:d1e4b9ad3b8b 561 RC_chan_1.energise (RCstick1, MotorA) ; // RC chan 1 drives both motors
JonFreeman 16:d1e4b9ad3b8b 562 RC_chan_1.energise (RCstick1, MotorB) ;
JonFreeman 8:93203f473f6e 563 break;
JonFreeman 11:bfb73f083009 564 case RC_IN2: // RC_chan_2
JonFreeman 16:d1e4b9ad3b8b 565 RC_chan_2.energise (RCstick2, MotorA) ; // RC chan 2 drives both motors
JonFreeman 16:d1e4b9ad3b8b 566 RC_chan_2.energise (RCstick2, MotorB) ;
JonFreeman 16:d1e4b9ad3b8b 567 break;
JonFreeman 16:d1e4b9ad3b8b 568 case RC_IN_BOTH: // Robot Mode
JonFreeman 16:d1e4b9ad3b8b 569 RC_chan_1.energise (RCstick1, MotorA) ; // Two RC chans drive two motors independently
JonFreeman 16:d1e4b9ad3b8b 570 RC_chan_2.energise (RCstick2, MotorB) ;
JonFreeman 8:93203f473f6e 571 break;
JonFreeman 8:93203f473f6e 572 default:
JonFreeman 12:d1d21a2941ef 573 if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) {
JonFreeman 16:d1e4b9ad3b8b 574 pc.printf ("Unrecognised state %d\r\n", user_settings.rd(COMM_SRC)); // set error flag instead here
JonFreeman 12:d1d21a2941ef 575 ESC_Error.set (FAULT_UNRECOGNISED_STATE, 1);
JonFreeman 12:d1d21a2941ef 576 }
JonFreeman 8:93203f473f6e 577 break;
JonFreeman 16:d1e4b9ad3b8b 578 } // endof switch (user_settings_bytes[COMM_SRC]) {
JonFreeman 12:d1d21a2941ef 579
JonFreeman 16:d1e4b9ad3b8b 580 //#define SERVO_OUT_TEST
JonFreeman 16:d1e4b9ad3b8b 581 //#ifdef SERVO_OUT_TEST
JonFreeman 15:2591e2008323 582 // static double servo_angle = 0.0; // For testing servo outs
JonFreeman 15:2591e2008323 583 // servo out test here December 2018
JonFreeman 15:2591e2008323 584 /*
JonFreeman 15:2591e2008323 585 servo_angle += 0.01;
JonFreeman 15:2591e2008323 586 if (servo_angle > TWOPI)
JonFreeman 15:2591e2008323 587 servo_angle -= TWOPI;
JonFreeman 15:2591e2008323 588 Servo1 = ((sin(servo_angle)+1.0) / 2.0);
JonFreeman 15:2591e2008323 589 Servo2 = ((cos(servo_angle)+1.0) / 2.0);
JonFreeman 15:2591e2008323 590 */
JonFreeman 15:2591e2008323 591 // Yep! Both servo outs work lovely December 2018
JonFreeman 16:d1e4b9ad3b8b 592 //#endif
JonFreeman 8:93203f473f6e 593
JonFreeman 0:435bf84ce48a 594 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 595 flag_8Hz = false;
JonFreeman 16:d1e4b9ad3b8b 596 LED = !LED; // Toggle green LED on board, should be seen to fast flash
JonFreeman 8:93203f473f6e 597 if (WatchDogEnable) {
JonFreeman 8:93203f473f6e 598 WatchDog--;
JonFreeman 13:ef7a06fa11de 599 if (WatchDog < 1) { // Deal with WatchDog timer timeout here
JonFreeman 13:ef7a06fa11de 600 WatchDog = 0;
JonFreeman 16:d1e4b9ad3b8b 601 setVI_both (0.0, 0.0); // set motor volts and amps to zero
JonFreeman 16:d1e4b9ad3b8b 602 pc.printf ("TIMEOUT %c\r\n", user_settings.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this
JonFreeman 8:93203f473f6e 603 } // End of dealing with WatchDog timer timeout
JonFreeman 8:93203f473f6e 604 }
JonFreeman 4:21d91465e4b1 605 eighth_sec_count++;
JonFreeman 4:21d91465e4b1 606 if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 4:21d91465e4b1 607 eighth_sec_count = 0;
JonFreeman 12:d1d21a2941ef 608 ESC_Error.report_any (false); // retain = false - reset error after reporting once
JonFreeman 0:435bf84ce48a 609 }
JonFreeman 0:435bf84ce48a 610 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 611 } // End of main programme loop
JonFreeman 0:435bf84ce48a 612 }
JonFreeman 11:bfb73f083009 613