Dual Brushless Motor ESC, 10-62V, up to 50A per motor. Motors ganged or independent, multiple control input methods, cycle-by-cycle current limit, speed mode and torque mode control. Motors tiny to kW. Speed limit and other parameters easily set in firmware. As used in 'The Brushless Brutalist' locomotive - www.jons-workshop.com. See also Model Engineer magazine June-October 2019.

Dependencies:   mbed BufferedSerial Servo PCT2075 FastPWM

Update 17th August 2020 Radio control inputs completed

Committer:
JonFreeman
Date:
Thu Apr 26 08:23:04 2018 +0000
Revision:
4:21d91465e4b1
Parent:
3:ecb00e0e8d68
Child:
5:ca86a7848d54
Adding setup in eeprom code, responses to be aligned to controller;

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