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@3:ecb00e0e8d68, 2018-03-18 (annotated)

Committer:
JonFreeman
Date:
Sun Mar 18 08:17:56 2018 +0000
Revision:
3:ecb00e0e8d68
Parent:
2:04761b196473
Child:
4:21d91465e4b1
Starting motors requires high-side mosfet drivers being enabled. Auto tickleup functions now included to switch high sides off and on again to charge high side supply capacitors (now 2u2, up from 100n)

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