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@6:f289a49c1eae, 2018-06-05 (annotated)

Committer:
JonFreeman
Date:
Tue Jun 05 07:19:39 2018 +0000
Revision:
6:f289a49c1eae
Parent:
5:ca86a7848d54
Child:
7:6deaeace9a3e
Migrating towards code for both STM32F401RET (64 pin) and STM32F446ZET7 (144 pin). Should resolve IO conflicts for larger device - getting servo ins and outs working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:435bf84ce48a 1 #include "mbed.h"
JonFreeman 0:435bf84ce48a 2 #include "DualBLS.h"
JonFreeman 0:435bf84ce48a 3 #include "BufferedSerial.h"
JonFreeman 0:435bf84ce48a 4 #include "FastPWM.h"
JonFreeman 4:21d91465e4b1 5 #include "Servo.h"
JonFreeman 5:ca86a7848d54 6
JonFreeman 5:ca86a7848d54 7 /*
JonFreeman 5:ca86a7848d54 8 New 29th May 2018 - YET TO CODE FOR - Fwd/Rev line from possible remote hand control box has signal routed to T5
JonFreeman 6:f289a49c1eae 9 Also new LMT01 temperature sensor routed to T1 - and rerouted to PC_13 at InterruptIn on T1 (ports A and B I think) not workable
JonFreeman 5:ca86a7848d54 10 */
JonFreeman 5:ca86a7848d54 11
JonFreeman 5:ca86a7848d54 12
JonFreeman 0:435bf84ce48a 13 /* STM32F401RE - compile using NUCLEO-F401RE
JonFreeman 6:f289a49c1eae 14 // PROJECT - Dual Brushless Motor Controller - Jon Freeman June 2018.
JonFreeman 0:435bf84ce48a 15
JonFreeman 0:435bf84ce48a 16 AnalogIn to read each motor current
JonFreeman 0:435bf84ce48a 17
JonFreeman 0:435bf84ce48a 18 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 19 CONTROL PHILOSOPHY
JonFreeman 0:435bf84ce48a 20 This Bogie drive board software should ensure sensible control when commands supplied are not sensible !
JonFreeman 0:435bf84ce48a 21
JonFreeman 0:435bf84ce48a 22 That is, smooth transition between Drive, Coast and Brake to be implemented here.
JonFreeman 0:435bf84ce48a 23 The remote controller must not be relied upon to deliver sensible command sequences.
JonFreeman 0:435bf84ce48a 24
JonFreeman 0:435bf84ce48a 25 So much the better if the remote controller does issue sensible commands, but ...
JonFreeman 0:435bf84ce48a 26
JonFreeman 0:435bf84ce48a 27 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 28
JonFreeman 0:435bf84ce48a 29
JonFreeman 0:435bf84ce48a 30 */
JonFreeman 6:f289a49c1eae 31 //#if defined (TARGET_NUCLEO_F446ZE)
JonFreeman 6:f289a49c1eae 32 #if defined (TARGET_NUCLEO_F401RE)
JonFreeman 0:435bf84ce48a 33
JonFreeman 0:435bf84ce48a 34 // Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out
JonFreeman 0:435bf84ce48a 35
JonFreeman 0:435bf84ce48a 36 // Port A -> MotorA, Port B -> MotorB
JonFreeman 0:435bf84ce48a 37 const uint16_t
JonFreeman 4:21d91465e4b1 38 AUL = 1 << 0, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - GOOD, works well with auto-tickle of high side drivers
JonFreeman 5:ca86a7848d54 39 AVL = 1 << 6, // These are which port bits connect to which mosfet driver
JonFreeman 0:435bf84ce48a 40 AWL = 1 << 4,
JonFreeman 0:435bf84ce48a 41
JonFreeman 0:435bf84ce48a 42 AUH = 1 << 1,
JonFreeman 0:435bf84ce48a 43 AVH = 1 << 7,
JonFreeman 0:435bf84ce48a 44 AWH = 1 << 8,
JonFreeman 0:435bf84ce48a 45
JonFreeman 5:ca86a7848d54 46 AUV = AUH | AVL, // Each of 6 possible output energisations made up of one hi and one low
JonFreeman 0:435bf84ce48a 47 AVU = AVH | AUL,
JonFreeman 0:435bf84ce48a 48 AUW = AUH | AWL,
JonFreeman 0:435bf84ce48a 49 AWU = AWH | AUL,
JonFreeman 0:435bf84ce48a 50 AVW = AVH | AWL,
JonFreeman 0:435bf84ce48a 51 AWV = AWH | AVL,
JonFreeman 0:435bf84ce48a 52
JonFreeman 3:ecb00e0e8d68 53 KEEP_L_MASK_A = AUL | AVL | AWL,
JonFreeman 3:ecb00e0e8d68 54 KEEP_H_MASK_A = AUH | AVH | AWH,
JonFreeman 3:ecb00e0e8d68 55
JonFreeman 5:ca86a7848d54 56 BRA = AUL | AVL | AWL, // All low side switches on (and all high side off) for braking
JonFreeman 0:435bf84ce48a 57
JonFreeman 5:ca86a7848d54 58 BUL = 1 << 0, // Likewise for MotorB but different port bits on different port
JonFreeman 0:435bf84ce48a 59 BVL = 1 << 1,
JonFreeman 0:435bf84ce48a 60 BWL = 1 << 2,
JonFreeman 0:435bf84ce48a 61
JonFreeman 0:435bf84ce48a 62 BUH = 1 << 10,
JonFreeman 0:435bf84ce48a 63 BVH = 1 << 12,
JonFreeman 0:435bf84ce48a 64 BWH = 1 << 13,
JonFreeman 0:435bf84ce48a 65
JonFreeman 0:435bf84ce48a 66 BUV = BUH | BVL,
JonFreeman 0:435bf84ce48a 67 BVU = BVH | BUL,
JonFreeman 0:435bf84ce48a 68 BUW = BUH | BWL,
JonFreeman 0:435bf84ce48a 69 BWU = BWH | BUL,
JonFreeman 0:435bf84ce48a 70 BVW = BVH | BWL,
JonFreeman 0:435bf84ce48a 71 BWV = BWH | BVL,
JonFreeman 0:435bf84ce48a 72
JonFreeman 3:ecb00e0e8d68 73 KEEP_L_MASK_B = BUL | BVL | BWL,
JonFreeman 3:ecb00e0e8d68 74 KEEP_H_MASK_B = BUH | BVH | BWH,
JonFreeman 3:ecb00e0e8d68 75
JonFreeman 0:435bf84ce48a 76 BRB = BUL | BVL | BWL,
JonFreeman 0:435bf84ce48a 77
JonFreeman 0:435bf84ce48a 78 PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH, // NEW METHOD FOR DGD21032 MOSFET DRIVERS
JonFreeman 0:435bf84ce48a 79 PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH;
JonFreeman 0:435bf84ce48a 80
JonFreeman 5:ca86a7848d54 81 PortOut MotA (PortA, PORT_A_MASK); // Activate output ports to motor drivers
JonFreeman 0:435bf84ce48a 82 PortOut MotB (PortB, PORT_B_MASK);
JonFreeman 0:435bf84ce48a 83
JonFreeman 0:435bf84ce48a 84 // Pin 1 VBAT NET +3V3
JonFreeman 6:f289a49c1eae 85
JonFreeman 6:f289a49c1eae 86 //DigitalIn J3 (PC_13, PullUp);// Pin 2 Jumper pulls to GND, R floats Hi
JonFreeman 6:f289a49c1eae 87 InterruptIn Temperature_pin (PC_13);// Pin 2 June 2018 - taken for temperature sensor - hard wired to T1 due to wrong thought T1 could be InterruptIn
JonFreeman 6:f289a49c1eae 88
JonFreeman 6:f289a49c1eae 89
JonFreeman 0:435bf84ce48a 90 // Pin 3 PC14-OSC32_IN NET O32I
JonFreeman 0:435bf84ce48a 91 // Pin 4 PC15-OSC32_OUT NET O32O
JonFreeman 0:435bf84ce48a 92 // Pin 5 PH0-OSC_IN NET PH1
JonFreeman 0:435bf84ce48a 93 // Pin 6 PH1-OSC_OUT NET PH1
JonFreeman 0:435bf84ce48a 94 // Pin 7 NRST NET NRST
JonFreeman 0:435bf84ce48a 95 AnalogIn Ain_DriverPot (PC_0); // Pin 8 Spare Analogue in, net SAIN fitted with external pull-down
JonFreeman 0:435bf84ce48a 96 AnalogIn Ain_SystemVolts (PC_1); // Pin 9
JonFreeman 5:ca86a7848d54 97 AnalogIn Motor_A_Current (PC_2); // Pin 10
JonFreeman 0:435bf84ce48a 98 AnalogIn Motor_B_Current (PC_3); // Pin 11
JonFreeman 0:435bf84ce48a 99 // Pin 12 VSSA/VREF- NET GND
JonFreeman 0:435bf84ce48a 100 // Pin 13 VDDA/VREF+ NET +3V3
JonFreeman 0:435bf84ce48a 101 // Pin 14 Port_A AUL
JonFreeman 0:435bf84ce48a 102 // Pin 15 Port_A AUH
JonFreeman 0:435bf84ce48a 103 // Pins 16, 17 BufferedSerial pc
JonFreeman 0:435bf84ce48a 104 BufferedSerial pc (PA_2, PA_3, 512, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead
JonFreeman 0:435bf84ce48a 105 // Pin 18 VSS NET GND
JonFreeman 0:435bf84ce48a 106 // Pin 19 VDD NET +3V3
JonFreeman 0:435bf84ce48a 107 // Pin 20 Port_A AWL
JonFreeman 0:435bf84ce48a 108 // Pin 21 DigitalOut led1(LED1);
JonFreeman 0:435bf84ce48a 109 DigitalOut LED (PA_5); // Pin 21
JonFreeman 0:435bf84ce48a 110 // Pin 22 Port_A AVL
JonFreeman 0:435bf84ce48a 111 // Pin 23 Port_A AVH
JonFreeman 0:435bf84ce48a 112 InterruptIn MBH2 (PC_4); // Pin 24
JonFreeman 0:435bf84ce48a 113 InterruptIn MBH3 (PC_5); // Pin 25
JonFreeman 0:435bf84ce48a 114 // Pin 26 Port_B BUL
JonFreeman 0:435bf84ce48a 115 // Pin 27 Port_B BVL
JonFreeman 0:435bf84ce48a 116 // Pin 28 Port_B BWL
JonFreeman 0:435bf84ce48a 117 // Pin 29 Port_B BUH
JonFreeman 0:435bf84ce48a 118 // Pin 30 VCAP1
JonFreeman 0:435bf84ce48a 119 // Pin 31 VSS
JonFreeman 0:435bf84ce48a 120 // Pin 32 VDD
JonFreeman 0:435bf84ce48a 121 // Pin 33 Port_B BVH
JonFreeman 0:435bf84ce48a 122 // Pin 34 Port_B BWH
JonFreeman 0:435bf84ce48a 123 DigitalOut T4 (PB_14); // Pin 35
JonFreeman 0:435bf84ce48a 124 DigitalOut T3 (PB_15); // Pin 36
JonFreeman 3:ecb00e0e8d68 125 // BufferedSerial com2 pins 37 Tx, 38 Rx
JonFreeman 3:ecb00e0e8d68 126 BufferedSerial com2 (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module
JonFreeman 0:435bf84ce48a 127 FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3
JonFreeman 0:435bf84ce48a 128 A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4
JonFreeman 0:435bf84ce48a 129 //InterruptIn MotB_Hall (PA_8); // Pin 41
JonFreeman 0:435bf84ce48a 130 // Pin 41 Port_A AWH
JonFreeman 3:ecb00e0e8d68 131 // BufferedSerial com3 pins 42 Tx, 43 Rx
JonFreeman 4:21d91465e4b1 132 //InterruptIn tryseredge (PA_9);
JonFreeman 3:ecb00e0e8d68 133 BufferedSerial com3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module
JonFreeman 4:21d91465e4b1 134 // PA_9 is Tx. I wonder, can we also use InterruptIn on this pin to generate interrupts on tx bit transitions ? Let's find out !
JonFreeman 4:21d91465e4b1 135 // No.
JonFreeman 0:435bf84ce48a 136
JonFreeman 0:435bf84ce48a 137 // Feb 2018 Pins 44 and 45 now liberated, could use for serial or other uses
JonFreeman 0:435bf84ce48a 138 //BufferedSerial extra_ser (PA_11, PA_12); // Pins 44, 45 tx, rx to XBee module
JonFreeman 0:435bf84ce48a 139 DigitalOut T2 (PA_11); // Pin 44
JonFreeman 5:ca86a7848d54 140 // was DigitalOut T1 (PA_12); // Pin 45
JonFreeman 6:f289a49c1eae 141
JonFreeman 6:f289a49c1eae 142
JonFreeman 6:f289a49c1eae 143 //InterruptIn T1 (PA_12); // Pin 45 now input counting pulses from LMT01 temperature sensor
JonFreeman 6:f289a49c1eae 144 // InterruptIn DOES NOT WORK ON PA_12. Boards are being made, will have to wire link PA12 to PC13
JonFreeman 6:f289a49c1eae 145 DigitalIn T1 (PA_12);
JonFreeman 6:f289a49c1eae 146 ////InterruptIn T1 (PC_13); // Pin 45 now input counting pulses from LMT01 temperature sensor
JonFreeman 6:f289a49c1eae 147
JonFreeman 6:f289a49c1eae 148
JonFreeman 6:f289a49c1eae 149
JonFreeman 0:435bf84ce48a 150 // Pin 46 SWDIO
JonFreeman 0:435bf84ce48a 151 // Pin 47 VSS
JonFreeman 0:435bf84ce48a 152 // Pin 48 VDD
JonFreeman 0:435bf84ce48a 153 // Pin 49 SWCLK
JonFreeman 5:ca86a7848d54 154
JonFreeman 5:ca86a7848d54 155 //Was DigitalOut T5 (PA_15); // Pin 50
JonFreeman 5:ca86a7848d54 156 DigitalIn T5 (PA_15); // Pin 50 now fwd/rev from remote control box if fitted
JonFreeman 0:435bf84ce48a 157 InterruptIn MAH1 (PC_10); // Pin 51
JonFreeman 0:435bf84ce48a 158 InterruptIn MAH2 (PC_11); // Pin 52
JonFreeman 0:435bf84ce48a 159 InterruptIn MAH3 (PC_12); // Pin 53
JonFreeman 0:435bf84ce48a 160 InterruptIn MBH1 (PD_2); // Pin 54
JonFreeman 0:435bf84ce48a 161 DigitalOut T6 (PB_3); // Pin 55
JonFreeman 0:435bf84ce48a 162 FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3
JonFreeman 0:435bf84ce48a 163 B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4
JonFreeman 0:435bf84ce48a 164
JonFreeman 0:435bf84ce48a 165 I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom
JonFreeman 0:435bf84ce48a 166 // Pin 60 BOOT0
JonFreeman 0:435bf84ce48a 167
JonFreeman 4:21d91465e4b1 168 // Servo pins, 2 off. Configured as Input to read radio control receiver
JonFreeman 4:21d91465e4b1 169 // If used as servo output, code gives pin to 'Servo' - seems to work
JonFreeman 0:435bf84ce48a 170 InterruptIn Servo1_i (PB_8); // Pin 61 to read output from rc rx
JonFreeman 0:435bf84ce48a 171 InterruptIn Servo2_i (PB_9); // Pin 62 to read output from rc rx
JonFreeman 0:435bf84ce48a 172
JonFreeman 0:435bf84ce48a 173 // Pin 63 VSS
JonFreeman 0:435bf84ce48a 174 // Pin 64 VDD
JonFreeman 0:435bf84ce48a 175 // SYSTEM CONSTANTS
JonFreeman 0:435bf84ce48a 176
JonFreeman 6:f289a49c1eae 177 #endif
JonFreeman 6:f289a49c1eae 178 #if defined (TARGET_NUCLEO_F446ZE)
JonFreeman 6:f289a49c1eae 179 #endif
JonFreeman 0:435bf84ce48a 180 /* Global variable declarations */
JonFreeman 3:ecb00e0e8d68 181 volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
JonFreeman 5:ca86a7848d54 182 int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
JonFreeman 0:435bf84ce48a 183 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 184 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 185 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 5:ca86a7848d54 186 AtoD_Semaphore = 0;
JonFreeman 5:ca86a7848d54 187 int IAm;
JonFreeman 0:435bf84ce48a 188 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 0:435bf84ce48a 189 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 6:f289a49c1eae 190 bool temp_sensor_exists = false;
JonFreeman 6:f289a49c1eae 191 char mode_bytes[36];
JonFreeman 3:ecb00e0e8d68 192
JonFreeman 6:f289a49c1eae 193 uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
JonFreeman 6:f289a49c1eae 194 last_temp_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
JonFreeman 6:f289a49c1eae 195 // struct single_bogie_options bogie;
JonFreeman 0:435bf84ce48a 196 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 197
JonFreeman 0:435bf84ce48a 198 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 199 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 6:f289a49c1eae 200 Ticker temperature_find_ticker;
JonFreeman 6:f289a49c1eae 201 Timer temperature_timer;
JonFreeman 0:435bf84ce48a 202
JonFreeman 0:435bf84ce48a 203 // Interrupt Service Routines
JonFreeman 6:f289a49c1eae 204 void ISR_temperature_find_ticker () { // every 960 us, i.e. slightly faster than once per milli sec
JonFreeman 6:f289a49c1eae 205 static bool readflag = false;
JonFreeman 6:f289a49c1eae 206 int t = temperature_timer.read_ms ();
JonFreeman 6:f289a49c1eae 207 if ((t == 5) && (!readflag)) {
JonFreeman 6:f289a49c1eae 208 last_temp_count = temp_sensor_count;
JonFreeman 6:f289a49c1eae 209 temp_sensor_count = 0;
JonFreeman 6:f289a49c1eae 210 readflag = true;
JonFreeman 6:f289a49c1eae 211 }
JonFreeman 6:f289a49c1eae 212 if (t == 6)
JonFreeman 6:f289a49c1eae 213 readflag = false;
JonFreeman 6:f289a49c1eae 214 }
JonFreeman 0:435bf84ce48a 215
JonFreeman 0:435bf84ce48a 216 /** void ISR_loop_timer ()
JonFreeman 0:435bf84ce48a 217 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 0:435bf84ce48a 218 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 0:435bf84ce48a 219 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 0:435bf84ce48a 220 */
JonFreeman 0:435bf84ce48a 221 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 222 {
JonFreeman 0:435bf84ce48a 223 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 0:435bf84ce48a 224 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 225 if ((sys_timer & 0x03) == 0)
JonFreeman 0:435bf84ce48a 226 flag_8Hz = true;
JonFreeman 0:435bf84ce48a 227 }
JonFreeman 0:435bf84ce48a 228
JonFreeman 0:435bf84ce48a 229 /** void ISR_voltage_reader ()
JonFreeman 0:435bf84ce48a 230 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 0:435bf84ce48a 231 *
JonFreeman 0:435bf84ce48a 232 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 0:435bf84ce48a 233 */
JonFreeman 0:435bf84ce48a 234
JonFreeman 5:ca86a7848d54 235 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
JonFreeman 0:435bf84ce48a 236 {
JonFreeman 0:435bf84ce48a 237 AtoD_Semaphore++;
JonFreeman 2:04761b196473 238 fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 239 }
JonFreeman 0:435bf84ce48a 240
JonFreeman 0:435bf84ce48a 241
JonFreeman 4:21d91465e4b1 242 class RControl_In
JonFreeman 4:21d91465e4b1 243 { // Read servo style pwm input
JonFreeman 0:435bf84ce48a 244 Timer t;
JonFreeman 0:435bf84ce48a 245 int32_t clock_old, clock_new;
JonFreeman 0:435bf84ce48a 246 int32_t pulse_width_us, period_us;
JonFreeman 4:21d91465e4b1 247 public:
JonFreeman 4:21d91465e4b1 248 RControl_In () {
JonFreeman 0:435bf84ce48a 249 pulse_width_us = period_us = 0;
JonFreeman 4:21d91465e4b1 250 com2.printf ("Setting up Radio_Congtrol_In\r\n");
JonFreeman 4:21d91465e4b1 251 } ;
JonFreeman 0:435bf84ce48a 252 bool validate_rx () ;
JonFreeman 0:435bf84ce48a 253 void rise () ;
JonFreeman 0:435bf84ce48a 254 void fall () ;
JonFreeman 4:21d91465e4b1 255 uint32_t pulsewidth () ;
JonFreeman 4:21d91465e4b1 256 uint32_t period () ;
JonFreeman 4:21d91465e4b1 257 bool rx_active;
JonFreeman 4:21d91465e4b1 258 } ;
JonFreeman 0:435bf84ce48a 259
JonFreeman 4:21d91465e4b1 260 uint32_t RControl_In::pulsewidth () {
JonFreeman 4:21d91465e4b1 261 return pulse_width_us;
JonFreeman 4:21d91465e4b1 262 }
JonFreeman 4:21d91465e4b1 263
JonFreeman 4:21d91465e4b1 264 uint32_t RControl_In::period () {
JonFreeman 4:21d91465e4b1 265 return period_us;
JonFreeman 4:21d91465e4b1 266 }
JonFreeman 4:21d91465e4b1 267
JonFreeman 4:21d91465e4b1 268 bool RControl_In::validate_rx ()
JonFreeman 0:435bf84ce48a 269 {
JonFreeman 0:435bf84ce48a 270 if ((clock() - clock_new) > 4)
JonFreeman 0:435bf84ce48a 271 rx_active = false;
JonFreeman 0:435bf84ce48a 272 else
JonFreeman 0:435bf84ce48a 273 rx_active = true;
JonFreeman 0:435bf84ce48a 274 return rx_active;
JonFreeman 0:435bf84ce48a 275 }
JonFreeman 0:435bf84ce48a 276
JonFreeman 6:f289a49c1eae 277 void RControl_In::rise () // These may not work as use of PortB as port may bugger InterruptIn use
JonFreeman 0:435bf84ce48a 278 {
JonFreeman 0:435bf84ce48a 279 t.stop ();
JonFreeman 0:435bf84ce48a 280 period_us = t.read_us ();
JonFreeman 0:435bf84ce48a 281 t.reset ();
JonFreeman 0:435bf84ce48a 282 t.start ();
JonFreeman 0:435bf84ce48a 283 }
JonFreeman 4:21d91465e4b1 284 void RControl_In::fall ()
JonFreeman 0:435bf84ce48a 285 {
JonFreeman 0:435bf84ce48a 286 pulse_width_us = t.read_us ();
JonFreeman 0:435bf84ce48a 287 clock_old = clock_new;
JonFreeman 0:435bf84ce48a 288 clock_new = clock();
JonFreeman 0:435bf84ce48a 289 if ((clock_new - clock_old) < 4)
JonFreeman 0:435bf84ce48a 290 rx_active = true;
JonFreeman 0:435bf84ce48a 291 }
JonFreeman 4:21d91465e4b1 292
JonFreeman 0:435bf84ce48a 293
JonFreeman 4:21d91465e4b1 294 Servo * Servos[2];
JonFreeman 4:21d91465e4b1 295
JonFreeman 0:435bf84ce48a 296 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 297 /*
JonFreeman 0:435bf84ce48a 298 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 299
JonFreeman 3:ecb00e0e8d68 300 1 1 1 1 0 0 0 ---___---___ Hall1
JonFreeman 3:ecb00e0e8d68 301 2 0 0 1 1 1 0 __---___---_ Hall2
JonFreeman 3:ecb00e0e8d68 302 4 1 0 0 0 1 1 -___---___-- Hall3
JonFreeman 0:435bf84ce48a 303
JonFreeman 0:435bf84ce48a 304 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 0:435bf84ce48a 305 */
JonFreeman 3:ecb00e0e8d68 306 const uint16_t A_tabl[] = { // Origial table
JonFreeman 0:435bf84ce48a 307 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 308 0, AWV,AVU,AWU,AUW,AUV,AVW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
JonFreeman 0:435bf84ce48a 309 0, AVW,AUV,AUW,AWU,AVU,AWV, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
JonFreeman 3:ecb00e0e8d68 310 0, BRA,BRA,BRA,BRA,BRA,BRA,0, // Regenerative Braking
JonFreeman 4:21d91465e4b1 311 KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
JonFreeman 3:ecb00e0e8d68 312 } ;
JonFreeman 4:21d91465e4b1 313 InterruptIn * AHarr[] = {
JonFreeman 4:21d91465e4b1 314 &MAH1,
JonFreeman 4:21d91465e4b1 315 &MAH2,
JonFreeman 4:21d91465e4b1 316 &MAH3
JonFreeman 3:ecb00e0e8d68 317 } ;
JonFreeman 0:435bf84ce48a 318 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 319 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 320 0, BWV,BVU,BWU,BUW,BUV,BVW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
JonFreeman 0:435bf84ce48a 321 0, BVW,BUV,BUW,BWU,BVU,BWV, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
JonFreeman 3:ecb00e0e8d68 322 0, BRB,BRB,BRB,BRB,BRB,BRB,0, // Regenerative Braking
JonFreeman 4:21d91465e4b1 323 KEEP_L_MASK_B, KEEP_H_MASK_B
JonFreeman 3:ecb00e0e8d68 324 } ;
JonFreeman 4:21d91465e4b1 325 InterruptIn * BHarr[] = {
JonFreeman 4:21d91465e4b1 326 &MBH1,
JonFreeman 4:21d91465e4b1 327 &MBH2,
JonFreeman 4:21d91465e4b1 328 &MBH3
JonFreeman 0:435bf84ce48a 329 } ;
JonFreeman 0:435bf84ce48a 330
JonFreeman 0:435bf84ce48a 331 class motor
JonFreeman 0:435bf84ce48a 332 {
JonFreeman 5:ca86a7848d54 333 uint32_t Hall_total, visible_mode, inner_mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
JonFreeman 3:ecb00e0e8d68 334 uint32_t latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp;
JonFreeman 3:ecb00e0e8d68 335 bool moving_flag;
JonFreeman 0:435bf84ce48a 336 const uint16_t * lut;
JonFreeman 0:435bf84ce48a 337 FastPWM * maxV, * maxI;
JonFreeman 0:435bf84ce48a 338 PortOut * Motor_Port;
JonFreeman 2:04761b196473 339 InterruptIn * Hall1, * Hall2, * Hall3;
JonFreeman 0:435bf84ce48a 340 public:
JonFreeman 2:04761b196473 341 struct currents {
JonFreeman 2:04761b196473 342 uint32_t max, min, ave;
JonFreeman 2:04761b196473 343 } I;
JonFreeman 3:ecb00e0e8d68 344 int32_t angle_cnt;
JonFreeman 0:435bf84ce48a 345 uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
JonFreeman 3:ecb00e0e8d68 346 uint32_t Hindex[2], tickleon, encoder_error_cnt;
JonFreeman 5:ca86a7848d54 347 uint32_t RPM, PPS;
JonFreeman 5:ca86a7848d54 348 double last_V, last_I;
JonFreeman 0:435bf84ce48a 349 motor () {} ; // Default constructor
JonFreeman 4:21d91465e4b1 350 motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
JonFreeman 0:435bf84ce48a 351 void set_V_limit (double) ; // Sets max motor voltage
JonFreeman 0:435bf84ce48a 352 void set_I_limit (double) ; // Sets max motor current
JonFreeman 0:435bf84ce48a 353 void Hall_change () ; // Called in response to edge on Hall input pin
JonFreeman 3:ecb00e0e8d68 354 void motor_set () ; // Energise Port with data determined by Hall sensors
JonFreeman 3:ecb00e0e8d68 355 void direction_set (int) ; // sets 'direction' with bit pattern to eor with FORWARD or REVERSE in set_mode
JonFreeman 3:ecb00e0e8d68 356 bool set_mode (int); // sets mode to HANDBRAKE, FORWARD, REVERSE or REGENBRAKE
JonFreeman 3:ecb00e0e8d68 357 bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs
JonFreeman 3:ecb00e0e8d68 358 void current_calc () ; // Updates 3 uint32_t I.min, I.ave, I.max
JonFreeman 3:ecb00e0e8d68 359 uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
JonFreeman 3:ecb00e0e8d68 360 int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
JonFreeman 3:ecb00e0e8d68 361 void high_side_off () ;
JonFreeman 3:ecb00e0e8d68 362 } ; //MotorA, MotorB, or even Motor[2];
JonFreeman 0:435bf84ce48a 363
JonFreeman 4:21d91465e4b1 364 motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
JonFreeman 4:21d91465e4b1 365 motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr);
JonFreeman 0:435bf84ce48a 366
JonFreeman 3:ecb00e0e8d68 367 motor * MotPtr[8]; // Array of pointers to some number of motor objects
JonFreeman 3:ecb00e0e8d68 368
JonFreeman 4:21d91465e4b1 369 motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
JonFreeman 3:ecb00e0e8d68 370 { // Constructor
JonFreeman 0:435bf84ce48a 371 maxV = _maxV_;
JonFreeman 0:435bf84ce48a 372 maxI = _maxI_;
JonFreeman 3:ecb00e0e8d68 373 Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
JonFreeman 0:435bf84ce48a 374 latest_pulses_per_sec = 0;
JonFreeman 0:435bf84ce48a 375 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 0:435bf84ce48a 376 edge_count_table[i] = 0;
JonFreeman 0:435bf84ce48a 377 if (lutptr != A_tabl && lutptr != B_tabl)
JonFreeman 0:435bf84ce48a 378 pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
JonFreeman 3:ecb00e0e8d68 379 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 380 Motor_Port = P;
JonFreeman 0:435bf84ce48a 381 pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
JonFreeman 0:435bf84ce48a 382 maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:435bf84ce48a 383 maxI->period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:435bf84ce48a 384 maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 0:435bf84ce48a 385 maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 5:ca86a7848d54 386 visible_mode = REGENBRAKE;
JonFreeman 5:ca86a7848d54 387 inner_mode = REGENBRAKE;
JonFreeman 0:435bf84ce48a 388 lut = lutptr;
JonFreeman 3:ecb00e0e8d68 389 Hindex[0] = Hindex[1] = read_Halls ();
JonFreeman 3:ecb00e0e8d68 390 ppstmp = 0;
JonFreeman 3:ecb00e0e8d68 391 tickleon = 0;
JonFreeman 3:ecb00e0e8d68 392 direction = 0;
JonFreeman 3:ecb00e0e8d68 393 angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel
JonFreeman 3:ecb00e0e8d68 394 encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction
JonFreeman 4:21d91465e4b1 395 Hall1 = Hall[0];
JonFreeman 4:21d91465e4b1 396 Hall2 = Hall[1];
JonFreeman 4:21d91465e4b1 397 Hall3 = Hall[2];
JonFreeman 5:ca86a7848d54 398 PPS = 0;
JonFreeman 5:ca86a7848d54 399 RPM = 0;
JonFreeman 5:ca86a7848d54 400 last_V = last_I = 0.0;
JonFreeman 3:ecb00e0e8d68 401 }
JonFreeman 3:ecb00e0e8d68 402
JonFreeman 5:ca86a7848d54 403 /**
JonFreeman 5:ca86a7848d54 404 void motor::direction_set (int dir) {
JonFreeman 5:ca86a7848d54 405 Used to set direction according to mode data from eeprom
JonFreeman 5:ca86a7848d54 406 */
JonFreeman 3:ecb00e0e8d68 407 void motor::direction_set (int dir) {
JonFreeman 3:ecb00e0e8d68 408 if (dir != 0)
JonFreeman 3:ecb00e0e8d68 409 dir = FORWARD | REVERSE; // bits used in eor
JonFreeman 3:ecb00e0e8d68 410 direction = dir;
JonFreeman 0:435bf84ce48a 411 }
JonFreeman 0:435bf84ce48a 412
JonFreeman 1:0fabe6fdb55b 413 int motor::read_Halls () {
JonFreeman 2:04761b196473 414 int x = 0;
JonFreeman 2:04761b196473 415 if (*Hall1 != 0) x |= 1;
JonFreeman 2:04761b196473 416 if (*Hall2 != 0) x |= 2;
JonFreeman 2:04761b196473 417 if (*Hall3 != 0) x |= 4;
JonFreeman 2:04761b196473 418 return x;
JonFreeman 2:04761b196473 419 }
JonFreeman 2:04761b196473 420
JonFreeman 3:ecb00e0e8d68 421 void motor::high_side_off () {
JonFreeman 3:ecb00e0e8d68 422 uint16_t p = *Motor_Port;
JonFreeman 3:ecb00e0e8d68 423 p &= lut[32]; // KEEP_L_MASK_A or B
JonFreeman 3:ecb00e0e8d68 424 *Motor_Port = p;
JonFreeman 1:0fabe6fdb55b 425 }
JonFreeman 1:0fabe6fdb55b 426
JonFreeman 0:435bf84ce48a 427 void motor::current_calc ()
JonFreeman 0:435bf84ce48a 428 {
JonFreeman 0:435bf84ce48a 429 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 0:435bf84ce48a 430 I.max = 0;
JonFreeman 0:435bf84ce48a 431 I.ave = 0;
JonFreeman 0:435bf84ce48a 432 uint16_t sample;
JonFreeman 0:435bf84ce48a 433 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 0:435bf84ce48a 434 sample = current_samples[i];
JonFreeman 0:435bf84ce48a 435 I.ave += sample;
JonFreeman 0:435bf84ce48a 436 if (I.min > sample)
JonFreeman 0:435bf84ce48a 437 I.min = sample;
JonFreeman 0:435bf84ce48a 438 if (I.max < sample)
JonFreeman 0:435bf84ce48a 439 I.max = sample;
JonFreeman 0:435bf84ce48a 440 }
JonFreeman 0:435bf84ce48a 441 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 0:435bf84ce48a 442 }
JonFreeman 0:435bf84ce48a 443
JonFreeman 0:435bf84ce48a 444 void motor::set_V_limit (double p) // Sets max motor voltage
JonFreeman 0:435bf84ce48a 445 {
JonFreeman 0:435bf84ce48a 446 if (p < 0.0)
JonFreeman 0:435bf84ce48a 447 p = 0.0;
JonFreeman 0:435bf84ce48a 448 if (p > 1.0)
JonFreeman 0:435bf84ce48a 449 p = 1.0;
JonFreeman 5:ca86a7848d54 450 last_V = p; // for read by diagnostics
JonFreeman 0:435bf84ce48a 451 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:435bf84ce48a 452 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:435bf84ce48a 453 maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 0:435bf84ce48a 454 }
JonFreeman 0:435bf84ce48a 455
JonFreeman 0:435bf84ce48a 456 void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 0:435bf84ce48a 457 {
JonFreeman 0:435bf84ce48a 458 int a;
JonFreeman 0:435bf84ce48a 459 if (p < 0.0)
JonFreeman 0:435bf84ce48a 460 p = 0.0;
JonFreeman 0:435bf84ce48a 461 if (p > 1.0)
JonFreeman 0:435bf84ce48a 462 p = 1.0;
JonFreeman 5:ca86a7848d54 463 last_I = p;
JonFreeman 0:435bf84ce48a 464 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:435bf84ce48a 465 if (a > MAX_PWM_TICKS)
JonFreeman 0:435bf84ce48a 466 a = MAX_PWM_TICKS;
JonFreeman 0:435bf84ce48a 467 if (a < 0)
JonFreeman 0:435bf84ce48a 468 a = 0;
JonFreeman 0:435bf84ce48a 469 maxI->pulsewidth_ticks (a); // PWM
JonFreeman 0:435bf84ce48a 470 }
JonFreeman 0:435bf84ce48a 471
JonFreeman 3:ecb00e0e8d68 472 uint32_t motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
JonFreeman 3:ecb00e0e8d68 473 { // Can also test for motor running or not here
JonFreeman 3:ecb00e0e8d68 474 if (ppstmp == Hall_total) {
JonFreeman 3:ecb00e0e8d68 475 moving_flag = false; // Zero Hall transitions since previous call - motor not moving
JonFreeman 4:21d91465e4b1 476 tickleon = TICKLE_TIMES;
JonFreeman 3:ecb00e0e8d68 477 }
JonFreeman 3:ecb00e0e8d68 478 else {
JonFreeman 3:ecb00e0e8d68 479 moving_flag = true;
JonFreeman 3:ecb00e0e8d68 480 ppstmp = Hall_total;
JonFreeman 3:ecb00e0e8d68 481 }
JonFreeman 3:ecb00e0e8d68 482 latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
JonFreeman 3:ecb00e0e8d68 483 edge_count_table[Hall_tab_ptr] = ppstmp;
JonFreeman 0:435bf84ce48a 484 Hall_tab_ptr++;
JonFreeman 0:435bf84ce48a 485 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 0:435bf84ce48a 486 Hall_tab_ptr = 0;
JonFreeman 5:ca86a7848d54 487 PPS = latest_pulses_per_sec;
JonFreeman 5:ca86a7848d54 488 RPM = (latest_pulses_per_sec * 60) / 24;
JonFreeman 0:435bf84ce48a 489 return latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 490 }
JonFreeman 0:435bf84ce48a 491
JonFreeman 3:ecb00e0e8d68 492 bool motor::is_moving ()
JonFreeman 3:ecb00e0e8d68 493 {
JonFreeman 3:ecb00e0e8d68 494 return moving_flag;
JonFreeman 3:ecb00e0e8d68 495 }
JonFreeman 3:ecb00e0e8d68 496
JonFreeman 5:ca86a7848d54 497 /**
JonFreeman 5:ca86a7848d54 498 bool motor::set_mode (int m)
JonFreeman 5:ca86a7848d54 499 Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
JonFreeman 5:ca86a7848d54 500 If this causes change of mode, also sets V and I to zero.
JonFreeman 5:ca86a7848d54 501 */
JonFreeman 0:435bf84ce48a 502 bool motor::set_mode (int m)
JonFreeman 0:435bf84ce48a 503 {
JonFreeman 2:04761b196473 504 if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
JonFreeman 2:04761b196473 505 pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 0:435bf84ce48a 506 return false;
JonFreeman 2:04761b196473 507 }
JonFreeman 5:ca86a7848d54 508 if (visible_mode != m) { // Mode change, kill volts and amps to be safe
JonFreeman 5:ca86a7848d54 509 set_V_limit (0.0);
JonFreeman 5:ca86a7848d54 510 set_I_limit (0.0);
JonFreeman 5:ca86a7848d54 511 visible_mode = m;
JonFreeman 5:ca86a7848d54 512 }
JonFreeman 3:ecb00e0e8d68 513 if (m == FORWARD || m == REVERSE)
JonFreeman 3:ecb00e0e8d68 514 m ^= direction;
JonFreeman 5:ca86a7848d54 515 inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
JonFreeman 0:435bf84ce48a 516 return true;
JonFreeman 0:435bf84ce48a 517 }
JonFreeman 0:435bf84ce48a 518
JonFreeman 0:435bf84ce48a 519 void motor::Hall_change ()
JonFreeman 0:435bf84ce48a 520 {
JonFreeman 3:ecb00e0e8d68 521 const int32_t delta_theta_lut[] = // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
JonFreeman 3:ecb00e0e8d68 522 {
JonFreeman 3:ecb00e0e8d68 523 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
JonFreeman 3:ecb00e0e8d68 524 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
JonFreeman 3:ecb00e0e8d68 525 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
JonFreeman 3:ecb00e0e8d68 526 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
JonFreeman 3:ecb00e0e8d68 527 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
JonFreeman 3:ecb00e0e8d68 528 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
JonFreeman 3:ecb00e0e8d68 529 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
JonFreeman 3:ecb00e0e8d68 530 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
JonFreeman 3:ecb00e0e8d68 531 } ;
JonFreeman 3:ecb00e0e8d68 532 int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
JonFreeman 3:ecb00e0e8d68 533 if (delta_theta == 0)
JonFreeman 3:ecb00e0e8d68 534 encoder_error_cnt++;
JonFreeman 3:ecb00e0e8d68 535 else
JonFreeman 3:ecb00e0e8d68 536 angle_cnt += delta_theta;
JonFreeman 5:ca86a7848d54 537 *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18
JonFreeman 0:435bf84ce48a 538 Hall_total++;
JonFreeman 3:ecb00e0e8d68 539 Hindex[1] = Hindex[0];
JonFreeman 0:435bf84ce48a 540 }
JonFreeman 2:04761b196473 541
JonFreeman 5:ca86a7848d54 542
JonFreeman 6:f289a49c1eae 543 void temp_sensor_isr () { // got rising edge from LMT01. ALMOST CERTAIN this misses some
JonFreeman 6:f289a49c1eae 544 int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
JonFreeman 6:f289a49c1eae 545 temperature_timer.reset ();
JonFreeman 5:ca86a7848d54 546 temp_sensor_count++;
JonFreeman 6:f289a49c1eae 547 if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
JonFreeman 6:f289a49c1eae 548 temp_sensor_count++;
JonFreeman 6:f289a49c1eae 549 // T2 = !T2; // scope hanger
JonFreeman 5:ca86a7848d54 550 }
JonFreeman 5:ca86a7848d54 551
JonFreeman 3:ecb00e0e8d68 552 void MAH_isr ()
JonFreeman 0:435bf84ce48a 553 {
JonFreeman 3:ecb00e0e8d68 554 uint32_t x = 0;
JonFreeman 3:ecb00e0e8d68 555 MotorA.high_side_off ();
JonFreeman 3:ecb00e0e8d68 556 T3 = !T3;
JonFreeman 3:ecb00e0e8d68 557 if (MAH1 != 0) x |= 1;
JonFreeman 3:ecb00e0e8d68 558 if (MAH2 != 0) x |= 2;
JonFreeman 3:ecb00e0e8d68 559 if (MAH3 != 0) x |= 4;
JonFreeman 3:ecb00e0e8d68 560 MotorA.Hindex[0] = x; // New in [0], old in [1]
JonFreeman 0:435bf84ce48a 561 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 562 }
JonFreeman 0:435bf84ce48a 563
JonFreeman 3:ecb00e0e8d68 564 void MBH_isr ()
JonFreeman 0:435bf84ce48a 565 {
JonFreeman 3:ecb00e0e8d68 566 uint32_t x = 0;
JonFreeman 3:ecb00e0e8d68 567 MotorB.high_side_off ();
JonFreeman 3:ecb00e0e8d68 568 if (MBH1 != 0) x |= 1;
JonFreeman 3:ecb00e0e8d68 569 if (MBH2 != 0) x |= 2;
JonFreeman 3:ecb00e0e8d68 570 if (MBH3 != 0) x |= 4;
JonFreeman 3:ecb00e0e8d68 571 MotorB.Hindex[0] = x;
JonFreeman 0:435bf84ce48a 572 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 573 }
JonFreeman 0:435bf84ce48a 574
JonFreeman 0:435bf84ce48a 575
JonFreeman 0:435bf84ce48a 576 // End of Interrupt Service Routines
JonFreeman 0:435bf84ce48a 577
JonFreeman 3:ecb00e0e8d68 578 void motor::motor_set ()
JonFreeman 3:ecb00e0e8d68 579 {
JonFreeman 3:ecb00e0e8d68 580 Hindex[0] = read_Halls ();
JonFreeman 5:ca86a7848d54 581 *Motor_Port = lut[inner_mode | Hindex[0]];
JonFreeman 3:ecb00e0e8d68 582 }
JonFreeman 3:ecb00e0e8d68 583
JonFreeman 2:04761b196473 584 void setVI (double v, double i) {
JonFreeman 4:21d91465e4b1 585 MotorA.set_V_limit (v); // Sets max motor voltage
JonFreeman 4:21d91465e4b1 586 MotorA.set_I_limit (i); // Sets max motor current
JonFreeman 4:21d91465e4b1 587 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 588 MotorB.set_I_limit (i);
JonFreeman 4:21d91465e4b1 589 }
JonFreeman 4:21d91465e4b1 590 void setV (double v) {
JonFreeman 2:04761b196473 591 MotorA.set_V_limit (v);
JonFreeman 4:21d91465e4b1 592 MotorB.set_V_limit (v);
JonFreeman 4:21d91465e4b1 593 }
JonFreeman 4:21d91465e4b1 594 void setI (double i) {
JonFreeman 2:04761b196473 595 MotorA.set_I_limit (i);
JonFreeman 2:04761b196473 596 MotorB.set_I_limit (i);
JonFreeman 0:435bf84ce48a 597 }
JonFreeman 2:04761b196473 598
JonFreeman 5:ca86a7848d54 599 void read_RPM (uint32_t * dest) {
JonFreeman 5:ca86a7848d54 600 dest[0] = MotorA.RPM;
JonFreeman 5:ca86a7848d54 601 dest[1] = MotorB.RPM;
JonFreeman 5:ca86a7848d54 602 }
JonFreeman 5:ca86a7848d54 603
JonFreeman 5:ca86a7848d54 604 void read_PPS (uint32_t * dest) {
JonFreeman 5:ca86a7848d54 605 dest[0] = MotorA.PPS;
JonFreeman 5:ca86a7848d54 606 dest[1] = MotorB.PPS;
JonFreeman 5:ca86a7848d54 607 }
JonFreeman 5:ca86a7848d54 608
JonFreeman 5:ca86a7848d54 609 void read_last_VI (double * d) { // only for test from cli
JonFreeman 5:ca86a7848d54 610 d[0] = MotorA.last_V;
JonFreeman 5:ca86a7848d54 611 d[1] = MotorA.last_I;
JonFreeman 5:ca86a7848d54 612 d[2] = MotorB.last_V;
JonFreeman 5:ca86a7848d54 613 d[3] = MotorB.last_I;
JonFreeman 5:ca86a7848d54 614 }
JonFreeman 5:ca86a7848d54 615
JonFreeman 3:ecb00e0e8d68 616
JonFreeman 5:ca86a7848d54 617 /**
JonFreeman 5:ca86a7848d54 618 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 5:ca86a7848d54 619 Not part of ISR
JonFreeman 5:ca86a7848d54 620 */
JonFreeman 3:ecb00e0e8d68 621 void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
JonFreeman 0:435bf84ce48a 622 {
JonFreeman 6:f289a49c1eae 623 static uint32_t i = 0, tab_ptr = 0;
JonFreeman 3:ecb00e0e8d68 624
JonFreeman 3:ecb00e0e8d68 625 if (MotorA.tickleon)
JonFreeman 3:ecb00e0e8d68 626 MotorA.high_side_off ();
JonFreeman 3:ecb00e0e8d68 627 if (MotorB.tickleon)
JonFreeman 3:ecb00e0e8d68 628 MotorB.high_side_off ();
JonFreeman 0:435bf84ce48a 629 if (AtoD_Semaphore > 20) {
JonFreeman 0:435bf84ce48a 630 pc.printf ("WARNING - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore);
JonFreeman 0:435bf84ce48a 631 AtoD_Semaphore = 20;
JonFreeman 0:435bf84ce48a 632 }
JonFreeman 0:435bf84ce48a 633 while (AtoD_Semaphore > 0) {
JonFreeman 0:435bf84ce48a 634 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 635 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 636 switch (i) { //
JonFreeman 0:435bf84ce48a 637 case 0:
JonFreeman 0:435bf84ce48a 638 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 639 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 640 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 641 case 1:
JonFreeman 0:435bf84ce48a 642 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 643 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 644 break;
JonFreeman 0:435bf84ce48a 645 case 2:
JonFreeman 0:435bf84ce48a 646 MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 647 break;
JonFreeman 0:435bf84ce48a 648 case 3:
JonFreeman 0:435bf84ce48a 649 MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 650 if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter
JonFreeman 0:435bf84ce48a 651 tab_ptr = 0;
JonFreeman 0:435bf84ce48a 652 break;
JonFreeman 0:435bf84ce48a 653 }
JonFreeman 0:435bf84ce48a 654 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 655 if (i > 3)
JonFreeman 0:435bf84ce48a 656 i = 0;
JonFreeman 3:ecb00e0e8d68 657 } // end of while (AtoD_Semaphore > 0) {
JonFreeman 3:ecb00e0e8d68 658 if (MotorA.tickleon) {
JonFreeman 3:ecb00e0e8d68 659 MotorA.tickleon--;
JonFreeman 5:ca86a7848d54 660 MotorA.motor_set (); // Reactivate any high side switches turned off above
JonFreeman 3:ecb00e0e8d68 661 }
JonFreeman 3:ecb00e0e8d68 662 if (MotorB.tickleon) {
JonFreeman 3:ecb00e0e8d68 663 MotorB.tickleon--;
JonFreeman 3:ecb00e0e8d68 664 MotorB.motor_set ();
JonFreeman 0:435bf84ce48a 665 }
JonFreeman 0:435bf84ce48a 666 }
JonFreeman 0:435bf84ce48a 667
JonFreeman 0:435bf84ce48a 668 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 669 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 0:435bf84ce48a 670 * ISR also filters signal
JonFreeman 0:435bf84ce48a 671 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 672 */
JonFreeman 0:435bf84ce48a 673 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 674 {
JonFreeman 5:ca86a7848d54 675 return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 676 }
JonFreeman 0:435bf84ce48a 677
JonFreeman 0:435bf84ce48a 678 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 679 {
JonFreeman 5:ca86a7848d54 680 return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 681 }
JonFreeman 0:435bf84ce48a 682
JonFreeman 5:ca86a7848d54 683 void read_supply_vi (double * val) { // called from cli
JonFreeman 5:ca86a7848d54 684 val[0] = MotorA.I.ave;
JonFreeman 5:ca86a7848d54 685 val[1] = MotorB.I.ave;
JonFreeman 5:ca86a7848d54 686 val[2] = Read_BatteryVolts ();
JonFreeman 0:435bf84ce48a 687 }
JonFreeman 0:435bf84ce48a 688
JonFreeman 5:ca86a7848d54 689 void mode_set (int mode, double val) { // called from cli to set fw, re, rb, hb
JonFreeman 2:04761b196473 690 MotorA.set_mode (mode);
JonFreeman 2:04761b196473 691 MotorB.set_mode (mode);
JonFreeman 2:04761b196473 692 if (mode == REGENBRAKE) {
JonFreeman 5:ca86a7848d54 693 if (val > 1.0)
JonFreeman 5:ca86a7848d54 694 val = 1.0;
JonFreeman 5:ca86a7848d54 695 if (val < 0.0)
JonFreeman 5:ca86a7848d54 696 val = 0.0;
JonFreeman 5:ca86a7848d54 697 val *= 0.9; // set upper limit, this is essential
JonFreeman 5:ca86a7848d54 698 val = sqrt (val); // to linearise effect
JonFreeman 5:ca86a7848d54 699 setVI (val, 1.0);
JonFreeman 2:04761b196473 700 }
JonFreeman 2:04761b196473 701 }
JonFreeman 0:435bf84ce48a 702
JonFreeman 0:435bf84ce48a 703 extern void command_line_interpreter () ;
JonFreeman 0:435bf84ce48a 704 extern int check_24LC64 () ; // Call from near top of main() to init i2c bus
JonFreeman 0:435bf84ce48a 705 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 0:435bf84ce48a 706 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 0:435bf84ce48a 707
JonFreeman 5:ca86a7848d54 708 /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes
JonFreeman 3:ecb00e0e8d68 709 uint8_t MotA_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 710 MotB_dir, // 0 or 1
JonFreeman 3:ecb00e0e8d68 711 gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode
JonFreeman 3:ecb00e0e8d68 712 serv1, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 713 serv2, // 0, 1, 2 = Not used, Input, Output
JonFreeman 3:ecb00e0e8d68 714 cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
JonFreeman 3:ecb00e0e8d68 715 last;
JonFreeman 3:ecb00e0e8d68 716 } ;
JonFreeman 5:ca86a7848d54 717 */
JonFreeman 3:ecb00e0e8d68 718 int I_Am () { // Returns boards id number as ASCII char
JonFreeman 5:ca86a7848d54 719 return IAm;
JonFreeman 3:ecb00e0e8d68 720 }
JonFreeman 3:ecb00e0e8d68 721
JonFreeman 4:21d91465e4b1 722
JonFreeman 0:435bf84ce48a 723 int main()
JonFreeman 0:435bf84ce48a 724 {
JonFreeman 4:21d91465e4b1 725 int eighth_sec_count = 0;
JonFreeman 0:435bf84ce48a 726
JonFreeman 4:21d91465e4b1 727 MotA = 0; // Output all 0s to Motor drive ports A and B
JonFreeman 0:435bf84ce48a 728 MotB = 0;
JonFreeman 4:21d91465e4b1 729 MotPtr[0] = &MotorA; // Pointers to motor class objects
JonFreeman 3:ecb00e0e8d68 730 MotPtr[1] = &MotorB;
JonFreeman 5:ca86a7848d54 731
JonFreeman 6:f289a49c1eae 732 Temperature_pin.fall (&temp_sensor_isr);
JonFreeman 6:f289a49c1eae 733 Temperature_pin.mode (PullUp);
JonFreeman 4:21d91465e4b1 734
JonFreeman 4:21d91465e4b1 735 MAH1.rise (& MAH_isr); // Set up interrupt vectors
JonFreeman 3:ecb00e0e8d68 736 MAH1.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 737 MAH2.rise (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 738 MAH2.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 739 MAH3.rise (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 740 MAH3.fall (& MAH_isr);
JonFreeman 3:ecb00e0e8d68 741
JonFreeman 3:ecb00e0e8d68 742 MBH1.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 743 MBH1.fall (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 744 MBH2.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 745 MBH2.fall (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 746 MBH3.rise (& MBH_isr);
JonFreeman 3:ecb00e0e8d68 747 MBH3.fall (& MBH_isr);
JonFreeman 4:21d91465e4b1 748
JonFreeman 0:435bf84ce48a 749 MAH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 750 MAH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 751 MAH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 752 MBH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 753 MBH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 754 MBH3.mode (PullUp);
JonFreeman 4:21d91465e4b1 755 Servo1_i.mode (PullUp);
JonFreeman 4:21d91465e4b1 756 Servo2_i.mode (PullUp);
JonFreeman 4:21d91465e4b1 757
JonFreeman 0:435bf84ce48a 758 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 0:435bf84ce48a 759 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 760 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 6:f289a49c1eae 761 temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960);
JonFreeman 0:435bf84ce48a 762 // Done setting up system interrupt timers
JonFreeman 6:f289a49c1eae 763 temperature_timer.start ();
JonFreeman 0:435bf84ce48a 764
JonFreeman 6:f289a49c1eae 765 // const int TXTBUFSIZ = 36;
JonFreeman 6:f289a49c1eae 766 // char buff[TXTBUFSIZ];
JonFreeman 3:ecb00e0e8d68 767 pc.baud (9600);
JonFreeman 4:21d91465e4b1 768 com3.baud (1200);
JonFreeman 4:21d91465e4b1 769 com2.baud (19200);
JonFreeman 6:f289a49c1eae 770
JonFreeman 5:ca86a7848d54 771 if (check_24LC64() != 0xa0) { // searches for i2c devices, returns address of highest found
JonFreeman 0:435bf84ce48a 772 pc.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 5:ca86a7848d54 773 com2.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 0:435bf84ce48a 774 }
JonFreeman 5:ca86a7848d54 775 else { // Found 24LC64 memory on I2C
JonFreeman 5:ca86a7848d54 776 bool k;
JonFreeman 5:ca86a7848d54 777 // static const char ramtst[] = "I found the man sir!";
JonFreeman 5:ca86a7848d54 778 // j = wr_24LC64 (0x1240, (char*)ramtst, strlen(ramtst));
JonFreeman 5:ca86a7848d54 779 // for (int i = 0; i < TXTBUFSIZ; i++) buff[i] = 0; // Clear buffer
JonFreeman 5:ca86a7848d54 780 // // need a way to check i2c busy - YES implemented ack_poll
JonFreeman 5:ca86a7848d54 781 // k = rd_24LC64 (0x1240, buff, strlen(ramtst));
JonFreeman 5:ca86a7848d54 782 // pc.printf("Ram test returned [%s], wr ret'd [%s], rd ret'd [%s]\r\n", buff, j ? "true" : "false", k ? "true" : "false");
JonFreeman 5:ca86a7848d54 783 // com2.printf("Ram test returned [%s], wr ret'd [%s], rd ret'd [%s]\r\n", buff, j ? "true" : "false", k ? "true" : "false");
JonFreeman 6:f289a49c1eae 784 k = rd_24LC64 (0, mode_bytes, 32);
JonFreeman 5:ca86a7848d54 785 // if (k)
JonFreeman 5:ca86a7848d54 786 // com2.printf ("Good read from eeprom\r\n");
JonFreeman 5:ca86a7848d54 787 if (!k)
JonFreeman 5:ca86a7848d54 788 com2.printf ("Error reading from eeprom\r\n");
JonFreeman 5:ca86a7848d54 789
JonFreeman 5:ca86a7848d54 790 int err = 0;
JonFreeman 6:f289a49c1eae 791 for (int i = 0; i < numof_eeprom_options; i++) {
JonFreeman 6:f289a49c1eae 792 if ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max)) {
JonFreeman 5:ca86a7848d54 793 com2.printf ("EEROM error with %s\r\n", option_list[i].t);
JonFreeman 5:ca86a7848d54 794 err++;
JonFreeman 5:ca86a7848d54 795 }
JonFreeman 5:ca86a7848d54 796 // else
JonFreeman 5:ca86a7848d54 797 // com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
JonFreeman 5:ca86a7848d54 798 }
JonFreeman 5:ca86a7848d54 799 IAm = '0';
JonFreeman 5:ca86a7848d54 800 if (err == 0) {
JonFreeman 6:f289a49c1eae 801 MotorA.direction_set (mode_bytes[MOTADIR]);
JonFreeman 6:f289a49c1eae 802 MotorB.direction_set (mode_bytes[MOTBDIR]);
JonFreeman 6:f289a49c1eae 803 IAm = mode_bytes[ID];
JonFreeman 5:ca86a7848d54 804 }
JonFreeman 5:ca86a7848d54 805 // Alternative ID 1 to 9
JonFreeman 5:ca86a7848d54 806 // com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
JonFreeman 5:ca86a7848d54 807 }
JonFreeman 6:f289a49c1eae 808 // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
JonFreeman 5:ca86a7848d54 809 T2 = 0; // T2, T3, T4 As yet unused pins
JonFreeman 0:435bf84ce48a 810 T3 = 0;
JonFreeman 0:435bf84ce48a 811 T4 = 0;
JonFreeman 5:ca86a7848d54 812 // T5 = 0; now input from fw/re on remote control box
JonFreeman 0:435bf84ce48a 813 T6 = 0;
JonFreeman 3:ecb00e0e8d68 814 // MotPtr[0]->set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 815 MotorA.set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 816 MotorB.set_mode (REGENBRAKE);
JonFreeman 5:ca86a7848d54 817 setVI (0.9, 0.5);
JonFreeman 5:ca86a7848d54 818
JonFreeman 4:21d91465e4b1 819 Servos[0] = Servos[1] = NULL;
JonFreeman 4:21d91465e4b1 820 // NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
JonFreeman 4:21d91465e4b1 821 // Only works with unconditional inline code
JonFreeman 4:21d91465e4b1 822 // However, setup code for Input by default,
JonFreeman 4:21d91465e4b1 823 // This can be used to monitor Servo output also !
JonFreeman 4:21d91465e4b1 824 Servo Servo1 (PB_8) ;
JonFreeman 4:21d91465e4b1 825 Servos[0] = & Servo1;
JonFreeman 4:21d91465e4b1 826 Servo Servo2 (PB_9) ;
JonFreeman 4:21d91465e4b1 827 Servos[1] = & Servo2;
JonFreeman 6:f289a49c1eae 828
JonFreeman 6:f289a49c1eae 829 pc.printf ("last_temp_count = %d\r\n", last_temp_count); // Has had time to do at least 1 conversion
JonFreeman 6:f289a49c1eae 830 if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C
JonFreeman 6:f289a49c1eae 831 temp_sensor_exists = true;
JonFreeman 4:21d91465e4b1 832 /*
JonFreeman 0:435bf84ce48a 833 // Setup Complete ! Can now start main control forever loop.
JonFreeman 3:ecb00e0e8d68 834 // March 16th 2018 thoughts !!!
JonFreeman 3:ecb00e0e8d68 835 // Control from one of several sources and types as selected in options bytes in eeprom.
JonFreeman 3:ecb00e0e8d68 836 // Control could be from e.g. Pot, Com2, Servos etc.
JonFreeman 3:ecb00e0e8d68 837 // Suggest e.g.
JonFreeman 4:21d91465e4b1 838 */ /*
JonFreeman 3:ecb00e0e8d68 839 switch (mode_byte) { // executed once only upon startup
JonFreeman 3:ecb00e0e8d68 840 case POT:
JonFreeman 3:ecb00e0e8d68 841 while (1) { // forever loop
JonFreeman 3:ecb00e0e8d68 842 call common_stuff ();
JonFreeman 3:ecb00e0e8d68 843 ...
JonFreeman 3:ecb00e0e8d68 844 }
JonFreeman 3:ecb00e0e8d68 845 break;
JonFreeman 3:ecb00e0e8d68 846 case COM2:
JonFreeman 3:ecb00e0e8d68 847 while (1) { // forever loop
JonFreeman 3:ecb00e0e8d68 848 call common_stuff ();
JonFreeman 3:ecb00e0e8d68 849 ...
JonFreeman 3:ecb00e0e8d68 850 }
JonFreeman 3:ecb00e0e8d68 851 break;
JonFreeman 3:ecb00e0e8d68 852 }
JonFreeman 3:ecb00e0e8d68 853 */
JonFreeman 6:f289a49c1eae 854 pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
JonFreeman 0:435bf84ce48a 855 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 856 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 0:435bf84ce48a 857 command_line_interpreter () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 858 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 0:435bf84ce48a 859 }
JonFreeman 0:435bf84ce48a 860 loop_flag = false; // Clear flag set by ticker interrupt handler
JonFreeman 5:ca86a7848d54 861 MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 5:ca86a7848d54 862 MotorB.pulses_per_sec (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
JonFreeman 3:ecb00e0e8d68 863 T4 = !T4; // toggle to hang scope on to verify loop execution
JonFreeman 0:435bf84ce48a 864 // do stuff
JonFreeman 0:435bf84ce48a 865 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 866 flag_8Hz = false;
JonFreeman 3:ecb00e0e8d68 867 LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 5:ca86a7848d54 868 WatchDog--;
JonFreeman 5:ca86a7848d54 869 if (WatchDog == 0) { // Deal with WatchDog timer timeout here
JonFreeman 5:ca86a7848d54 870 setVI (0.0, 0.0); // set motor volts and amps to zero
JonFreeman 5:ca86a7848d54 871 com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
JonFreeman 5:ca86a7848d54 872 } // End of dealing with WatchDog timer timeout
JonFreeman 5:ca86a7848d54 873 if (WatchDog < 0)
JonFreeman 5:ca86a7848d54 874 WatchDog = 0;
JonFreeman 4:21d91465e4b1 875 eighth_sec_count++;
JonFreeman 4:21d91465e4b1 876 if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 4:21d91465e4b1 877 eighth_sec_count = 0;
JonFreeman 6:f289a49c1eae 878 MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
JonFreeman 6:f289a49c1eae 879 MotorB.current_calc ();
JonFreeman 6:f289a49c1eae 880 if (temp_sensor_exists) {
JonFreeman 6:f289a49c1eae 881 double tmprt = (double) last_temp_count;
JonFreeman 6:f289a49c1eae 882 tmprt /= 16.0;
JonFreeman 6:f289a49c1eae 883 tmprt -= 50.0;
JonFreeman 6:f289a49c1eae 884 pc.printf ("Temp %.2f\r\n", tmprt);
JonFreeman 6:f289a49c1eae 885 }
JonFreeman 5:ca86a7848d54 886 // com2.printf ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IAmin %d, IAave %d, IAmax %d, IB %d, Arpm %d, Brpm %d\r\n", Read_BatteryVolts(), Read_DriverPot(), MotorA.read_Halls (), MotorB.read_Halls (), MotorA.I.min, MotorA.I.ave, MotorA.I.max, MotorB.I.ave, (Apps * 60) / 24, (Bpps * 60) / 24);
JonFreeman 0:435bf84ce48a 887 }
JonFreeman 0:435bf84ce48a 888 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 889 } // End of main programme loop
JonFreeman 0:435bf84ce48a 890 }