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@0:435bf84ce48a, 2018-03-01 (annotated)

Committer:
JonFreeman
Date:
Thu Mar 01 11:29:28 2018 +0000
Revision:
0:435bf84ce48a
Child:
1:0fabe6fdb55b
Tested so far on Nucleo F401RE.; About to experiment reading pressure sensor MPL3115A2 using library code;

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 #include "MPL3115A2.h"
JonFreeman 0:435bf84ce48a 6 /* STM32F401RE - compile using NUCLEO-F401RE
JonFreeman 0:435bf84ce48a 7 // PROJECT - Dual Brushless Motor Controller - March 2018.
JonFreeman 0:435bf84ce48a 8
JonFreeman 0:435bf84ce48a 9 DigitalIn nFault1 (); needs pullup
JonFreeman 0:435bf84ce48a 10 DigitalIn nFault2 (); needs pullup
JonFreeman 0:435bf84ce48a 11 AnalogIn to read each motor current
JonFreeman 0:435bf84ce48a 12
JonFreeman 0:435bf84ce48a 13 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 14 CONTROL PHILOSOPHY
JonFreeman 0:435bf84ce48a 15 This Bogie drive board software should ensure sensible control when commands supplied are not sensible !
JonFreeman 0:435bf84ce48a 16
JonFreeman 0:435bf84ce48a 17 That is, smooth transition between Drive, Coast and Brake to be implemented here.
JonFreeman 0:435bf84ce48a 18 The remote controller must not be relied upon to deliver sensible command sequences.
JonFreeman 0:435bf84ce48a 19
JonFreeman 0:435bf84ce48a 20 So much the better if the remote controller does issue sensible commands, but ...
JonFreeman 0:435bf84ce48a 21
JonFreeman 0:435bf84ce48a 22 ____________________________________________________________________________________
JonFreeman 0:435bf84ce48a 23
JonFreeman 0:435bf84ce48a 24
JonFreeman 0:435bf84ce48a 25 */
JonFreeman 0:435bf84ce48a 26
JonFreeman 0:435bf84ce48a 27 // Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out
JonFreeman 0:435bf84ce48a 28 #define SERVO1_IN
JonFreeman 0:435bf84ce48a 29 //#define SERVO1_OUT
JonFreeman 0:435bf84ce48a 30 //#define SERVO2_IN
JonFreeman 0:435bf84ce48a 31 #define SERVO2_OUT
JonFreeman 0:435bf84ce48a 32
JonFreeman 0:435bf84ce48a 33
JonFreeman 0:435bf84ce48a 34 // Port A -> MotorA, Port B -> MotorB
JonFreeman 0:435bf84ce48a 35 const uint16_t
JonFreeman 0:435bf84ce48a 36 AUL = 1 << 0, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side)
JonFreeman 0:435bf84ce48a 37 AVL = 1 << 6,
JonFreeman 0:435bf84ce48a 38 AWL = 1 << 4,
JonFreeman 0:435bf84ce48a 39
JonFreeman 0:435bf84ce48a 40 AUH = 1 << 1,
JonFreeman 0:435bf84ce48a 41 AVH = 1 << 7,
JonFreeman 0:435bf84ce48a 42 AWH = 1 << 8,
JonFreeman 0:435bf84ce48a 43
JonFreeman 0:435bf84ce48a 44 AUV = AUH | AVL,
JonFreeman 0:435bf84ce48a 45 AVU = AVH | AUL,
JonFreeman 0:435bf84ce48a 46 AUW = AUH | AWL,
JonFreeman 0:435bf84ce48a 47 AWU = AWH | AUL,
JonFreeman 0:435bf84ce48a 48 AVW = AVH | AWL,
JonFreeman 0:435bf84ce48a 49 AWV = AWH | AVL,
JonFreeman 0:435bf84ce48a 50
JonFreeman 0:435bf84ce48a 51 BRA = AUL | AVL | AWL,
JonFreeman 0:435bf84ce48a 52
JonFreeman 0:435bf84ce48a 53 BUL = 1 << 0,
JonFreeman 0:435bf84ce48a 54 BVL = 1 << 1,
JonFreeman 0:435bf84ce48a 55 BWL = 1 << 2,
JonFreeman 0:435bf84ce48a 56
JonFreeman 0:435bf84ce48a 57 BUH = 1 << 10,
JonFreeman 0:435bf84ce48a 58 BVH = 1 << 12,
JonFreeman 0:435bf84ce48a 59 BWH = 1 << 13,
JonFreeman 0:435bf84ce48a 60
JonFreeman 0:435bf84ce48a 61 BUV = BUH | BVL,
JonFreeman 0:435bf84ce48a 62 BVU = BVH | BUL,
JonFreeman 0:435bf84ce48a 63 BUW = BUH | BWL,
JonFreeman 0:435bf84ce48a 64 BWU = BWH | BUL,
JonFreeman 0:435bf84ce48a 65 BVW = BVH | BWL,
JonFreeman 0:435bf84ce48a 66 BWV = BWH | BVL,
JonFreeman 0:435bf84ce48a 67
JonFreeman 0:435bf84ce48a 68 BRB = BUL | BVL | BWL,
JonFreeman 0:435bf84ce48a 69
JonFreeman 0:435bf84ce48a 70 PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH, // NEW METHOD FOR DGD21032 MOSFET DRIVERS
JonFreeman 0:435bf84ce48a 71 PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH;
JonFreeman 0:435bf84ce48a 72
JonFreeman 0:435bf84ce48a 73 PortOut MotA (PortA, PORT_A_MASK);
JonFreeman 0:435bf84ce48a 74 PortOut MotB (PortB, PORT_B_MASK);
JonFreeman 0:435bf84ce48a 75
JonFreeman 0:435bf84ce48a 76 // Pin 1 VBAT NET +3V3
JonFreeman 0:435bf84ce48a 77 DigitalIn J3 (PC_13, PullUp);// Pin 2 Jumper pulls to GND, R floats Hi
JonFreeman 0:435bf84ce48a 78 // Pin 3 PC14-OSC32_IN NET O32I
JonFreeman 0:435bf84ce48a 79 // Pin 4 PC15-OSC32_OUT NET O32O
JonFreeman 0:435bf84ce48a 80 // Pin 5 PH0-OSC_IN NET PH1
JonFreeman 0:435bf84ce48a 81 // Pin 6 PH1-OSC_OUT NET PH1
JonFreeman 0:435bf84ce48a 82 // Pin 7 NRST NET NRST
JonFreeman 0:435bf84ce48a 83 AnalogIn Ain_DriverPot (PC_0); // Pin 8 Spare Analogue in, net SAIN fitted with external pull-down
JonFreeman 0:435bf84ce48a 84 AnalogIn Ain_SystemVolts (PC_1); // Pin 9
JonFreeman 0:435bf84ce48a 85 AnalogIn Motor_A_Current (PC_2); // Pin 10 might as well use up WSRA stock here
JonFreeman 0:435bf84ce48a 86 AnalogIn Motor_B_Current (PC_3); // Pin 11
JonFreeman 0:435bf84ce48a 87 // Pin 12 VSSA/VREF- NET GND
JonFreeman 0:435bf84ce48a 88 // Pin 13 VDDA/VREF+ NET +3V3
JonFreeman 0:435bf84ce48a 89 // Pin 14 Port_A AUL
JonFreeman 0:435bf84ce48a 90 // Pin 15 Port_A AUH
JonFreeman 0:435bf84ce48a 91 // Pins 16, 17 BufferedSerial pc
JonFreeman 0:435bf84ce48a 92 BufferedSerial pc (PA_2, PA_3, 512, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead
JonFreeman 0:435bf84ce48a 93 // Pin 18 VSS NET GND
JonFreeman 0:435bf84ce48a 94 // Pin 19 VDD NET +3V3
JonFreeman 0:435bf84ce48a 95 // Pin 20 Port_A AWL
JonFreeman 0:435bf84ce48a 96 // Pin 21 DigitalOut led1(LED1);
JonFreeman 0:435bf84ce48a 97 DigitalOut LED (PA_5); // Pin 21
JonFreeman 0:435bf84ce48a 98 // Pin 22 Port_A AVL
JonFreeman 0:435bf84ce48a 99 // Pin 23 Port_A AVH
JonFreeman 0:435bf84ce48a 100 InterruptIn MBH2 (PC_4); // Pin 24
JonFreeman 0:435bf84ce48a 101 InterruptIn MBH3 (PC_5); // Pin 25
JonFreeman 0:435bf84ce48a 102 // Pin 26 Port_B BUL
JonFreeman 0:435bf84ce48a 103 // Pin 27 Port_B BVL
JonFreeman 0:435bf84ce48a 104 // Pin 28 Port_B BWL
JonFreeman 0:435bf84ce48a 105 // Pin 29 Port_B BUH
JonFreeman 0:435bf84ce48a 106 // Pin 30 VCAP1
JonFreeman 0:435bf84ce48a 107 // Pin 31 VSS
JonFreeman 0:435bf84ce48a 108 // Pin 32 VDD
JonFreeman 0:435bf84ce48a 109 // Pin 33 Port_B BVH
JonFreeman 0:435bf84ce48a 110 // Pin 34 Port_B BWH
JonFreeman 0:435bf84ce48a 111 DigitalOut T4 (PB_14); // Pin 35
JonFreeman 0:435bf84ce48a 112 DigitalOut T3 (PB_15); // Pin 36
JonFreeman 0:435bf84ce48a 113 // BufferedSerial xb pins 37 Tx, 38 Rx
JonFreeman 0:435bf84ce48a 114 BufferedSerial xb (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module
JonFreeman 0:435bf84ce48a 115 FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3
JonFreeman 0:435bf84ce48a 116 A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4
JonFreeman 0:435bf84ce48a 117 //InterruptIn MotB_Hall (PA_8); // Pin 41
JonFreeman 0:435bf84ce48a 118 // Pin 41 Port_A AWH
JonFreeman 0:435bf84ce48a 119 // BufferedSerial ser3 pins 42 Tx, 43 Rx
JonFreeman 0:435bf84ce48a 120 BufferedSerial ser3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module
JonFreeman 0:435bf84ce48a 121
JonFreeman 0:435bf84ce48a 122 // Feb 2018 Pins 44 and 45 now liberated, could use for serial or other uses
JonFreeman 0:435bf84ce48a 123 //BufferedSerial extra_ser (PA_11, PA_12); // Pins 44, 45 tx, rx to XBee module
JonFreeman 0:435bf84ce48a 124 DigitalOut T2 (PA_11); // Pin 44
JonFreeman 0:435bf84ce48a 125 DigitalOut T1 (PA_12); // Pin 45
JonFreeman 0:435bf84ce48a 126 // Pin 46 SWDIO
JonFreeman 0:435bf84ce48a 127 // Pin 47 VSS
JonFreeman 0:435bf84ce48a 128 // Pin 48 VDD
JonFreeman 0:435bf84ce48a 129 // Pin 49 SWCLK
JonFreeman 0:435bf84ce48a 130 DigitalOut T5 (PA_15); // Pin 50
JonFreeman 0:435bf84ce48a 131 InterruptIn MAH1 (PC_10); // Pin 51
JonFreeman 0:435bf84ce48a 132 InterruptIn MAH2 (PC_11); // Pin 52
JonFreeman 0:435bf84ce48a 133 InterruptIn MAH3 (PC_12); // Pin 53
JonFreeman 0:435bf84ce48a 134 InterruptIn MBH1 (PD_2); // Pin 54
JonFreeman 0:435bf84ce48a 135 DigitalOut T6 (PB_3); // Pin 55
JonFreeman 0:435bf84ce48a 136 FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3
JonFreeman 0:435bf84ce48a 137 B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4
JonFreeman 0:435bf84ce48a 138
JonFreeman 0:435bf84ce48a 139 I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom
JonFreeman 0:435bf84ce48a 140 // Pin 60 BOOT0
JonFreeman 0:435bf84ce48a 141
JonFreeman 0:435bf84ce48a 142 //MPL3115A2(PinName sda, PinName scl, int addr) ;
JonFreeman 0:435bf84ce48a 143 //MPL3115A2 PressureSensor (PB_7, PB_6, 0xa0) ;
JonFreeman 0:435bf84ce48a 144 //double PressureSensor.getPressure ();
JonFreeman 0:435bf84ce48a 145
JonFreeman 0:435bf84ce48a 146 #ifdef SERVO1_IN
JonFreeman 0:435bf84ce48a 147 InterruptIn Servo1_i (PB_8); // Pin 61 to read output from rc rx
JonFreeman 0:435bf84ce48a 148 #endif
JonFreeman 0:435bf84ce48a 149 #ifdef SERVO1_OUT
JonFreeman 0:435bf84ce48a 150 FastPWM Servo1_o (PB_8, 2); //Prescaler 2. This is pwm 4/3
JonFreeman 0:435bf84ce48a 151 #endif
JonFreeman 0:435bf84ce48a 152
JonFreeman 0:435bf84ce48a 153 #ifdef SERVO2_IN
JonFreeman 0:435bf84ce48a 154 InterruptIn Servo2_i (PB_9); // Pin 62 to read output from rc rx
JonFreeman 0:435bf84ce48a 155 #endif
JonFreeman 0:435bf84ce48a 156 #ifdef SERVO2_OUT
JonFreeman 0:435bf84ce48a 157 FastPWM Servo2_o (PB_9, 2); // Prescaler 2. This is pwm 4/4
JonFreeman 0:435bf84ce48a 158 #endif
JonFreeman 0:435bf84ce48a 159
JonFreeman 0:435bf84ce48a 160 // Pin 63 VSS
JonFreeman 0:435bf84ce48a 161 // Pin 64 VDD
JonFreeman 0:435bf84ce48a 162 // SYSTEM CONSTANTS
JonFreeman 0:435bf84ce48a 163
JonFreeman 0:435bf84ce48a 164 /* Please Do Not Alter these */
JonFreeman 0:435bf84ce48a 165 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 166 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 167 MAIN_LOOP_ITERATION_Hz = 1000000 / MAIN_LOOP_REPEAT_TIME_US,
JonFreeman 0:435bf84ce48a 168 CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings
JonFreeman 0:435bf84ce48a 169 HANDBRAKE = 0,
JonFreeman 0:435bf84ce48a 170 FORWARD = 8,
JonFreeman 0:435bf84ce48a 171 REVERSE = 16,
JonFreeman 0:435bf84ce48a 172 REGENBRAKE = 24,
JonFreeman 0:435bf84ce48a 173 PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear
JonFreeman 0:435bf84ce48a 174 MAX_PWM_TICKS = SystemCoreClock / PWM_HZ;
JonFreeman 0:435bf84ce48a 175
JonFreeman 0:435bf84ce48a 176 /* End of Please Do Not Alter these */
JonFreeman 0:435bf84ce48a 177
JonFreeman 0:435bf84ce48a 178 /* Global variable declarations */
JonFreeman 0:435bf84ce48a 179 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 180 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 181 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 182 AtoD_Semaphore = 0;
JonFreeman 0:435bf84ce48a 183 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 0:435bf84ce48a 184 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 0:435bf84ce48a 185 bool sounder_on = false;
JonFreeman 0:435bf84ce48a 186 double test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only
JonFreeman 0:435bf84ce48a 187 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 188
JonFreeman 0:435bf84ce48a 189 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 190 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 0:435bf84ce48a 191
JonFreeman 0:435bf84ce48a 192 // Interrupt Service Routines
JonFreeman 0:435bf84ce48a 193
JonFreeman 0:435bf84ce48a 194 /** void ISR_loop_timer ()
JonFreeman 0:435bf84ce48a 195 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 0:435bf84ce48a 196 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 0:435bf84ce48a 197 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 0:435bf84ce48a 198 */
JonFreeman 0:435bf84ce48a 199 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 200 {
JonFreeman 0:435bf84ce48a 201 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 0:435bf84ce48a 202 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 203 if ((sys_timer & 0x03) == 0)
JonFreeman 0:435bf84ce48a 204 flag_8Hz = true;
JonFreeman 0:435bf84ce48a 205 }
JonFreeman 0:435bf84ce48a 206
JonFreeman 0:435bf84ce48a 207 /** void ISR_voltage_reader ()
JonFreeman 0:435bf84ce48a 208 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 0:435bf84ce48a 209 *
JonFreeman 0:435bf84ce48a 210 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 0:435bf84ce48a 211 */
JonFreeman 0:435bf84ce48a 212
JonFreeman 0:435bf84ce48a 213 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine - few us between readings
JonFreeman 0:435bf84ce48a 214 {
JonFreeman 0:435bf84ce48a 215 AtoD_Semaphore++;
JonFreeman 0:435bf84ce48a 216 }
JonFreeman 0:435bf84ce48a 217
JonFreeman 0:435bf84ce48a 218
JonFreeman 0:435bf84ce48a 219 /*
JonFreeman 0:435bf84ce48a 220 Servo - mutex uses :
JonFreeman 0:435bf84ce48a 221 0. Unused
JonFreeman 0:435bf84ce48a 222 1. Input of pwm from model control Rx
JonFreeman 0:435bf84ce48a 223 2. Output pwm to drive model control servo
JonFreeman 0:435bf84ce48a 224 */
JonFreeman 0:435bf84ce48a 225 //enum {SERVO_UNUSED, SERVO_PWM_IN, SERVO_PWM_OUT} ;
JonFreeman 0:435bf84ce48a 226 class Servo
JonFreeman 0:435bf84ce48a 227 {
JonFreeman 0:435bf84ce48a 228 FastPWM * out;
JonFreeman 0:435bf84ce48a 229 Timer t;
JonFreeman 0:435bf84ce48a 230
JonFreeman 0:435bf84ce48a 231 public:
JonFreeman 0:435bf84ce48a 232
JonFreeman 0:435bf84ce48a 233 bool rx_active;
JonFreeman 0:435bf84ce48a 234 int32_t clock_old, clock_new;
JonFreeman 0:435bf84ce48a 235 int32_t pulse_width_us, period_us;
JonFreeman 0:435bf84ce48a 236 Servo () { // Constructor
JonFreeman 0:435bf84ce48a 237 pulse_width_us = period_us = 0;
JonFreeman 0:435bf84ce48a 238 clock_old = clock_new = 0;
JonFreeman 0:435bf84ce48a 239 out = NULL;
JonFreeman 0:435bf84ce48a 240 rx_active = false;
JonFreeman 0:435bf84ce48a 241 }
JonFreeman 0:435bf84ce48a 242 bool validate_rx () ;
JonFreeman 0:435bf84ce48a 243 void rise () ;
JonFreeman 0:435bf84ce48a 244 void fall () ;
JonFreeman 0:435bf84ce48a 245 void out_pw_set (double);
JonFreeman 0:435bf84ce48a 246 void period_ticks (uint32_t);
JonFreeman 0:435bf84ce48a 247 void pulsewidth_ticks (uint32_t);
JonFreeman 0:435bf84ce48a 248 void set_out (FastPWM *);
JonFreeman 0:435bf84ce48a 249 } S1, S2;
JonFreeman 0:435bf84ce48a 250
JonFreeman 0:435bf84ce48a 251 bool Servo::validate_rx ()
JonFreeman 0:435bf84ce48a 252 {
JonFreeman 0:435bf84ce48a 253 if ((clock() - clock_new) > 4)
JonFreeman 0:435bf84ce48a 254 rx_active = false;
JonFreeman 0:435bf84ce48a 255 else
JonFreeman 0:435bf84ce48a 256 rx_active = true;
JonFreeman 0:435bf84ce48a 257 return rx_active;
JonFreeman 0:435bf84ce48a 258 }
JonFreeman 0:435bf84ce48a 259
JonFreeman 0:435bf84ce48a 260 void Servo::rise ()
JonFreeman 0:435bf84ce48a 261 {
JonFreeman 0:435bf84ce48a 262 t.stop ();
JonFreeman 0:435bf84ce48a 263 period_us = t.read_us ();
JonFreeman 0:435bf84ce48a 264 t.reset ();
JonFreeman 0:435bf84ce48a 265 t.start ();
JonFreeman 0:435bf84ce48a 266 }
JonFreeman 0:435bf84ce48a 267 void Servo::fall ()
JonFreeman 0:435bf84ce48a 268 {
JonFreeman 0:435bf84ce48a 269 pulse_width_us = t.read_us ();
JonFreeman 0:435bf84ce48a 270 clock_old = clock_new;
JonFreeman 0:435bf84ce48a 271 clock_new = clock();
JonFreeman 0:435bf84ce48a 272 if ((clock_new - clock_old) < 4)
JonFreeman 0:435bf84ce48a 273 rx_active = true;
JonFreeman 0:435bf84ce48a 274 }
JonFreeman 0:435bf84ce48a 275 void Servo::out_pw_set (double outpw)
JonFreeman 0:435bf84ce48a 276 {
JonFreeman 0:435bf84ce48a 277 if (outpw > 1.0)
JonFreeman 0:435bf84ce48a 278 outpw = 1.0;
JonFreeman 0:435bf84ce48a 279 if (outpw < 0.0)
JonFreeman 0:435bf84ce48a 280 outpw = 0.0;
JonFreeman 0:435bf84ce48a 281 outpw *= 1.2; // Change range to 1.2 ms to cover out pulse width range 0.9 to 2.1 ms
JonFreeman 0:435bf84ce48a 282 outpw += 0.9; // Bias to nom min servo range
JonFreeman 0:435bf84ce48a 283 pulsewidth_ticks ((uint32_t)(outpw * (SystemCoreClock / 2000)));
JonFreeman 0:435bf84ce48a 284 }
JonFreeman 0:435bf84ce48a 285 void Servo::period_ticks (uint32_t pt)
JonFreeman 0:435bf84ce48a 286 {
JonFreeman 0:435bf84ce48a 287 if (out) out->period_ticks (pt);
JonFreeman 0:435bf84ce48a 288 }
JonFreeman 0:435bf84ce48a 289 void Servo::pulsewidth_ticks (uint32_t wt)
JonFreeman 0:435bf84ce48a 290 {
JonFreeman 0:435bf84ce48a 291 if (out) out->pulsewidth_ticks (wt);
JonFreeman 0:435bf84ce48a 292 }
JonFreeman 0:435bf84ce48a 293 void Servo::set_out (FastPWM * op)
JonFreeman 0:435bf84ce48a 294 {
JonFreeman 0:435bf84ce48a 295 out = op;
JonFreeman 0:435bf84ce48a 296 }
JonFreeman 0:435bf84ce48a 297
JonFreeman 0:435bf84ce48a 298 void Servo1rise ()
JonFreeman 0:435bf84ce48a 299 {
JonFreeman 0:435bf84ce48a 300 S1.rise ();
JonFreeman 0:435bf84ce48a 301 }
JonFreeman 0:435bf84ce48a 302 void Servo1fall ()
JonFreeman 0:435bf84ce48a 303 {
JonFreeman 0:435bf84ce48a 304 S1.fall ();
JonFreeman 0:435bf84ce48a 305 }
JonFreeman 0:435bf84ce48a 306 void Servo2rise ()
JonFreeman 0:435bf84ce48a 307 {
JonFreeman 0:435bf84ce48a 308 S2.rise ();
JonFreeman 0:435bf84ce48a 309 }
JonFreeman 0:435bf84ce48a 310 void Servo2fall ()
JonFreeman 0:435bf84ce48a 311 {
JonFreeman 0:435bf84ce48a 312 S2.fall ();
JonFreeman 0:435bf84ce48a 313 }
JonFreeman 0:435bf84ce48a 314 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 315 /*
JonFreeman 0:435bf84ce48a 316 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 317
JonFreeman 0:435bf84ce48a 318 1 1 1 1 0 0 0 ---___---___
JonFreeman 0:435bf84ce48a 319 2 0 0 1 1 1 0 __---___---_
JonFreeman 0:435bf84ce48a 320 4 1 0 0 0 1 1 -___---___--
JonFreeman 0:435bf84ce48a 321
JonFreeman 0:435bf84ce48a 322 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 0:435bf84ce48a 323 */
JonFreeman 0:435bf84ce48a 324 const uint16_t A_tabl[] = {
JonFreeman 0:435bf84ce48a 325 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 326 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 327 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 0:435bf84ce48a 328 0, BRA,BRA,BRA,BRA,BRA,BRA,0 // Regenerative Braking
JonFreeman 0:435bf84ce48a 329 } ;
JonFreeman 0:435bf84ce48a 330 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 331 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 332 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 333 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 0:435bf84ce48a 334 0, BRB,BRB,BRB,BRB,BRB,BRB,0 // Regenerative Braking
JonFreeman 0:435bf84ce48a 335 } ;
JonFreeman 0:435bf84ce48a 336
JonFreeman 0:435bf84ce48a 337
JonFreeman 0:435bf84ce48a 338 class motor
JonFreeman 0:435bf84ce48a 339 {
JonFreeman 0:435bf84ce48a 340 struct currents {
JonFreeman 0:435bf84ce48a 341 uint32_t max, min, ave;
JonFreeman 0:435bf84ce48a 342 } I;
JonFreeman 0:435bf84ce48a 343 uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
JonFreeman 0:435bf84ce48a 344 uint32_t Hall_tab_ptr, latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 345 const uint16_t * lut;
JonFreeman 0:435bf84ce48a 346 FastPWM * maxV, * maxI;
JonFreeman 0:435bf84ce48a 347 PortOut * Motor_Port;
JonFreeman 0:435bf84ce48a 348 public:
JonFreeman 0:435bf84ce48a 349 uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
JonFreeman 0:435bf84ce48a 350 uint32_t index;
JonFreeman 0:435bf84ce48a 351 motor () {} ; // Default constructor
JonFreeman 0:435bf84ce48a 352 motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * ) ;
JonFreeman 0:435bf84ce48a 353 void set_V_limit (double) ; // Sets max motor voltage
JonFreeman 0:435bf84ce48a 354 void set_I_limit (double) ; // Sets max motor current
JonFreeman 0:435bf84ce48a 355 void Hall_change () ; // Called in response to edge on Hall input pin
JonFreeman 0:435bf84ce48a 356 bool set_mode (int);
JonFreeman 0:435bf84ce48a 357 void current_calc () ;
JonFreeman 0:435bf84ce48a 358 uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
JonFreeman 0:435bf84ce48a 359 } ; //MotorA, MotorB;
JonFreeman 0:435bf84ce48a 360
JonFreeman 0:435bf84ce48a 361 motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl);
JonFreeman 0:435bf84ce48a 362 motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl);
JonFreeman 0:435bf84ce48a 363
JonFreeman 0:435bf84ce48a 364 motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr ) // Constructor
JonFreeman 0:435bf84ce48a 365 {
JonFreeman 0:435bf84ce48a 366 maxV = _maxV_;
JonFreeman 0:435bf84ce48a 367 maxI = _maxI_;
JonFreeman 0:435bf84ce48a 368 Hall_total = mode = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
JonFreeman 0:435bf84ce48a 369 index = 0;
JonFreeman 0:435bf84ce48a 370 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 371 latest_pulses_per_sec = 0;
JonFreeman 0:435bf84ce48a 372 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 0:435bf84ce48a 373 edge_count_table[i] = 0;
JonFreeman 0:435bf84ce48a 374 if (lutptr != A_tabl && lutptr != B_tabl)
JonFreeman 0:435bf84ce48a 375 pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
JonFreeman 0:435bf84ce48a 376 Motor_Port = P;
JonFreeman 0:435bf84ce48a 377 pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
JonFreeman 0:435bf84ce48a 378 maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:435bf84ce48a 379 maxI->period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:435bf84ce48a 380 maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 0:435bf84ce48a 381 maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 0:435bf84ce48a 382 // if (P != PortA && P != PortB)
JonFreeman 0:435bf84ce48a 383 // pc.printf ("Fatal in 'motor' constructor, Invalid Port\r\n");
JonFreeman 0:435bf84ce48a 384 // else
JonFreeman 0:435bf84ce48a 385 // PortOut Motor_P (P, *mask); // PortA for motor A, PortB for motor B
JonFreeman 0:435bf84ce48a 386 lut = lutptr;
JonFreeman 0:435bf84ce48a 387 }
JonFreeman 0:435bf84ce48a 388
JonFreeman 0:435bf84ce48a 389 void motor::current_calc ()
JonFreeman 0:435bf84ce48a 390 {
JonFreeman 0:435bf84ce48a 391 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 0:435bf84ce48a 392 I.max = 0;
JonFreeman 0:435bf84ce48a 393 I.ave = 0;
JonFreeman 0:435bf84ce48a 394 uint16_t sample;
JonFreeman 0:435bf84ce48a 395 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 0:435bf84ce48a 396 sample = current_samples[i];
JonFreeman 0:435bf84ce48a 397 I.ave += sample;
JonFreeman 0:435bf84ce48a 398 if (I.min > sample)
JonFreeman 0:435bf84ce48a 399 I.min = sample;
JonFreeman 0:435bf84ce48a 400 if (I.max < sample)
JonFreeman 0:435bf84ce48a 401 I.max = sample;
JonFreeman 0:435bf84ce48a 402 }
JonFreeman 0:435bf84ce48a 403 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 0:435bf84ce48a 404 }
JonFreeman 0:435bf84ce48a 405
JonFreeman 0:435bf84ce48a 406 void motor::set_V_limit (double p) // Sets max motor voltage
JonFreeman 0:435bf84ce48a 407 {
JonFreeman 0:435bf84ce48a 408 if (p < 0.0)
JonFreeman 0:435bf84ce48a 409 p = 0.0;
JonFreeman 0:435bf84ce48a 410 if (p > 1.0)
JonFreeman 0:435bf84ce48a 411 p = 1.0;
JonFreeman 0:435bf84ce48a 412 // last_pwm = p;
JonFreeman 0:435bf84ce48a 413 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:435bf84ce48a 414 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:435bf84ce48a 415 maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 0:435bf84ce48a 416 }
JonFreeman 0:435bf84ce48a 417
JonFreeman 0:435bf84ce48a 418 void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 0:435bf84ce48a 419 {
JonFreeman 0:435bf84ce48a 420 int a;
JonFreeman 0:435bf84ce48a 421 if (p < 0.0)
JonFreeman 0:435bf84ce48a 422 p = 0.0;
JonFreeman 0:435bf84ce48a 423 if (p > 1.0)
JonFreeman 0:435bf84ce48a 424 p = 1.0;
JonFreeman 0:435bf84ce48a 425 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:435bf84ce48a 426 if (a > MAX_PWM_TICKS)
JonFreeman 0:435bf84ce48a 427 a = MAX_PWM_TICKS;
JonFreeman 0:435bf84ce48a 428 if (a < 0)
JonFreeman 0:435bf84ce48a 429 a = 0;
JonFreeman 0:435bf84ce48a 430 maxI->pulsewidth_ticks (a); // PWM
JonFreeman 0:435bf84ce48a 431 }
JonFreeman 0:435bf84ce48a 432
JonFreeman 0:435bf84ce48a 433 uint32_t motor::pulses_per_sec () // call this once per main loop pass to keep count = edges per sec
JonFreeman 0:435bf84ce48a 434 {
JonFreeman 0:435bf84ce48a 435 uint32_t tmp = Hall_total;
JonFreeman 0:435bf84ce48a 436 latest_pulses_per_sec = tmp - edge_count_table[Hall_tab_ptr];
JonFreeman 0:435bf84ce48a 437 edge_count_table[Hall_tab_ptr] = tmp;
JonFreeman 0:435bf84ce48a 438 Hall_tab_ptr++;
JonFreeman 0:435bf84ce48a 439 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 0:435bf84ce48a 440 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 441 return latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 442 }
JonFreeman 0:435bf84ce48a 443
JonFreeman 0:435bf84ce48a 444 bool motor::set_mode (int m)
JonFreeman 0:435bf84ce48a 445 {
JonFreeman 0:435bf84ce48a 446 if (m != HANDBRAKE && m != FORWARD && m != REVERSE && m !=REGENBRAKE)
JonFreeman 0:435bf84ce48a 447 return false;
JonFreeman 0:435bf84ce48a 448 mode = m;
JonFreeman 0:435bf84ce48a 449 return true;
JonFreeman 0:435bf84ce48a 450 }
JonFreeman 0:435bf84ce48a 451
JonFreeman 0:435bf84ce48a 452 void motor::Hall_change ()
JonFreeman 0:435bf84ce48a 453 {
JonFreeman 0:435bf84ce48a 454 Hall_total++;
JonFreeman 0:435bf84ce48a 455 mode &= 0x038L; // safety
JonFreeman 0:435bf84ce48a 456 *Motor_Port = lut[mode + index];
JonFreeman 0:435bf84ce48a 457 }
JonFreeman 0:435bf84ce48a 458 void MAH1r ()
JonFreeman 0:435bf84ce48a 459 {
JonFreeman 0:435bf84ce48a 460 MotorA.index = 1;
JonFreeman 0:435bf84ce48a 461 if (MAH2 != 0) MotorA.index |= 2;
JonFreeman 0:435bf84ce48a 462 if (MAH3 != 0) MotorA.index |= 4;
JonFreeman 0:435bf84ce48a 463 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 464 }
JonFreeman 0:435bf84ce48a 465 void MAH1f ()
JonFreeman 0:435bf84ce48a 466 {
JonFreeman 0:435bf84ce48a 467 MotorA.index = 0;
JonFreeman 0:435bf84ce48a 468 if (MAH2 != 0) MotorA.index |= 2;
JonFreeman 0:435bf84ce48a 469 if (MAH3 != 0) MotorA.index |= 4;
JonFreeman 0:435bf84ce48a 470 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 471 }
JonFreeman 0:435bf84ce48a 472 void MAH2r ()
JonFreeman 0:435bf84ce48a 473 {
JonFreeman 0:435bf84ce48a 474 MotorA.index = 2;
JonFreeman 0:435bf84ce48a 475 if (MAH1 != 0) MotorA.index |= 1;
JonFreeman 0:435bf84ce48a 476 if (MAH3 != 0) MotorA.index |= 4;
JonFreeman 0:435bf84ce48a 477 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 478 }
JonFreeman 0:435bf84ce48a 479 void MAH2f ()
JonFreeman 0:435bf84ce48a 480 {
JonFreeman 0:435bf84ce48a 481 MotorA.index = 0;
JonFreeman 0:435bf84ce48a 482 if (MAH1 != 0) MotorA.index |= 1;
JonFreeman 0:435bf84ce48a 483 if (MAH3 != 0) MotorA.index |= 4;
JonFreeman 0:435bf84ce48a 484 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 485 }
JonFreeman 0:435bf84ce48a 486 void MAH3r ()
JonFreeman 0:435bf84ce48a 487 {
JonFreeman 0:435bf84ce48a 488 MotorA.index = 4;
JonFreeman 0:435bf84ce48a 489 if (MAH1 != 0) MotorA.index |= 1;
JonFreeman 0:435bf84ce48a 490 if (MAH2 != 0) MotorA.index |= 2;
JonFreeman 0:435bf84ce48a 491 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 492 }
JonFreeman 0:435bf84ce48a 493 void MAH3f ()
JonFreeman 0:435bf84ce48a 494 {
JonFreeman 0:435bf84ce48a 495 MotorA.index = 0;
JonFreeman 0:435bf84ce48a 496 if (MAH1 != 0) MotorA.index |= 1;
JonFreeman 0:435bf84ce48a 497 if (MAH2 != 0) MotorA.index |= 2;
JonFreeman 0:435bf84ce48a 498 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 499 }
JonFreeman 0:435bf84ce48a 500
JonFreeman 0:435bf84ce48a 501 void MBH1r ()
JonFreeman 0:435bf84ce48a 502 {
JonFreeman 0:435bf84ce48a 503 MotorB.index = 1;
JonFreeman 0:435bf84ce48a 504 if (MBH2 != 0) MotorB.index |= 2;
JonFreeman 0:435bf84ce48a 505 if (MBH3 != 0) MotorB.index |= 4;
JonFreeman 0:435bf84ce48a 506 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 507 }
JonFreeman 0:435bf84ce48a 508 void MBH1f ()
JonFreeman 0:435bf84ce48a 509 {
JonFreeman 0:435bf84ce48a 510 MotorB.index = 0;
JonFreeman 0:435bf84ce48a 511 if (MBH2 != 0) MotorB.index |= 2;
JonFreeman 0:435bf84ce48a 512 if (MBH3 != 0) MotorB.index |= 4;
JonFreeman 0:435bf84ce48a 513 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 514 }
JonFreeman 0:435bf84ce48a 515 void MBH2r ()
JonFreeman 0:435bf84ce48a 516 {
JonFreeman 0:435bf84ce48a 517 MotorB.index = 2;
JonFreeman 0:435bf84ce48a 518 if (MBH1 != 0) MotorB.index |= 1;
JonFreeman 0:435bf84ce48a 519 if (MBH3 != 0) MotorB.index |= 4;
JonFreeman 0:435bf84ce48a 520 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 521 }
JonFreeman 0:435bf84ce48a 522 void MBH2f ()
JonFreeman 0:435bf84ce48a 523 {
JonFreeman 0:435bf84ce48a 524 MotorB.index = 0;
JonFreeman 0:435bf84ce48a 525 if (MBH1 != 0) MotorB.index |= 1;
JonFreeman 0:435bf84ce48a 526 if (MBH3 != 0) MotorB.index |= 4;
JonFreeman 0:435bf84ce48a 527 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 528 }
JonFreeman 0:435bf84ce48a 529 void MBH3r ()
JonFreeman 0:435bf84ce48a 530 {
JonFreeman 0:435bf84ce48a 531 MotorB.index = 4;
JonFreeman 0:435bf84ce48a 532 if (MBH1 != 0) MotorB.index |= 1;
JonFreeman 0:435bf84ce48a 533 if (MBH2 != 0) MotorB.index |= 2;
JonFreeman 0:435bf84ce48a 534 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 535 }
JonFreeman 0:435bf84ce48a 536 void MBH3f ()
JonFreeman 0:435bf84ce48a 537 {
JonFreeman 0:435bf84ce48a 538 MotorB.index = 0;
JonFreeman 0:435bf84ce48a 539 if (MBH1 != 0) MotorB.index |= 1;
JonFreeman 0:435bf84ce48a 540 if (MBH2 != 0) MotorB.index |= 2;
JonFreeman 0:435bf84ce48a 541 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 542 }
JonFreeman 0:435bf84ce48a 543
JonFreeman 0:435bf84ce48a 544
JonFreeman 0:435bf84ce48a 545 // End of Interrupt Service Routines
JonFreeman 0:435bf84ce48a 546
JonFreeman 0:435bf84ce48a 547 void buggery_fuck () // simulate hall movement to observe port output bits
JonFreeman 0:435bf84ce48a 548 {
JonFreeman 0:435bf84ce48a 549 /* MotorA.index++;
JonFreeman 0:435bf84ce48a 550 if (MotorA.index > 6)
JonFreeman 0:435bf84ce48a 551 MotorA.index = 1;
JonFreeman 0:435bf84ce48a 552 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 553 */
JonFreeman 0:435bf84ce48a 554 }
JonFreeman 0:435bf84ce48a 555 void AtoD_reader ()
JonFreeman 0:435bf84ce48a 556 {
JonFreeman 0:435bf84ce48a 557 static uint32_t i = 0, tab_ptr = 0;
JonFreeman 0:435bf84ce48a 558 if (AtoD_Semaphore > 20) {
JonFreeman 0:435bf84ce48a 559 pc.printf ("WARNING - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore);
JonFreeman 0:435bf84ce48a 560 AtoD_Semaphore = 20;
JonFreeman 0:435bf84ce48a 561 }
JonFreeman 0:435bf84ce48a 562 while (AtoD_Semaphore > 0) {
JonFreeman 0:435bf84ce48a 563 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 564 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 565 switch (i) { //
JonFreeman 0:435bf84ce48a 566 case 0:
JonFreeman 0:435bf84ce48a 567 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 568 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 569 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 570 case 1:
JonFreeman 0:435bf84ce48a 571 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 572 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 573 break;
JonFreeman 0:435bf84ce48a 574 case 2:
JonFreeman 0:435bf84ce48a 575 MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 576 break;
JonFreeman 0:435bf84ce48a 577 case 3:
JonFreeman 0:435bf84ce48a 578 MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 579 if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter
JonFreeman 0:435bf84ce48a 580 tab_ptr = 0;
JonFreeman 0:435bf84ce48a 581 break;
JonFreeman 0:435bf84ce48a 582 }
JonFreeman 0:435bf84ce48a 583 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 584 if (i > 3)
JonFreeman 0:435bf84ce48a 585 i = 0;
JonFreeman 0:435bf84ce48a 586 }
JonFreeman 0:435bf84ce48a 587 }
JonFreeman 0:435bf84ce48a 588
JonFreeman 0:435bf84ce48a 589 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 590 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 0:435bf84ce48a 591 * ISR also filters signal
JonFreeman 0:435bf84ce48a 592 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 593 */
JonFreeman 0:435bf84ce48a 594 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 595 {
JonFreeman 0:435bf84ce48a 596 #ifdef KNIFEANDFORK
JonFreeman 0:435bf84ce48a 597 return test_pot; // may be altered using cli
JonFreeman 0:435bf84ce48a 598 #else
JonFreeman 0:435bf84ce48a 599 return (double) driverpot_reading / 65535.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 600 #endif
JonFreeman 0:435bf84ce48a 601 }
JonFreeman 0:435bf84ce48a 602
JonFreeman 0:435bf84ce48a 603 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 604 {
JonFreeman 0:435bf84ce48a 605 return (double) volt_reading / 1800.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 606 }
JonFreeman 0:435bf84ce48a 607
JonFreeman 0:435bf84ce48a 608 double read_volts () // A test function
JonFreeman 0:435bf84ce48a 609 {
JonFreeman 0:435bf84ce48a 610 pc.printf ("pot = %.4f, System Voltage = %.2f\r\n", Read_DriverPot(), Read_BatteryVolts());
JonFreeman 0:435bf84ce48a 611 return Read_BatteryVolts();
JonFreeman 0:435bf84ce48a 612 }
JonFreeman 0:435bf84ce48a 613
JonFreeman 0:435bf84ce48a 614
JonFreeman 0:435bf84ce48a 615 extern void command_line_interpreter () ;
JonFreeman 0:435bf84ce48a 616 extern int check_24LC64 () ; // Call from near top of main() to init i2c bus
JonFreeman 0:435bf84ce48a 617 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 0:435bf84ce48a 618 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 0:435bf84ce48a 619
JonFreeman 0:435bf84ce48a 620 int main()
JonFreeman 0:435bf84ce48a 621 {
JonFreeman 0:435bf84ce48a 622 int j = 0;
JonFreeman 0:435bf84ce48a 623 uint32_t Apps, Bpps;
JonFreeman 0:435bf84ce48a 624
JonFreeman 0:435bf84ce48a 625 MotA = 0; // Motor drive ports A and B
JonFreeman 0:435bf84ce48a 626 MotB = 0;
JonFreeman 0:435bf84ce48a 627 MAH1.rise (& MAH1r);
JonFreeman 0:435bf84ce48a 628 MAH1.fall (& MAH1f);
JonFreeman 0:435bf84ce48a 629 MAH2.rise (& MAH2r);
JonFreeman 0:435bf84ce48a 630 MAH2.fall (& MAH2f);
JonFreeman 0:435bf84ce48a 631 MAH3.rise (& MAH3r);
JonFreeman 0:435bf84ce48a 632 MAH3.fall (& MAH3f);
JonFreeman 0:435bf84ce48a 633
JonFreeman 0:435bf84ce48a 634 MBH1.rise (& MBH1r);
JonFreeman 0:435bf84ce48a 635 MBH1.fall (& MBH1f);
JonFreeman 0:435bf84ce48a 636 MBH2.rise (& MBH2r);
JonFreeman 0:435bf84ce48a 637 MBH2.fall (& MBH2f);
JonFreeman 0:435bf84ce48a 638 MBH3.rise (& MBH3r);
JonFreeman 0:435bf84ce48a 639 MBH3.fall (& MBH3f);
JonFreeman 0:435bf84ce48a 640
JonFreeman 0:435bf84ce48a 641 MAH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 642 MAH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 643 MAH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 644 MBH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 645 MBH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 646 MBH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 647 pc.printf ("\tAbandon Hope %d\r\n", LED ? 0 : 1);
JonFreeman 0:435bf84ce48a 648 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 0:435bf84ce48a 649 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 650 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 651 // Done setting up system interrupt timers
JonFreeman 0:435bf84ce48a 652
JonFreeman 0:435bf84ce48a 653 const int TXTBUFSIZ = 36;
JonFreeman 0:435bf84ce48a 654 char buff[TXTBUFSIZ];
JonFreeman 0:435bf84ce48a 655 bool eerom_detected = false;
JonFreeman 0:435bf84ce48a 656 pc.printf ("RAM test - ");
JonFreeman 0:435bf84ce48a 657 if (check_24LC64() != 0xa0) // searches for i2c devices, returns address of highest found
JonFreeman 0:435bf84ce48a 658 pc.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 0:435bf84ce48a 659 else // i2c.write returned 0, think this means device responded with 'ACK', found it anyway
JonFreeman 0:435bf84ce48a 660 eerom_detected = true;
JonFreeman 0:435bf84ce48a 661 if (eerom_detected) {
JonFreeman 0:435bf84ce48a 662 bool j, k;
JonFreeman 0:435bf84ce48a 663 pc.printf ("ok\r\n");
JonFreeman 0:435bf84ce48a 664 static const char ramtst[] = "I found the man sir!";
JonFreeman 0:435bf84ce48a 665 j = wr_24LC64 (0x1240, (char*)ramtst, strlen(ramtst));
JonFreeman 0:435bf84ce48a 666 for (int i = 0; i < TXTBUFSIZ; i++) buff[i] = 0; // Clear buffer
JonFreeman 0:435bf84ce48a 667 // need a way to check i2c busy - YES implemented ack_poll
JonFreeman 0:435bf84ce48a 668 k = rd_24LC64 (0x1240, buff, strlen(ramtst));
JonFreeman 0:435bf84ce48a 669 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 670 }
JonFreeman 0:435bf84ce48a 671 T1 = 0; // As yet unused pins
JonFreeman 0:435bf84ce48a 672 T2 = 0;
JonFreeman 0:435bf84ce48a 673 T3 = 0;
JonFreeman 0:435bf84ce48a 674 T4 = 0;
JonFreeman 0:435bf84ce48a 675 T5 = 0;
JonFreeman 0:435bf84ce48a 676 T6 = 0;
JonFreeman 0:435bf84ce48a 677 // MotorA.set_mode (HANDBRAKE);
JonFreeman 0:435bf84ce48a 678 // MotorB.set_mode (HANDBRAKE);
JonFreeman 0:435bf84ce48a 679 MotorA.set_mode (FORWARD);
JonFreeman 0:435bf84ce48a 680 MotorB.set_mode (FORWARD);
JonFreeman 0:435bf84ce48a 681 MotorA.set_V_limit (0.1);
JonFreeman 0:435bf84ce48a 682 MotorB.set_V_limit (0.0);
JonFreeman 0:435bf84ce48a 683 MotorA.set_I_limit (0.0);
JonFreeman 0:435bf84ce48a 684 MotorB.set_I_limit (0.0);
JonFreeman 0:435bf84ce48a 685
JonFreeman 0:435bf84ce48a 686 // Setup Complete ! Can now start main control forever loop.
JonFreeman 0:435bf84ce48a 687
JonFreeman 0:435bf84ce48a 688 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 689 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 0:435bf84ce48a 690 command_line_interpreter () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 691 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 0:435bf84ce48a 692 }
JonFreeman 0:435bf84ce48a 693 loop_flag = false; // Clear flag set by ticker interrupt handler
JonFreeman 0:435bf84ce48a 694 Apps = MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 0:435bf84ce48a 695 Bpps = MotorB.pulses_per_sec ();
JonFreeman 0:435bf84ce48a 696
JonFreeman 0:435bf84ce48a 697 buggery_fuck ();
JonFreeman 0:435bf84ce48a 698
JonFreeman 0:435bf84ce48a 699 // do stuff
JonFreeman 0:435bf84ce48a 700 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 701 flag_8Hz = false;
JonFreeman 0:435bf84ce48a 702 LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 0:435bf84ce48a 703 j++;
JonFreeman 0:435bf84ce48a 704 if (j > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 0:435bf84ce48a 705 j = 0;
JonFreeman 0:435bf84ce48a 706 //double pres = PressureSensor.getPressure ();
JonFreeman 0:435bf84ce48a 707 //double pres = PressureSensor.getTemperature ();
JonFreeman 0:435bf84ce48a 708 pc.printf ("Apps %d, Bpps %d, sys_timer %d\r\n", Apps, Bpps, sys_timer);
JonFreeman 0:435bf84ce48a 709 // pc.printf ("Apps %d, Bpps %d, sys_timer %d, pressure %.2f\r\n", Apps, Bpps, sys_timer, pres);
JonFreeman 0:435bf84ce48a 710 // pc.printf ("V=%+.1f, I=%+.1f, CtrlOut %.3f, RPM %d, %s\r\n", Read_BatteryVolts(), AmpsReading, ControlOut, ReadEngineRPM(), engine_warm ? "Running mode" : "Startup mode");
JonFreeman 0:435bf84ce48a 711 }
JonFreeman 0:435bf84ce48a 712 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 713 } // End of main programme loop
JonFreeman 0:435bf84ce48a 714 }