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?
User | Revision | Line number | New 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 | } |