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@1:0fabe6fdb55b, 2018-03-07 (annotated)

Committer:
JonFreeman
Date:
Wed Mar 07 08:29:18 2018 +0000
Revision:
1:0fabe6fdb55b
Parent:
0:435bf84ce48a
Child:
2:04761b196473
About to start testing twin bldc motor controller board STM2;

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 0:435bf84ce48a 50 BRA = AUL | AVL | AWL,
JonFreeman 0:435bf84ce48a 51
JonFreeman 0:435bf84ce48a 52 BUL = 1 << 0,
JonFreeman 0:435bf84ce48a 53 BVL = 1 << 1,
JonFreeman 0:435bf84ce48a 54 BWL = 1 << 2,
JonFreeman 0:435bf84ce48a 55
JonFreeman 0:435bf84ce48a 56 BUH = 1 << 10,
JonFreeman 0:435bf84ce48a 57 BVH = 1 << 12,
JonFreeman 0:435bf84ce48a 58 BWH = 1 << 13,
JonFreeman 0:435bf84ce48a 59
JonFreeman 0:435bf84ce48a 60 BUV = BUH | BVL,
JonFreeman 0:435bf84ce48a 61 BVU = BVH | BUL,
JonFreeman 0:435bf84ce48a 62 BUW = BUH | BWL,
JonFreeman 0:435bf84ce48a 63 BWU = BWH | BUL,
JonFreeman 0:435bf84ce48a 64 BVW = BVH | BWL,
JonFreeman 0:435bf84ce48a 65 BWV = BWH | BVL,
JonFreeman 0:435bf84ce48a 66
JonFreeman 0:435bf84ce48a 67 BRB = BUL | BVL | BWL,
JonFreeman 0:435bf84ce48a 68
JonFreeman 0:435bf84ce48a 69 PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH, // NEW METHOD FOR DGD21032 MOSFET DRIVERS
JonFreeman 0:435bf84ce48a 70 PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH;
JonFreeman 0:435bf84ce48a 71
JonFreeman 0:435bf84ce48a 72 PortOut MotA (PortA, PORT_A_MASK);
JonFreeman 0:435bf84ce48a 73 PortOut MotB (PortB, PORT_B_MASK);
JonFreeman 0:435bf84ce48a 74
JonFreeman 0:435bf84ce48a 75 // Pin 1 VBAT NET +3V3
JonFreeman 0:435bf84ce48a 76 DigitalIn J3 (PC_13, PullUp);// Pin 2 Jumper pulls to GND, R floats Hi
JonFreeman 0:435bf84ce48a 77 // Pin 3 PC14-OSC32_IN NET O32I
JonFreeman 0:435bf84ce48a 78 // Pin 4 PC15-OSC32_OUT NET O32O
JonFreeman 0:435bf84ce48a 79 // Pin 5 PH0-OSC_IN NET PH1
JonFreeman 0:435bf84ce48a 80 // Pin 6 PH1-OSC_OUT NET PH1
JonFreeman 0:435bf84ce48a 81 // Pin 7 NRST NET NRST
JonFreeman 0:435bf84ce48a 82 AnalogIn Ain_DriverPot (PC_0); // Pin 8 Spare Analogue in, net SAIN fitted with external pull-down
JonFreeman 0:435bf84ce48a 83 AnalogIn Ain_SystemVolts (PC_1); // Pin 9
JonFreeman 0:435bf84ce48a 84 AnalogIn Motor_A_Current (PC_2); // Pin 10 might as well use up WSRA stock here
JonFreeman 0:435bf84ce48a 85 AnalogIn Motor_B_Current (PC_3); // Pin 11
JonFreeman 0:435bf84ce48a 86 // Pin 12 VSSA/VREF- NET GND
JonFreeman 0:435bf84ce48a 87 // Pin 13 VDDA/VREF+ NET +3V3
JonFreeman 0:435bf84ce48a 88 // Pin 14 Port_A AUL
JonFreeman 0:435bf84ce48a 89 // Pin 15 Port_A AUH
JonFreeman 0:435bf84ce48a 90 // Pins 16, 17 BufferedSerial pc
JonFreeman 0:435bf84ce48a 91 BufferedSerial pc (PA_2, PA_3, 512, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead
JonFreeman 0:435bf84ce48a 92 // Pin 18 VSS NET GND
JonFreeman 0:435bf84ce48a 93 // Pin 19 VDD NET +3V3
JonFreeman 0:435bf84ce48a 94 // Pin 20 Port_A AWL
JonFreeman 0:435bf84ce48a 95 // Pin 21 DigitalOut led1(LED1);
JonFreeman 0:435bf84ce48a 96 DigitalOut LED (PA_5); // Pin 21
JonFreeman 0:435bf84ce48a 97 // Pin 22 Port_A AVL
JonFreeman 0:435bf84ce48a 98 // Pin 23 Port_A AVH
JonFreeman 0:435bf84ce48a 99 InterruptIn MBH2 (PC_4); // Pin 24
JonFreeman 0:435bf84ce48a 100 InterruptIn MBH3 (PC_5); // Pin 25
JonFreeman 0:435bf84ce48a 101 // Pin 26 Port_B BUL
JonFreeman 0:435bf84ce48a 102 // Pin 27 Port_B BVL
JonFreeman 0:435bf84ce48a 103 // Pin 28 Port_B BWL
JonFreeman 0:435bf84ce48a 104 // Pin 29 Port_B BUH
JonFreeman 0:435bf84ce48a 105 // Pin 30 VCAP1
JonFreeman 0:435bf84ce48a 106 // Pin 31 VSS
JonFreeman 0:435bf84ce48a 107 // Pin 32 VDD
JonFreeman 0:435bf84ce48a 108 // Pin 33 Port_B BVH
JonFreeman 0:435bf84ce48a 109 // Pin 34 Port_B BWH
JonFreeman 0:435bf84ce48a 110 DigitalOut T4 (PB_14); // Pin 35
JonFreeman 0:435bf84ce48a 111 DigitalOut T3 (PB_15); // Pin 36
JonFreeman 0:435bf84ce48a 112 // BufferedSerial xb pins 37 Tx, 38 Rx
JonFreeman 0:435bf84ce48a 113 BufferedSerial xb (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module
JonFreeman 0:435bf84ce48a 114 FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3
JonFreeman 0:435bf84ce48a 115 A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4
JonFreeman 0:435bf84ce48a 116 //InterruptIn MotB_Hall (PA_8); // Pin 41
JonFreeman 0:435bf84ce48a 117 // Pin 41 Port_A AWH
JonFreeman 0:435bf84ce48a 118 // BufferedSerial ser3 pins 42 Tx, 43 Rx
JonFreeman 0:435bf84ce48a 119 BufferedSerial ser3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module
JonFreeman 0:435bf84ce48a 120
JonFreeman 0:435bf84ce48a 121 // Feb 2018 Pins 44 and 45 now liberated, could use for serial or other uses
JonFreeman 0:435bf84ce48a 122 //BufferedSerial extra_ser (PA_11, PA_12); // Pins 44, 45 tx, rx to XBee module
JonFreeman 0:435bf84ce48a 123 DigitalOut T2 (PA_11); // Pin 44
JonFreeman 0:435bf84ce48a 124 DigitalOut T1 (PA_12); // Pin 45
JonFreeman 0:435bf84ce48a 125 // Pin 46 SWDIO
JonFreeman 0:435bf84ce48a 126 // Pin 47 VSS
JonFreeman 0:435bf84ce48a 127 // Pin 48 VDD
JonFreeman 0:435bf84ce48a 128 // Pin 49 SWCLK
JonFreeman 0:435bf84ce48a 129 DigitalOut T5 (PA_15); // Pin 50
JonFreeman 0:435bf84ce48a 130 InterruptIn MAH1 (PC_10); // Pin 51
JonFreeman 0:435bf84ce48a 131 InterruptIn MAH2 (PC_11); // Pin 52
JonFreeman 0:435bf84ce48a 132 InterruptIn MAH3 (PC_12); // Pin 53
JonFreeman 0:435bf84ce48a 133 InterruptIn MBH1 (PD_2); // Pin 54
JonFreeman 0:435bf84ce48a 134 DigitalOut T6 (PB_3); // Pin 55
JonFreeman 0:435bf84ce48a 135 FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3
JonFreeman 0:435bf84ce48a 136 B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4
JonFreeman 0:435bf84ce48a 137
JonFreeman 0:435bf84ce48a 138 I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom
JonFreeman 0:435bf84ce48a 139 // Pin 60 BOOT0
JonFreeman 0:435bf84ce48a 140
JonFreeman 0:435bf84ce48a 141 #ifdef SERVO1_IN
JonFreeman 0:435bf84ce48a 142 InterruptIn Servo1_i (PB_8); // Pin 61 to read output from rc rx
JonFreeman 0:435bf84ce48a 143 #endif
JonFreeman 0:435bf84ce48a 144 #ifdef SERVO1_OUT
JonFreeman 0:435bf84ce48a 145 FastPWM Servo1_o (PB_8, 2); //Prescaler 2. This is pwm 4/3
JonFreeman 0:435bf84ce48a 146 #endif
JonFreeman 0:435bf84ce48a 147
JonFreeman 0:435bf84ce48a 148 #ifdef SERVO2_IN
JonFreeman 0:435bf84ce48a 149 InterruptIn Servo2_i (PB_9); // Pin 62 to read output from rc rx
JonFreeman 0:435bf84ce48a 150 #endif
JonFreeman 0:435bf84ce48a 151 #ifdef SERVO2_OUT
JonFreeman 0:435bf84ce48a 152 FastPWM Servo2_o (PB_9, 2); // Prescaler 2. This is pwm 4/4
JonFreeman 0:435bf84ce48a 153 #endif
JonFreeman 0:435bf84ce48a 154
JonFreeman 0:435bf84ce48a 155 // Pin 63 VSS
JonFreeman 0:435bf84ce48a 156 // Pin 64 VDD
JonFreeman 0:435bf84ce48a 157 // SYSTEM CONSTANTS
JonFreeman 0:435bf84ce48a 158
JonFreeman 0:435bf84ce48a 159 /* Please Do Not Alter these */
JonFreeman 0:435bf84ce48a 160 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 161 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 162 MAIN_LOOP_ITERATION_Hz = 1000000 / MAIN_LOOP_REPEAT_TIME_US,
JonFreeman 0:435bf84ce48a 163 CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings
JonFreeman 0:435bf84ce48a 164 HANDBRAKE = 0,
JonFreeman 0:435bf84ce48a 165 FORWARD = 8,
JonFreeman 0:435bf84ce48a 166 REVERSE = 16,
JonFreeman 0:435bf84ce48a 167 REGENBRAKE = 24,
JonFreeman 0:435bf84ce48a 168 PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear
JonFreeman 0:435bf84ce48a 169 MAX_PWM_TICKS = SystemCoreClock / PWM_HZ;
JonFreeman 0:435bf84ce48a 170
JonFreeman 0:435bf84ce48a 171 /* End of Please Do Not Alter these */
JonFreeman 0:435bf84ce48a 172
JonFreeman 0:435bf84ce48a 173 /* Global variable declarations */
JonFreeman 0:435bf84ce48a 174 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 175 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 176 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 177 AtoD_Semaphore = 0;
JonFreeman 0:435bf84ce48a 178 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 0:435bf84ce48a 179 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 0:435bf84ce48a 180 bool sounder_on = false;
JonFreeman 0:435bf84ce48a 181 double test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only
JonFreeman 0:435bf84ce48a 182 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 183
JonFreeman 0:435bf84ce48a 184 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 185 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 0:435bf84ce48a 186
JonFreeman 0:435bf84ce48a 187 // Interrupt Service Routines
JonFreeman 0:435bf84ce48a 188
JonFreeman 0:435bf84ce48a 189 /** void ISR_loop_timer ()
JonFreeman 0:435bf84ce48a 190 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 0:435bf84ce48a 191 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 0:435bf84ce48a 192 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 0:435bf84ce48a 193 */
JonFreeman 0:435bf84ce48a 194 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 195 {
JonFreeman 0:435bf84ce48a 196 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 0:435bf84ce48a 197 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 198 if ((sys_timer & 0x03) == 0)
JonFreeman 0:435bf84ce48a 199 flag_8Hz = true;
JonFreeman 0:435bf84ce48a 200 }
JonFreeman 0:435bf84ce48a 201
JonFreeman 0:435bf84ce48a 202 /** void ISR_voltage_reader ()
JonFreeman 0:435bf84ce48a 203 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 0:435bf84ce48a 204 *
JonFreeman 0:435bf84ce48a 205 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 0:435bf84ce48a 206 */
JonFreeman 0:435bf84ce48a 207
JonFreeman 0:435bf84ce48a 208 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine - few us between readings
JonFreeman 0:435bf84ce48a 209 {
JonFreeman 0:435bf84ce48a 210 AtoD_Semaphore++;
JonFreeman 0:435bf84ce48a 211 }
JonFreeman 0:435bf84ce48a 212
JonFreeman 0:435bf84ce48a 213
JonFreeman 0:435bf84ce48a 214 /*
JonFreeman 0:435bf84ce48a 215 Servo - mutex uses :
JonFreeman 0:435bf84ce48a 216 0. Unused
JonFreeman 0:435bf84ce48a 217 1. Input of pwm from model control Rx
JonFreeman 0:435bf84ce48a 218 2. Output pwm to drive model control servo
JonFreeman 0:435bf84ce48a 219 */
JonFreeman 0:435bf84ce48a 220 //enum {SERVO_UNUSED, SERVO_PWM_IN, SERVO_PWM_OUT} ;
JonFreeman 0:435bf84ce48a 221 class Servo
JonFreeman 0:435bf84ce48a 222 {
JonFreeman 0:435bf84ce48a 223 FastPWM * out;
JonFreeman 0:435bf84ce48a 224 Timer t;
JonFreeman 0:435bf84ce48a 225
JonFreeman 0:435bf84ce48a 226 public:
JonFreeman 0:435bf84ce48a 227
JonFreeman 0:435bf84ce48a 228 bool rx_active;
JonFreeman 0:435bf84ce48a 229 int32_t clock_old, clock_new;
JonFreeman 0:435bf84ce48a 230 int32_t pulse_width_us, period_us;
JonFreeman 0:435bf84ce48a 231 Servo () { // Constructor
JonFreeman 0:435bf84ce48a 232 pulse_width_us = period_us = 0;
JonFreeman 0:435bf84ce48a 233 clock_old = clock_new = 0;
JonFreeman 0:435bf84ce48a 234 out = NULL;
JonFreeman 0:435bf84ce48a 235 rx_active = false;
JonFreeman 0:435bf84ce48a 236 }
JonFreeman 0:435bf84ce48a 237 bool validate_rx () ;
JonFreeman 0:435bf84ce48a 238 void rise () ;
JonFreeman 0:435bf84ce48a 239 void fall () ;
JonFreeman 0:435bf84ce48a 240 void out_pw_set (double);
JonFreeman 0:435bf84ce48a 241 void period_ticks (uint32_t);
JonFreeman 0:435bf84ce48a 242 void pulsewidth_ticks (uint32_t);
JonFreeman 0:435bf84ce48a 243 void set_out (FastPWM *);
JonFreeman 0:435bf84ce48a 244 } S1, S2;
JonFreeman 0:435bf84ce48a 245
JonFreeman 0:435bf84ce48a 246 bool Servo::validate_rx ()
JonFreeman 0:435bf84ce48a 247 {
JonFreeman 0:435bf84ce48a 248 if ((clock() - clock_new) > 4)
JonFreeman 0:435bf84ce48a 249 rx_active = false;
JonFreeman 0:435bf84ce48a 250 else
JonFreeman 0:435bf84ce48a 251 rx_active = true;
JonFreeman 0:435bf84ce48a 252 return rx_active;
JonFreeman 0:435bf84ce48a 253 }
JonFreeman 0:435bf84ce48a 254
JonFreeman 0:435bf84ce48a 255 void Servo::rise ()
JonFreeman 0:435bf84ce48a 256 {
JonFreeman 0:435bf84ce48a 257 t.stop ();
JonFreeman 0:435bf84ce48a 258 period_us = t.read_us ();
JonFreeman 0:435bf84ce48a 259 t.reset ();
JonFreeman 0:435bf84ce48a 260 t.start ();
JonFreeman 0:435bf84ce48a 261 }
JonFreeman 0:435bf84ce48a 262 void Servo::fall ()
JonFreeman 0:435bf84ce48a 263 {
JonFreeman 0:435bf84ce48a 264 pulse_width_us = t.read_us ();
JonFreeman 0:435bf84ce48a 265 clock_old = clock_new;
JonFreeman 0:435bf84ce48a 266 clock_new = clock();
JonFreeman 0:435bf84ce48a 267 if ((clock_new - clock_old) < 4)
JonFreeman 0:435bf84ce48a 268 rx_active = true;
JonFreeman 0:435bf84ce48a 269 }
JonFreeman 0:435bf84ce48a 270 void Servo::out_pw_set (double outpw)
JonFreeman 0:435bf84ce48a 271 {
JonFreeman 0:435bf84ce48a 272 if (outpw > 1.0)
JonFreeman 0:435bf84ce48a 273 outpw = 1.0;
JonFreeman 0:435bf84ce48a 274 if (outpw < 0.0)
JonFreeman 0:435bf84ce48a 275 outpw = 0.0;
JonFreeman 0:435bf84ce48a 276 outpw *= 1.2; // Change range to 1.2 ms to cover out pulse width range 0.9 to 2.1 ms
JonFreeman 0:435bf84ce48a 277 outpw += 0.9; // Bias to nom min servo range
JonFreeman 0:435bf84ce48a 278 pulsewidth_ticks ((uint32_t)(outpw * (SystemCoreClock / 2000)));
JonFreeman 0:435bf84ce48a 279 }
JonFreeman 0:435bf84ce48a 280 void Servo::period_ticks (uint32_t pt)
JonFreeman 0:435bf84ce48a 281 {
JonFreeman 0:435bf84ce48a 282 if (out) out->period_ticks (pt);
JonFreeman 0:435bf84ce48a 283 }
JonFreeman 0:435bf84ce48a 284 void Servo::pulsewidth_ticks (uint32_t wt)
JonFreeman 0:435bf84ce48a 285 {
JonFreeman 0:435bf84ce48a 286 if (out) out->pulsewidth_ticks (wt);
JonFreeman 0:435bf84ce48a 287 }
JonFreeman 0:435bf84ce48a 288 void Servo::set_out (FastPWM * op)
JonFreeman 0:435bf84ce48a 289 {
JonFreeman 0:435bf84ce48a 290 out = op;
JonFreeman 0:435bf84ce48a 291 }
JonFreeman 0:435bf84ce48a 292
JonFreeman 0:435bf84ce48a 293 void Servo1rise ()
JonFreeman 0:435bf84ce48a 294 {
JonFreeman 0:435bf84ce48a 295 S1.rise ();
JonFreeman 0:435bf84ce48a 296 }
JonFreeman 0:435bf84ce48a 297 void Servo1fall ()
JonFreeman 0:435bf84ce48a 298 {
JonFreeman 0:435bf84ce48a 299 S1.fall ();
JonFreeman 0:435bf84ce48a 300 }
JonFreeman 0:435bf84ce48a 301 void Servo2rise ()
JonFreeman 0:435bf84ce48a 302 {
JonFreeman 0:435bf84ce48a 303 S2.rise ();
JonFreeman 0:435bf84ce48a 304 }
JonFreeman 0:435bf84ce48a 305 void Servo2fall ()
JonFreeman 0:435bf84ce48a 306 {
JonFreeman 0:435bf84ce48a 307 S2.fall ();
JonFreeman 0:435bf84ce48a 308 }
JonFreeman 0:435bf84ce48a 309 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 310 /*
JonFreeman 0:435bf84ce48a 311 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 312
JonFreeman 0:435bf84ce48a 313 1 1 1 1 0 0 0 ---___---___
JonFreeman 0:435bf84ce48a 314 2 0 0 1 1 1 0 __---___---_
JonFreeman 0:435bf84ce48a 315 4 1 0 0 0 1 1 -___---___--
JonFreeman 0:435bf84ce48a 316
JonFreeman 0:435bf84ce48a 317 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 0:435bf84ce48a 318 */
JonFreeman 0:435bf84ce48a 319 const uint16_t A_tabl[] = {
JonFreeman 0:435bf84ce48a 320 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 321 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 322 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 323 0, BRA,BRA,BRA,BRA,BRA,BRA,0 // Regenerative Braking
JonFreeman 0:435bf84ce48a 324 } ;
JonFreeman 0:435bf84ce48a 325 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 326 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 327 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 328 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 329 0, BRB,BRB,BRB,BRB,BRB,BRB,0 // Regenerative Braking
JonFreeman 0:435bf84ce48a 330 } ;
JonFreeman 0:435bf84ce48a 331
JonFreeman 0:435bf84ce48a 332
JonFreeman 0:435bf84ce48a 333 class motor
JonFreeman 0:435bf84ce48a 334 {
JonFreeman 0:435bf84ce48a 335 struct currents {
JonFreeman 0:435bf84ce48a 336 uint32_t max, min, ave;
JonFreeman 0:435bf84ce48a 337 } I;
JonFreeman 0:435bf84ce48a 338 uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
JonFreeman 0:435bf84ce48a 339 uint32_t Hall_tab_ptr, latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 340 const uint16_t * lut;
JonFreeman 0:435bf84ce48a 341 FastPWM * maxV, * maxI;
JonFreeman 0:435bf84ce48a 342 PortOut * Motor_Port;
JonFreeman 0:435bf84ce48a 343 public:
JonFreeman 0:435bf84ce48a 344 uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
JonFreeman 1:0fabe6fdb55b 345 uint32_t Hindex;
JonFreeman 0:435bf84ce48a 346 motor () {} ; // Default constructor
JonFreeman 0:435bf84ce48a 347 motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * ) ;
JonFreeman 0:435bf84ce48a 348 void set_V_limit (double) ; // Sets max motor voltage
JonFreeman 0:435bf84ce48a 349 void set_I_limit (double) ; // Sets max motor current
JonFreeman 0:435bf84ce48a 350 void Hall_change () ; // Called in response to edge on Hall input pin
JonFreeman 0:435bf84ce48a 351 bool set_mode (int);
JonFreeman 0:435bf84ce48a 352 void current_calc () ;
JonFreeman 0:435bf84ce48a 353 uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
JonFreeman 1:0fabe6fdb55b 354 int read_Halls () ;
JonFreeman 0:435bf84ce48a 355 } ; //MotorA, MotorB;
JonFreeman 0:435bf84ce48a 356
JonFreeman 0:435bf84ce48a 357 motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl);
JonFreeman 0:435bf84ce48a 358 motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl);
JonFreeman 0:435bf84ce48a 359
JonFreeman 0:435bf84ce48a 360 motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr ) // Constructor
JonFreeman 0:435bf84ce48a 361 {
JonFreeman 0:435bf84ce48a 362 maxV = _maxV_;
JonFreeman 0:435bf84ce48a 363 maxI = _maxI_;
JonFreeman 0:435bf84ce48a 364 Hall_total = mode = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
JonFreeman 1:0fabe6fdb55b 365 Hindex = 0;
JonFreeman 0:435bf84ce48a 366 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 367 latest_pulses_per_sec = 0;
JonFreeman 0:435bf84ce48a 368 for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
JonFreeman 0:435bf84ce48a 369 edge_count_table[i] = 0;
JonFreeman 0:435bf84ce48a 370 if (lutptr != A_tabl && lutptr != B_tabl)
JonFreeman 0:435bf84ce48a 371 pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
JonFreeman 0:435bf84ce48a 372 Motor_Port = P;
JonFreeman 0:435bf84ce48a 373 pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
JonFreeman 0:435bf84ce48a 374 maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:435bf84ce48a 375 maxI->period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:435bf84ce48a 376 maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
JonFreeman 0:435bf84ce48a 377 maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
JonFreeman 0:435bf84ce48a 378 // if (P != PortA && P != PortB)
JonFreeman 0:435bf84ce48a 379 // pc.printf ("Fatal in 'motor' constructor, Invalid Port\r\n");
JonFreeman 0:435bf84ce48a 380 // else
JonFreeman 0:435bf84ce48a 381 // PortOut Motor_P (P, *mask); // PortA for motor A, PortB for motor B
JonFreeman 0:435bf84ce48a 382 lut = lutptr;
JonFreeman 0:435bf84ce48a 383 }
JonFreeman 0:435bf84ce48a 384
JonFreeman 1:0fabe6fdb55b 385 int motor::read_Halls () {
JonFreeman 1:0fabe6fdb55b 386 return Hindex;
JonFreeman 1:0fabe6fdb55b 387 }
JonFreeman 1:0fabe6fdb55b 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 1:0fabe6fdb55b 456 *Motor_Port = lut[mode + Hindex];
JonFreeman 0:435bf84ce48a 457 }
JonFreeman 0:435bf84ce48a 458 void MAH1r ()
JonFreeman 0:435bf84ce48a 459 {
JonFreeman 1:0fabe6fdb55b 460 MotorA.Hindex = 1;
JonFreeman 1:0fabe6fdb55b 461 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 462 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 463 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 464 }
JonFreeman 0:435bf84ce48a 465 void MAH1f ()
JonFreeman 0:435bf84ce48a 466 {
JonFreeman 1:0fabe6fdb55b 467 MotorA.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 468 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 469 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 470 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 471 }
JonFreeman 0:435bf84ce48a 472 void MAH2r ()
JonFreeman 0:435bf84ce48a 473 {
JonFreeman 1:0fabe6fdb55b 474 MotorA.Hindex = 2;
JonFreeman 1:0fabe6fdb55b 475 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 476 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 477 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 478 }
JonFreeman 0:435bf84ce48a 479 void MAH2f ()
JonFreeman 0:435bf84ce48a 480 {
JonFreeman 1:0fabe6fdb55b 481 MotorA.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 482 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 483 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 484 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 485 }
JonFreeman 0:435bf84ce48a 486 void MAH3r ()
JonFreeman 0:435bf84ce48a 487 {
JonFreeman 1:0fabe6fdb55b 488 MotorA.Hindex = 4;
JonFreeman 1:0fabe6fdb55b 489 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 490 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 0:435bf84ce48a 491 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 492 }
JonFreeman 0:435bf84ce48a 493 void MAH3f ()
JonFreeman 0:435bf84ce48a 494 {
JonFreeman 1:0fabe6fdb55b 495 MotorA.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 496 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 497 if (MAH2 != 0) MotorA.Hindex |= 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 1:0fabe6fdb55b 503 MotorB.Hindex = 1;
JonFreeman 1:0fabe6fdb55b 504 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 505 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 506 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 507 }
JonFreeman 0:435bf84ce48a 508 void MBH1f ()
JonFreeman 0:435bf84ce48a 509 {
JonFreeman 1:0fabe6fdb55b 510 MotorB.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 511 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 512 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 513 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 514 }
JonFreeman 0:435bf84ce48a 515 void MBH2r ()
JonFreeman 0:435bf84ce48a 516 {
JonFreeman 1:0fabe6fdb55b 517 MotorB.Hindex = 2;
JonFreeman 1:0fabe6fdb55b 518 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 519 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 520 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 521 }
JonFreeman 0:435bf84ce48a 522 void MBH2f ()
JonFreeman 0:435bf84ce48a 523 {
JonFreeman 1:0fabe6fdb55b 524 MotorB.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 525 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 526 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 527 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 528 }
JonFreeman 0:435bf84ce48a 529 void MBH3r ()
JonFreeman 0:435bf84ce48a 530 {
JonFreeman 1:0fabe6fdb55b 531 MotorB.Hindex = 4;
JonFreeman 1:0fabe6fdb55b 532 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 533 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 0:435bf84ce48a 534 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 535 }
JonFreeman 0:435bf84ce48a 536 void MBH3f ()
JonFreeman 0:435bf84ce48a 537 {
JonFreeman 1:0fabe6fdb55b 538 MotorB.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 539 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 540 if (MBH2 != 0) MotorB.Hindex |= 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 1:0fabe6fdb55b 706 uint8_t stat;
JonFreeman 1:0fabe6fdb55b 707 stat = PressureSensor.getFstatus();
JonFreeman 1:0fabe6fdb55b 708 double pres = PressureSensor.getPressure ();
JonFreeman 1:0fabe6fdb55b 709 //double tmpr = PressureSensor.getTemperature ();
JonFreeman 0:435bf84ce48a 710 //double pres = PressureSensor.getTemperature ();
JonFreeman 1:0fabe6fdb55b 711 // pc.printf ("Apps %d, Bpps %d, sys_timer %d\r\n", Apps, Bpps, sys_timer);
JonFreeman 1:0fabe6fdb55b 712 pc.printf ("Apps %d, Bpps %d, sys_timer %d, status %d, pressure %.2f\r\n", Apps, Bpps, sys_timer, stat, pres);
JonFreeman 0:435bf84ce48a 713 // 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 714 }
JonFreeman 0:435bf84ce48a 715 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 716 } // End of main programme loop
JonFreeman 0:435bf84ce48a 717 }