Jon Freeman / Mbed 2 deprecated Brushless_STM3_Ctrl_2018_11

STM3 ESC dual brushless motor controller. 10-60v, motor power rating tiny to kW. Ganged or independent motor control As used in 'The Brute' locomotive - www.jons-workshop.com

Dependencies:   mbed BufferedSerial Servo FastPWM

main.cpp@2:04761b196473, 2018-03-10 (annotated)

Committer:
JonFreeman
Date:
Sat Mar 10 10:11:07 2018 +0000
Revision:
2:04761b196473
Parent:
1:0fabe6fdb55b
Child:
3:ecb00e0e8d68
Proved board basically works, drives 1 brushless motor, not fitted all comps to test second. About to change MotorA and MotorB to Motor[2] array - more general for N motors;

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 PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear
JonFreeman 0:435bf84ce48a 165 MAX_PWM_TICKS = SystemCoreClock / PWM_HZ;
JonFreeman 0:435bf84ce48a 166
JonFreeman 0:435bf84ce48a 167 /* End of Please Do Not Alter these */
JonFreeman 0:435bf84ce48a 168
JonFreeman 0:435bf84ce48a 169 /* Global variable declarations */
JonFreeman 0:435bf84ce48a 170 uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
JonFreeman 0:435bf84ce48a 171 driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
JonFreeman 0:435bf84ce48a 172 sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
JonFreeman 2:04761b196473 173 fast_sys_timer = 0, // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
JonFreeman 0:435bf84ce48a 174 AtoD_Semaphore = 0;
JonFreeman 0:435bf84ce48a 175 bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop
JonFreeman 0:435bf84ce48a 176 bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec
JonFreeman 0:435bf84ce48a 177 bool sounder_on = false;
JonFreeman 0:435bf84ce48a 178 double test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only
JonFreeman 0:435bf84ce48a 179 /* End of Global variable declarations */
JonFreeman 0:435bf84ce48a 180
JonFreeman 0:435bf84ce48a 181 Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
JonFreeman 0:435bf84ce48a 182 Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
JonFreeman 0:435bf84ce48a 183
JonFreeman 0:435bf84ce48a 184 // Interrupt Service Routines
JonFreeman 0:435bf84ce48a 185
JonFreeman 0:435bf84ce48a 186 /** void ISR_loop_timer ()
JonFreeman 0:435bf84ce48a 187 * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
JonFreeman 0:435bf84ce48a 188 * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
JonFreeman 0:435bf84ce48a 189 * Increments global 'sys_timer', usable anywhere as general measure of elapsed time
JonFreeman 0:435bf84ce48a 190 */
JonFreeman 0:435bf84ce48a 191 void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
JonFreeman 0:435bf84ce48a 192 {
JonFreeman 0:435bf84ce48a 193 loop_flag = true; // set flag to allow main programme loop to proceed
JonFreeman 0:435bf84ce48a 194 sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 195 if ((sys_timer & 0x03) == 0)
JonFreeman 0:435bf84ce48a 196 flag_8Hz = true;
JonFreeman 0:435bf84ce48a 197 }
JonFreeman 0:435bf84ce48a 198
JonFreeman 0:435bf84ce48a 199 /** void ISR_voltage_reader ()
JonFreeman 0:435bf84ce48a 200 * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
JonFreeman 0:435bf84ce48a 201 *
JonFreeman 0:435bf84ce48a 202 * AtoD_reader() called from convenient point in code to take readings outside of ISRs
JonFreeman 0:435bf84ce48a 203 */
JonFreeman 0:435bf84ce48a 204
JonFreeman 0:435bf84ce48a 205 void ISR_voltage_reader () // This is Ticker Interrupt Service Routine - few us between readings
JonFreeman 0:435bf84ce48a 206 {
JonFreeman 0:435bf84ce48a 207 AtoD_Semaphore++;
JonFreeman 2:04761b196473 208 fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
JonFreeman 0:435bf84ce48a 209 }
JonFreeman 0:435bf84ce48a 210
JonFreeman 0:435bf84ce48a 211
JonFreeman 0:435bf84ce48a 212 /*
JonFreeman 0:435bf84ce48a 213 Servo - mutex uses :
JonFreeman 0:435bf84ce48a 214 0. Unused
JonFreeman 0:435bf84ce48a 215 1. Input of pwm from model control Rx
JonFreeman 0:435bf84ce48a 216 2. Output pwm to drive model control servo
JonFreeman 0:435bf84ce48a 217 */
JonFreeman 0:435bf84ce48a 218 //enum {SERVO_UNUSED, SERVO_PWM_IN, SERVO_PWM_OUT} ;
JonFreeman 0:435bf84ce48a 219 class Servo
JonFreeman 0:435bf84ce48a 220 {
JonFreeman 0:435bf84ce48a 221 FastPWM * out;
JonFreeman 0:435bf84ce48a 222 Timer t;
JonFreeman 0:435bf84ce48a 223
JonFreeman 0:435bf84ce48a 224 public:
JonFreeman 0:435bf84ce48a 225
JonFreeman 0:435bf84ce48a 226 bool rx_active;
JonFreeman 0:435bf84ce48a 227 int32_t clock_old, clock_new;
JonFreeman 0:435bf84ce48a 228 int32_t pulse_width_us, period_us;
JonFreeman 0:435bf84ce48a 229 Servo () { // Constructor
JonFreeman 0:435bf84ce48a 230 pulse_width_us = period_us = 0;
JonFreeman 0:435bf84ce48a 231 clock_old = clock_new = 0;
JonFreeman 0:435bf84ce48a 232 out = NULL;
JonFreeman 0:435bf84ce48a 233 rx_active = false;
JonFreeman 0:435bf84ce48a 234 }
JonFreeman 0:435bf84ce48a 235 bool validate_rx () ;
JonFreeman 0:435bf84ce48a 236 void rise () ;
JonFreeman 0:435bf84ce48a 237 void fall () ;
JonFreeman 0:435bf84ce48a 238 void out_pw_set (double);
JonFreeman 0:435bf84ce48a 239 void period_ticks (uint32_t);
JonFreeman 0:435bf84ce48a 240 void pulsewidth_ticks (uint32_t);
JonFreeman 0:435bf84ce48a 241 void set_out (FastPWM *);
JonFreeman 0:435bf84ce48a 242 } S1, S2;
JonFreeman 0:435bf84ce48a 243
JonFreeman 0:435bf84ce48a 244 bool Servo::validate_rx ()
JonFreeman 0:435bf84ce48a 245 {
JonFreeman 0:435bf84ce48a 246 if ((clock() - clock_new) > 4)
JonFreeman 0:435bf84ce48a 247 rx_active = false;
JonFreeman 0:435bf84ce48a 248 else
JonFreeman 0:435bf84ce48a 249 rx_active = true;
JonFreeman 0:435bf84ce48a 250 return rx_active;
JonFreeman 0:435bf84ce48a 251 }
JonFreeman 0:435bf84ce48a 252
JonFreeman 0:435bf84ce48a 253 void Servo::rise ()
JonFreeman 0:435bf84ce48a 254 {
JonFreeman 0:435bf84ce48a 255 t.stop ();
JonFreeman 0:435bf84ce48a 256 period_us = t.read_us ();
JonFreeman 0:435bf84ce48a 257 t.reset ();
JonFreeman 0:435bf84ce48a 258 t.start ();
JonFreeman 0:435bf84ce48a 259 }
JonFreeman 0:435bf84ce48a 260 void Servo::fall ()
JonFreeman 0:435bf84ce48a 261 {
JonFreeman 0:435bf84ce48a 262 pulse_width_us = t.read_us ();
JonFreeman 0:435bf84ce48a 263 clock_old = clock_new;
JonFreeman 0:435bf84ce48a 264 clock_new = clock();
JonFreeman 0:435bf84ce48a 265 if ((clock_new - clock_old) < 4)
JonFreeman 0:435bf84ce48a 266 rx_active = true;
JonFreeman 0:435bf84ce48a 267 }
JonFreeman 0:435bf84ce48a 268 void Servo::out_pw_set (double outpw)
JonFreeman 0:435bf84ce48a 269 {
JonFreeman 0:435bf84ce48a 270 if (outpw > 1.0)
JonFreeman 0:435bf84ce48a 271 outpw = 1.0;
JonFreeman 0:435bf84ce48a 272 if (outpw < 0.0)
JonFreeman 0:435bf84ce48a 273 outpw = 0.0;
JonFreeman 0:435bf84ce48a 274 outpw *= 1.2; // Change range to 1.2 ms to cover out pulse width range 0.9 to 2.1 ms
JonFreeman 0:435bf84ce48a 275 outpw += 0.9; // Bias to nom min servo range
JonFreeman 0:435bf84ce48a 276 pulsewidth_ticks ((uint32_t)(outpw * (SystemCoreClock / 2000)));
JonFreeman 0:435bf84ce48a 277 }
JonFreeman 0:435bf84ce48a 278 void Servo::period_ticks (uint32_t pt)
JonFreeman 0:435bf84ce48a 279 {
JonFreeman 0:435bf84ce48a 280 if (out) out->period_ticks (pt);
JonFreeman 0:435bf84ce48a 281 }
JonFreeman 0:435bf84ce48a 282 void Servo::pulsewidth_ticks (uint32_t wt)
JonFreeman 0:435bf84ce48a 283 {
JonFreeman 0:435bf84ce48a 284 if (out) out->pulsewidth_ticks (wt);
JonFreeman 0:435bf84ce48a 285 }
JonFreeman 0:435bf84ce48a 286 void Servo::set_out (FastPWM * op)
JonFreeman 0:435bf84ce48a 287 {
JonFreeman 0:435bf84ce48a 288 out = op;
JonFreeman 0:435bf84ce48a 289 }
JonFreeman 0:435bf84ce48a 290
JonFreeman 0:435bf84ce48a 291 void Servo1rise ()
JonFreeman 0:435bf84ce48a 292 {
JonFreeman 0:435bf84ce48a 293 S1.rise ();
JonFreeman 0:435bf84ce48a 294 }
JonFreeman 0:435bf84ce48a 295 void Servo1fall ()
JonFreeman 0:435bf84ce48a 296 {
JonFreeman 0:435bf84ce48a 297 S1.fall ();
JonFreeman 0:435bf84ce48a 298 }
JonFreeman 0:435bf84ce48a 299 void Servo2rise ()
JonFreeman 0:435bf84ce48a 300 {
JonFreeman 0:435bf84ce48a 301 S2.rise ();
JonFreeman 0:435bf84ce48a 302 }
JonFreeman 0:435bf84ce48a 303 void Servo2fall ()
JonFreeman 0:435bf84ce48a 304 {
JonFreeman 0:435bf84ce48a 305 S2.fall ();
JonFreeman 0:435bf84ce48a 306 }
JonFreeman 0:435bf84ce48a 307 //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
JonFreeman 0:435bf84ce48a 308 /*
JonFreeman 0:435bf84ce48a 309 5 1 3 2 6 4 SENSOR SEQUENCE
JonFreeman 0:435bf84ce48a 310
JonFreeman 0:435bf84ce48a 311 1 1 1 1 0 0 0 ---___---___
JonFreeman 0:435bf84ce48a 312 2 0 0 1 1 1 0 __---___---_
JonFreeman 0:435bf84ce48a 313 4 1 0 0 0 1 1 -___---___--
JonFreeman 0:435bf84ce48a 314
JonFreeman 0:435bf84ce48a 315 UV WV WU VU VW UW OUTPUT SEQUENCE
JonFreeman 0:435bf84ce48a 316 */
JonFreeman 0:435bf84ce48a 317 const uint16_t A_tabl[] = {
JonFreeman 0:435bf84ce48a 318 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 319 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 320 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 321 0, BRA,BRA,BRA,BRA,BRA,BRA,0 // Regenerative Braking
JonFreeman 0:435bf84ce48a 322 } ;
JonFreeman 0:435bf84ce48a 323 const uint16_t B_tabl[] = {
JonFreeman 0:435bf84ce48a 324 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
JonFreeman 0:435bf84ce48a 325 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 326 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 327 0, BRB,BRB,BRB,BRB,BRB,BRB,0 // Regenerative Braking
JonFreeman 0:435bf84ce48a 328 } ;
JonFreeman 0:435bf84ce48a 329
JonFreeman 0:435bf84ce48a 330
JonFreeman 0:435bf84ce48a 331 class motor
JonFreeman 0:435bf84ce48a 332 {
JonFreeman 0:435bf84ce48a 333 uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
JonFreeman 0:435bf84ce48a 334 uint32_t Hall_tab_ptr, latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 335 const uint16_t * lut;
JonFreeman 0:435bf84ce48a 336 FastPWM * maxV, * maxI;
JonFreeman 0:435bf84ce48a 337 PortOut * Motor_Port;
JonFreeman 2:04761b196473 338 InterruptIn * Hall1, * Hall2, * Hall3;
JonFreeman 0:435bf84ce48a 339 public:
JonFreeman 2:04761b196473 340 struct currents {
JonFreeman 2:04761b196473 341 uint32_t max, min, ave;
JonFreeman 2:04761b196473 342 } I;
JonFreeman 0:435bf84ce48a 343 uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
JonFreeman 1:0fabe6fdb55b 344 uint32_t Hindex;
JonFreeman 2:04761b196473 345 // PinName Hall1, Hall2, Hall3;
JonFreeman 0:435bf84ce48a 346 motor () {} ; // Default constructor
JonFreeman 2:04761b196473 347 motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * , InterruptIn * H1, InterruptIn * H2, InterruptIn * H3) ;
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 2:04761b196473 355 void tickle () ;
JonFreeman 0:435bf84ce48a 356 } ; //MotorA, MotorB;
JonFreeman 0:435bf84ce48a 357
JonFreeman 2:04761b196473 358 motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, &MAH1, &MAH2, &MAH3);
JonFreeman 2:04761b196473 359 motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, &MBH1, &MBH2, &MBH3);
JonFreeman 0:435bf84ce48a 360
JonFreeman 2:04761b196473 361 motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn * H1, InterruptIn * H2, InterruptIn * H3) // Constructor
JonFreeman 0:435bf84ce48a 362 {
JonFreeman 2:04761b196473 363 Hall1 =H1;
JonFreeman 2:04761b196473 364 Hall2 =H2;
JonFreeman 2:04761b196473 365 Hall3 =H3;
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 1:0fabe6fdb55b 369 Hindex = 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 1:0fabe6fdb55b 389 int motor::read_Halls () {
JonFreeman 2:04761b196473 390 int x = 0;
JonFreeman 2:04761b196473 391 if (*Hall1 != 0) x |= 1;
JonFreeman 2:04761b196473 392 if (*Hall2 != 0) x |= 2;
JonFreeman 2:04761b196473 393 if (*Hall3 != 0) x |= 4;
JonFreeman 2:04761b196473 394 Hindex = x;
JonFreeman 2:04761b196473 395 return x;
JonFreeman 2:04761b196473 396 // return Hindex;
JonFreeman 2:04761b196473 397 }
JonFreeman 2:04761b196473 398
JonFreeman 2:04761b196473 399 void motor::tickle () // Attempt to get mosfet driver to drive high side fets
JonFreeman 2:04761b196473 400 {
JonFreeman 2:04761b196473 401 volatile int t;
JonFreeman 2:04761b196473 402 for (int cnt = 0; cnt < 20; cnt++) {
JonFreeman 2:04761b196473 403 *Motor_Port = 0;
JonFreeman 2:04761b196473 404 t = fast_sys_timer;
JonFreeman 2:04761b196473 405 int x = read_Halls () | mode;
JonFreeman 2:04761b196473 406 pc.printf ("Ti");
JonFreeman 2:04761b196473 407 *Motor_Port = lut[x];
JonFreeman 2:04761b196473 408 pc.printf ("%d\t", t);
JonFreeman 2:04761b196473 409 }
JonFreeman 2:04761b196473 410 }
JonFreeman 2:04761b196473 411
JonFreeman 2:04761b196473 412 void tickle () {
JonFreeman 2:04761b196473 413 MotorA.tickle ();
JonFreeman 1:0fabe6fdb55b 414 }
JonFreeman 1:0fabe6fdb55b 415
JonFreeman 0:435bf84ce48a 416 void motor::current_calc ()
JonFreeman 0:435bf84ce48a 417 {
JonFreeman 0:435bf84ce48a 418 I.min = 0x0fffffff; // samples are 16 bit
JonFreeman 0:435bf84ce48a 419 I.max = 0;
JonFreeman 0:435bf84ce48a 420 I.ave = 0;
JonFreeman 0:435bf84ce48a 421 uint16_t sample;
JonFreeman 0:435bf84ce48a 422 for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
JonFreeman 0:435bf84ce48a 423 sample = current_samples[i];
JonFreeman 0:435bf84ce48a 424 I.ave += sample;
JonFreeman 0:435bf84ce48a 425 if (I.min > sample)
JonFreeman 0:435bf84ce48a 426 I.min = sample;
JonFreeman 0:435bf84ce48a 427 if (I.max < sample)
JonFreeman 0:435bf84ce48a 428 I.max = sample;
JonFreeman 0:435bf84ce48a 429 }
JonFreeman 0:435bf84ce48a 430 I.ave /= CURRENT_SAMPLES_AVERAGED;
JonFreeman 0:435bf84ce48a 431 }
JonFreeman 0:435bf84ce48a 432
JonFreeman 0:435bf84ce48a 433 void motor::set_V_limit (double p) // Sets max motor voltage
JonFreeman 0:435bf84ce48a 434 {
JonFreeman 0:435bf84ce48a 435 if (p < 0.0)
JonFreeman 0:435bf84ce48a 436 p = 0.0;
JonFreeman 0:435bf84ce48a 437 if (p > 1.0)
JonFreeman 0:435bf84ce48a 438 p = 1.0;
JonFreeman 0:435bf84ce48a 439 // last_pwm = p;
JonFreeman 0:435bf84ce48a 440 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:435bf84ce48a 441 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:435bf84ce48a 442 maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
JonFreeman 0:435bf84ce48a 443 }
JonFreeman 0:435bf84ce48a 444
JonFreeman 0:435bf84ce48a 445 void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
JonFreeman 0:435bf84ce48a 446 {
JonFreeman 0:435bf84ce48a 447 int a;
JonFreeman 0:435bf84ce48a 448 if (p < 0.0)
JonFreeman 0:435bf84ce48a 449 p = 0.0;
JonFreeman 0:435bf84ce48a 450 if (p > 1.0)
JonFreeman 0:435bf84ce48a 451 p = 1.0;
JonFreeman 0:435bf84ce48a 452 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:435bf84ce48a 453 if (a > MAX_PWM_TICKS)
JonFreeman 0:435bf84ce48a 454 a = MAX_PWM_TICKS;
JonFreeman 0:435bf84ce48a 455 if (a < 0)
JonFreeman 0:435bf84ce48a 456 a = 0;
JonFreeman 0:435bf84ce48a 457 maxI->pulsewidth_ticks (a); // PWM
JonFreeman 0:435bf84ce48a 458 }
JonFreeman 0:435bf84ce48a 459
JonFreeman 0:435bf84ce48a 460 uint32_t motor::pulses_per_sec () // call this once per main loop pass to keep count = edges per sec
JonFreeman 0:435bf84ce48a 461 {
JonFreeman 0:435bf84ce48a 462 uint32_t tmp = Hall_total;
JonFreeman 0:435bf84ce48a 463 latest_pulses_per_sec = tmp - edge_count_table[Hall_tab_ptr];
JonFreeman 0:435bf84ce48a 464 edge_count_table[Hall_tab_ptr] = tmp;
JonFreeman 0:435bf84ce48a 465 Hall_tab_ptr++;
JonFreeman 0:435bf84ce48a 466 if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
JonFreeman 0:435bf84ce48a 467 Hall_tab_ptr = 0;
JonFreeman 0:435bf84ce48a 468 return latest_pulses_per_sec;
JonFreeman 0:435bf84ce48a 469 }
JonFreeman 0:435bf84ce48a 470
JonFreeman 0:435bf84ce48a 471 bool motor::set_mode (int m)
JonFreeman 0:435bf84ce48a 472 {
JonFreeman 2:04761b196473 473 if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
JonFreeman 2:04761b196473 474 pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
JonFreeman 0:435bf84ce48a 475 return false;
JonFreeman 2:04761b196473 476 }
JonFreeman 0:435bf84ce48a 477 mode = m;
JonFreeman 0:435bf84ce48a 478 return true;
JonFreeman 0:435bf84ce48a 479 }
JonFreeman 0:435bf84ce48a 480
JonFreeman 0:435bf84ce48a 481 void motor::Hall_change ()
JonFreeman 0:435bf84ce48a 482 {
JonFreeman 0:435bf84ce48a 483 Hall_total++;
JonFreeman 2:04761b196473 484 // mode &= 0x038L; // safety
JonFreeman 2:04761b196473 485 *Motor_Port = lut[mode | Hindex];
JonFreeman 0:435bf84ce48a 486 }
JonFreeman 2:04761b196473 487
JonFreeman 0:435bf84ce48a 488 void MAH1r ()
JonFreeman 0:435bf84ce48a 489 {
JonFreeman 1:0fabe6fdb55b 490 MotorA.Hindex = 1;
JonFreeman 1:0fabe6fdb55b 491 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 492 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 493 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 494 }
JonFreeman 0:435bf84ce48a 495 void MAH1f ()
JonFreeman 0:435bf84ce48a 496 {
JonFreeman 1:0fabe6fdb55b 497 MotorA.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 498 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 499 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 500 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 501 }
JonFreeman 0:435bf84ce48a 502 void MAH2r ()
JonFreeman 0:435bf84ce48a 503 {
JonFreeman 1:0fabe6fdb55b 504 MotorA.Hindex = 2;
JonFreeman 1:0fabe6fdb55b 505 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 506 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 507 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 508 }
JonFreeman 0:435bf84ce48a 509 void MAH2f ()
JonFreeman 0:435bf84ce48a 510 {
JonFreeman 1:0fabe6fdb55b 511 MotorA.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 512 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 513 if (MAH3 != 0) MotorA.Hindex |= 4;
JonFreeman 0:435bf84ce48a 514 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 515 }
JonFreeman 0:435bf84ce48a 516 void MAH3r ()
JonFreeman 0:435bf84ce48a 517 {
JonFreeman 1:0fabe6fdb55b 518 MotorA.Hindex = 4;
JonFreeman 1:0fabe6fdb55b 519 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 520 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 0:435bf84ce48a 521 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 522 }
JonFreeman 0:435bf84ce48a 523 void MAH3f ()
JonFreeman 0:435bf84ce48a 524 {
JonFreeman 1:0fabe6fdb55b 525 MotorA.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 526 if (MAH1 != 0) MotorA.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 527 if (MAH2 != 0) MotorA.Hindex |= 2;
JonFreeman 0:435bf84ce48a 528 MotorA.Hall_change ();
JonFreeman 0:435bf84ce48a 529 }
JonFreeman 0:435bf84ce48a 530
JonFreeman 0:435bf84ce48a 531 void MBH1r ()
JonFreeman 0:435bf84ce48a 532 {
JonFreeman 1:0fabe6fdb55b 533 MotorB.Hindex = 1;
JonFreeman 1:0fabe6fdb55b 534 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 535 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 536 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 537 }
JonFreeman 0:435bf84ce48a 538 void MBH1f ()
JonFreeman 0:435bf84ce48a 539 {
JonFreeman 1:0fabe6fdb55b 540 MotorB.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 541 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 1:0fabe6fdb55b 542 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 543 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 544 }
JonFreeman 0:435bf84ce48a 545 void MBH2r ()
JonFreeman 0:435bf84ce48a 546 {
JonFreeman 1:0fabe6fdb55b 547 MotorB.Hindex = 2;
JonFreeman 1:0fabe6fdb55b 548 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 549 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 550 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 551 }
JonFreeman 0:435bf84ce48a 552 void MBH2f ()
JonFreeman 0:435bf84ce48a 553 {
JonFreeman 1:0fabe6fdb55b 554 MotorB.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 555 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 556 if (MBH3 != 0) MotorB.Hindex |= 4;
JonFreeman 0:435bf84ce48a 557 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 558 }
JonFreeman 0:435bf84ce48a 559 void MBH3r ()
JonFreeman 0:435bf84ce48a 560 {
JonFreeman 1:0fabe6fdb55b 561 MotorB.Hindex = 4;
JonFreeman 1:0fabe6fdb55b 562 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 563 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 0:435bf84ce48a 564 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 565 }
JonFreeman 0:435bf84ce48a 566 void MBH3f ()
JonFreeman 0:435bf84ce48a 567 {
JonFreeman 1:0fabe6fdb55b 568 MotorB.Hindex = 0;
JonFreeman 1:0fabe6fdb55b 569 if (MBH1 != 0) MotorB.Hindex |= 1;
JonFreeman 1:0fabe6fdb55b 570 if (MBH2 != 0) MotorB.Hindex |= 2;
JonFreeman 0:435bf84ce48a 571 MotorB.Hall_change ();
JonFreeman 0:435bf84ce48a 572 }
JonFreeman 0:435bf84ce48a 573
JonFreeman 0:435bf84ce48a 574
JonFreeman 0:435bf84ce48a 575 // End of Interrupt Service Routines
JonFreeman 0:435bf84ce48a 576
JonFreeman 2:04761b196473 577 void setVI (double v, double i) {
JonFreeman 2:04761b196473 578 // void set_V_limit (double) ; // Sets max motor voltage
JonFreeman 2:04761b196473 579 // void set_I_limit (double) ; // Sets max motor current
JonFreeman 2:04761b196473 580 MotorA.set_V_limit (v);
JonFreeman 2:04761b196473 581 MotorA.set_I_limit (i);
JonFreeman 2:04761b196473 582 MotorB.set_V_limit (v);
JonFreeman 2:04761b196473 583 MotorB.set_I_limit (i);
JonFreeman 0:435bf84ce48a 584 }
JonFreeman 2:04761b196473 585
JonFreeman 0:435bf84ce48a 586 void AtoD_reader ()
JonFreeman 0:435bf84ce48a 587 {
JonFreeman 0:435bf84ce48a 588 static uint32_t i = 0, tab_ptr = 0;
JonFreeman 0:435bf84ce48a 589 if (AtoD_Semaphore > 20) {
JonFreeman 0:435bf84ce48a 590 pc.printf ("WARNING - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore);
JonFreeman 0:435bf84ce48a 591 AtoD_Semaphore = 20;
JonFreeman 0:435bf84ce48a 592 }
JonFreeman 0:435bf84ce48a 593 while (AtoD_Semaphore > 0) {
JonFreeman 0:435bf84ce48a 594 AtoD_Semaphore--;
JonFreeman 0:435bf84ce48a 595 // Code here to sequence through reading voltmeter, demand pot, ammeters
JonFreeman 0:435bf84ce48a 596 switch (i) { //
JonFreeman 0:435bf84ce48a 597 case 0:
JonFreeman 0:435bf84ce48a 598 volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading
JonFreeman 0:435bf84ce48a 599 volt_reading >>= 1; // Result = Result / 2
JonFreeman 0:435bf84ce48a 600 break; // i.e. Very simple digital low pass filter
JonFreeman 0:435bf84ce48a 601 case 1:
JonFreeman 0:435bf84ce48a 602 driverpot_reading += Ain_DriverPot.read_u16 ();
JonFreeman 0:435bf84ce48a 603 driverpot_reading >>= 1;
JonFreeman 0:435bf84ce48a 604 break;
JonFreeman 0:435bf84ce48a 605 case 2:
JonFreeman 0:435bf84ce48a 606 MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 607 break;
JonFreeman 0:435bf84ce48a 608 case 3:
JonFreeman 0:435bf84ce48a 609 MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); //
JonFreeman 0:435bf84ce48a 610 if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter
JonFreeman 0:435bf84ce48a 611 tab_ptr = 0;
JonFreeman 0:435bf84ce48a 612 break;
JonFreeman 0:435bf84ce48a 613 }
JonFreeman 0:435bf84ce48a 614 i++; // prepare to read the next input in response to the next interrupt
JonFreeman 0:435bf84ce48a 615 if (i > 3)
JonFreeman 0:435bf84ce48a 616 i = 0;
JonFreeman 0:435bf84ce48a 617 }
JonFreeman 0:435bf84ce48a 618 }
JonFreeman 0:435bf84ce48a 619
JonFreeman 0:435bf84ce48a 620 /** double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 621 * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
JonFreeman 0:435bf84ce48a 622 * ISR also filters signal
JonFreeman 0:435bf84ce48a 623 * This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
JonFreeman 0:435bf84ce48a 624 */
JonFreeman 0:435bf84ce48a 625 double Read_DriverPot ()
JonFreeman 0:435bf84ce48a 626 {
JonFreeman 0:435bf84ce48a 627 return (double) driverpot_reading / 65535.0; // Normalise 0.0 <= control pot <= 1.0
JonFreeman 0:435bf84ce48a 628 }
JonFreeman 0:435bf84ce48a 629
JonFreeman 0:435bf84ce48a 630 double Read_BatteryVolts ()
JonFreeman 0:435bf84ce48a 631 {
JonFreeman 2:04761b196473 632 return (double) volt_reading / 951.0; // divisor fiddled to make voltage reading correct !
JonFreeman 0:435bf84ce48a 633 }
JonFreeman 0:435bf84ce48a 634
JonFreeman 0:435bf84ce48a 635 double read_volts () // A test function
JonFreeman 0:435bf84ce48a 636 {
JonFreeman 0:435bf84ce48a 637 pc.printf ("pot = %.4f, System Voltage = %.2f\r\n", Read_DriverPot(), Read_BatteryVolts());
JonFreeman 0:435bf84ce48a 638 return Read_BatteryVolts();
JonFreeman 0:435bf84ce48a 639 }
JonFreeman 0:435bf84ce48a 640
JonFreeman 2:04761b196473 641 void mode_test (int mode, double val) {
JonFreeman 2:04761b196473 642 MotorA.set_mode (mode);
JonFreeman 2:04761b196473 643 MotorB.set_mode (mode);
JonFreeman 2:04761b196473 644 if (mode == REGENBRAKE) {
JonFreeman 2:04761b196473 645
JonFreeman 2:04761b196473 646 }
JonFreeman 2:04761b196473 647 }
JonFreeman 0:435bf84ce48a 648
JonFreeman 0:435bf84ce48a 649 extern void command_line_interpreter () ;
JonFreeman 0:435bf84ce48a 650 extern int check_24LC64 () ; // Call from near top of main() to init i2c bus
JonFreeman 0:435bf84ce48a 651 extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
JonFreeman 0:435bf84ce48a 652 extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
JonFreeman 0:435bf84ce48a 653
JonFreeman 0:435bf84ce48a 654 int main()
JonFreeman 0:435bf84ce48a 655 {
JonFreeman 0:435bf84ce48a 656 int j = 0;
JonFreeman 0:435bf84ce48a 657 uint32_t Apps, Bpps;
JonFreeman 0:435bf84ce48a 658
JonFreeman 0:435bf84ce48a 659 MotA = 0; // Motor drive ports A and B
JonFreeman 0:435bf84ce48a 660 MotB = 0;
JonFreeman 0:435bf84ce48a 661 MAH1.rise (& MAH1r);
JonFreeman 0:435bf84ce48a 662 MAH1.fall (& MAH1f);
JonFreeman 0:435bf84ce48a 663 MAH2.rise (& MAH2r);
JonFreeman 0:435bf84ce48a 664 MAH2.fall (& MAH2f);
JonFreeman 0:435bf84ce48a 665 MAH3.rise (& MAH3r);
JonFreeman 0:435bf84ce48a 666 MAH3.fall (& MAH3f);
JonFreeman 0:435bf84ce48a 667
JonFreeman 0:435bf84ce48a 668 MBH1.rise (& MBH1r);
JonFreeman 0:435bf84ce48a 669 MBH1.fall (& MBH1f);
JonFreeman 0:435bf84ce48a 670 MBH2.rise (& MBH2r);
JonFreeman 0:435bf84ce48a 671 MBH2.fall (& MBH2f);
JonFreeman 0:435bf84ce48a 672 MBH3.rise (& MBH3r);
JonFreeman 0:435bf84ce48a 673 MBH3.fall (& MBH3f);
JonFreeman 0:435bf84ce48a 674
JonFreeman 0:435bf84ce48a 675 MAH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 676 MAH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 677 MAH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 678 MBH1.mode (PullUp);
JonFreeman 0:435bf84ce48a 679 MBH2.mode (PullUp);
JonFreeman 0:435bf84ce48a 680 MBH3.mode (PullUp);
JonFreeman 0:435bf84ce48a 681 pc.printf ("\tAbandon Hope %d\r\n", LED ? 0 : 1);
JonFreeman 0:435bf84ce48a 682 // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
JonFreeman 0:435bf84ce48a 683 tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 684 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
JonFreeman 0:435bf84ce48a 685 // Done setting up system interrupt timers
JonFreeman 0:435bf84ce48a 686
JonFreeman 0:435bf84ce48a 687 const int TXTBUFSIZ = 36;
JonFreeman 0:435bf84ce48a 688 char buff[TXTBUFSIZ];
JonFreeman 0:435bf84ce48a 689 bool eerom_detected = false;
JonFreeman 0:435bf84ce48a 690 pc.printf ("RAM test - ");
JonFreeman 0:435bf84ce48a 691 if (check_24LC64() != 0xa0) // searches for i2c devices, returns address of highest found
JonFreeman 0:435bf84ce48a 692 pc.printf ("Check for 24LC64 eeprom FAILED\r\n");
JonFreeman 0:435bf84ce48a 693 else // i2c.write returned 0, think this means device responded with 'ACK', found it anyway
JonFreeman 0:435bf84ce48a 694 eerom_detected = true;
JonFreeman 0:435bf84ce48a 695 if (eerom_detected) {
JonFreeman 0:435bf84ce48a 696 bool j, k;
JonFreeman 0:435bf84ce48a 697 pc.printf ("ok\r\n");
JonFreeman 0:435bf84ce48a 698 static const char ramtst[] = "I found the man sir!";
JonFreeman 0:435bf84ce48a 699 j = wr_24LC64 (0x1240, (char*)ramtst, strlen(ramtst));
JonFreeman 0:435bf84ce48a 700 for (int i = 0; i < TXTBUFSIZ; i++) buff[i] = 0; // Clear buffer
JonFreeman 0:435bf84ce48a 701 // need a way to check i2c busy - YES implemented ack_poll
JonFreeman 0:435bf84ce48a 702 k = rd_24LC64 (0x1240, buff, strlen(ramtst));
JonFreeman 0:435bf84ce48a 703 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 704 }
JonFreeman 0:435bf84ce48a 705 T1 = 0; // As yet unused pins
JonFreeman 0:435bf84ce48a 706 T2 = 0;
JonFreeman 0:435bf84ce48a 707 T3 = 0;
JonFreeman 0:435bf84ce48a 708 T4 = 0;
JonFreeman 0:435bf84ce48a 709 T5 = 0;
JonFreeman 0:435bf84ce48a 710 T6 = 0;
JonFreeman 2:04761b196473 711 MotorA.set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 712 MotorB.set_mode (REGENBRAKE);
JonFreeman 2:04761b196473 713 // MotorA.set_mode (FORWARD);
JonFreeman 2:04761b196473 714 // MotorB.set_mode (FORWARD);
JonFreeman 2:04761b196473 715 MotorA.set_V_limit (0.9);
JonFreeman 2:04761b196473 716 MotorB.set_V_limit (0.9);
JonFreeman 2:04761b196473 717 MotorA.set_I_limit (0.5);
JonFreeman 2:04761b196473 718 MotorB.set_I_limit (0.5);
JonFreeman 0:435bf84ce48a 719
JonFreeman 0:435bf84ce48a 720 // Setup Complete ! Can now start main control forever loop.
JonFreeman 0:435bf84ce48a 721
JonFreeman 0:435bf84ce48a 722 while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
JonFreeman 0:435bf84ce48a 723 while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
JonFreeman 0:435bf84ce48a 724 command_line_interpreter () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
JonFreeman 0:435bf84ce48a 725 AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
JonFreeman 0:435bf84ce48a 726 }
JonFreeman 0:435bf84ce48a 727 loop_flag = false; // Clear flag set by ticker interrupt handler
JonFreeman 0:435bf84ce48a 728 Apps = MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second
JonFreeman 0:435bf84ce48a 729 Bpps = MotorB.pulses_per_sec ();
JonFreeman 0:435bf84ce48a 730
JonFreeman 0:435bf84ce48a 731 // do stuff
JonFreeman 0:435bf84ce48a 732 if (flag_8Hz) { // do slower stuff
JonFreeman 0:435bf84ce48a 733 flag_8Hz = false;
JonFreeman 2:04761b196473 734 // LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 0:435bf84ce48a 735 j++;
JonFreeman 0:435bf84ce48a 736 if (j > 6) { // Send some status info out of serial port every second and a bit or thereabouts
JonFreeman 0:435bf84ce48a 737 j = 0;
JonFreeman 2:04761b196473 738 LED = !LED; // Toggle LED on board, should be seen to fast flash
JonFreeman 2:04761b196473 739 MotorA.current_calc ();
JonFreeman 2:04761b196473 740 MotorB.current_calc ();
JonFreeman 2:04761b196473 741 // pc.printf ("Hello\r\n");
JonFreeman 2:04761b196473 742 // uint8_t stat;
JonFreeman 2:04761b196473 743 // pc.printf ("Arpm %d, Brpm %d, sys_timer %d, HA %d, HB %d\r\n", (Apps * 60) / 24, (Bpps * 60) / 24, sys_timer, MotorA.read_Halls (), MotorB.read_Halls ());
JonFreeman 2:04761b196473 744 // pc.printf ("Apps %d, Bpps %d, sys_timer %d, HA %d, HB %d\r\n", Apps, Bpps, sys_timer, MotorA.read_Halls (), MotorB.read_Halls ());
JonFreeman 2:04761b196473 745 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 0:435bf84ce48a 746 }
JonFreeman 0:435bf84ce48a 747 } // End of if(flag_8Hz)
JonFreeman 0:435bf84ce48a 748 } // End of main programme loop
JonFreeman 0:435bf84ce48a 749 }