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
Diff: main.cpp
- Revision:
- 0:435bf84ce48a
- Child:
- 1:0fabe6fdb55b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 01 11:29:28 2018 +0000 @@ -0,0 +1,714 @@ +#include "mbed.h" +#include "DualBLS.h" +#include "BufferedSerial.h" +#include "FastPWM.h" +#include "MPL3115A2.h" +/* STM32F401RE - compile using NUCLEO-F401RE +// PROJECT - Dual Brushless Motor Controller - March 2018. + +DigitalIn nFault1 (); needs pullup +DigitalIn nFault2 (); needs pullup +AnalogIn to read each motor current + +____________________________________________________________________________________ + CONTROL PHILOSOPHY +This Bogie drive board software should ensure sensible control when commands supplied are not sensible ! + +That is, smooth transition between Drive, Coast and Brake to be implemented here. +The remote controller must not be relied upon to deliver sensible command sequences. + +So much the better if the remote controller does issue sensible commands, but ... + +____________________________________________________________________________________ + + +*/ + +// Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out +#define SERVO1_IN +//#define SERVO1_OUT +//#define SERVO2_IN +#define SERVO2_OUT + + +// Port A -> MotorA, Port B -> MotorB +const uint16_t +AUL = 1 << 0, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) +AVL = 1 << 6, +AWL = 1 << 4, + +AUH = 1 << 1, +AVH = 1 << 7, +AWH = 1 << 8, + +AUV = AUH | AVL, +AVU = AVH | AUL, +AUW = AUH | AWL, +AWU = AWH | AUL, +AVW = AVH | AWL, +AWV = AWH | AVL, + +BRA = AUL | AVL | AWL, + +BUL = 1 << 0, +BVL = 1 << 1, +BWL = 1 << 2, + +BUH = 1 << 10, +BVH = 1 << 12, +BWH = 1 << 13, + +BUV = BUH | BVL, +BVU = BVH | BUL, +BUW = BUH | BWL, +BWU = BWH | BUL, +BVW = BVH | BWL, +BWV = BWH | BVL, + +BRB = BUL | BVL | BWL, + +PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH, // NEW METHOD FOR DGD21032 MOSFET DRIVERS +PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH; + +PortOut MotA (PortA, PORT_A_MASK); +PortOut MotB (PortB, PORT_B_MASK); + +// Pin 1 VBAT NET +3V3 +DigitalIn J3 (PC_13, PullUp);// Pin 2 Jumper pulls to GND, R floats Hi +// Pin 3 PC14-OSC32_IN NET O32I +// Pin 4 PC15-OSC32_OUT NET O32O +// Pin 5 PH0-OSC_IN NET PH1 +// Pin 6 PH1-OSC_OUT NET PH1 +// Pin 7 NRST NET NRST +AnalogIn Ain_DriverPot (PC_0); // Pin 8 Spare Analogue in, net SAIN fitted with external pull-down +AnalogIn Ain_SystemVolts (PC_1); // Pin 9 +AnalogIn Motor_A_Current (PC_2); // Pin 10 might as well use up WSRA stock here +AnalogIn Motor_B_Current (PC_3); // Pin 11 +// Pin 12 VSSA/VREF- NET GND +// Pin 13 VDDA/VREF+ NET +3V3 +// Pin 14 Port_A AUL +// Pin 15 Port_A AUH +// Pins 16, 17 BufferedSerial pc +BufferedSerial pc (PA_2, PA_3, 512, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead +// Pin 18 VSS NET GND +// Pin 19 VDD NET +3V3 +// Pin 20 Port_A AWL +// Pin 21 DigitalOut led1(LED1); +DigitalOut LED (PA_5); // Pin 21 +// Pin 22 Port_A AVL +// Pin 23 Port_A AVH +InterruptIn MBH2 (PC_4); // Pin 24 +InterruptIn MBH3 (PC_5); // Pin 25 +// Pin 26 Port_B BUL +// Pin 27 Port_B BVL +// Pin 28 Port_B BWL +// Pin 29 Port_B BUH +// Pin 30 VCAP1 +// Pin 31 VSS +// Pin 32 VDD +// Pin 33 Port_B BVH +// Pin 34 Port_B BWH +DigitalOut T4 (PB_14); // Pin 35 +DigitalOut T3 (PB_15); // Pin 36 +// BufferedSerial xb pins 37 Tx, 38 Rx +BufferedSerial xb (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module +FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3 + A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4 +//InterruptIn MotB_Hall (PA_8); // Pin 41 +// Pin 41 Port_A AWH +// BufferedSerial ser3 pins 42 Tx, 43 Rx +BufferedSerial ser3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module + +// Feb 2018 Pins 44 and 45 now liberated, could use for serial or other uses +//BufferedSerial extra_ser (PA_11, PA_12); // Pins 44, 45 tx, rx to XBee module +DigitalOut T2 (PA_11); // Pin 44 +DigitalOut T1 (PA_12); // Pin 45 +// Pin 46 SWDIO +// Pin 47 VSS +// Pin 48 VDD +// Pin 49 SWCLK +DigitalOut T5 (PA_15); // Pin 50 +InterruptIn MAH1 (PC_10); // Pin 51 +InterruptIn MAH2 (PC_11); // Pin 52 +InterruptIn MAH3 (PC_12); // Pin 53 +InterruptIn MBH1 (PD_2); // Pin 54 +DigitalOut T6 (PB_3); // Pin 55 +FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3 + B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4 + +I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom +// Pin 60 BOOT0 + +//MPL3115A2(PinName sda, PinName scl, int addr) ; +//MPL3115A2 PressureSensor (PB_7, PB_6, 0xa0) ; +//double PressureSensor.getPressure (); + +#ifdef SERVO1_IN +InterruptIn Servo1_i (PB_8); // Pin 61 to read output from rc rx +#endif +#ifdef SERVO1_OUT +FastPWM Servo1_o (PB_8, 2); //Prescaler 2. This is pwm 4/3 +#endif + +#ifdef SERVO2_IN +InterruptIn Servo2_i (PB_9); // Pin 62 to read output from rc rx +#endif +#ifdef SERVO2_OUT +FastPWM Servo2_o (PB_9, 2); // Prescaler 2. This is pwm 4/4 +#endif + +// Pin 63 VSS +// Pin 64 VDD +// SYSTEM CONSTANTS + +/* Please Do Not Alter these */ +const int VOLTAGE_READ_INTERVAL_US = 50, // Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass + MAIN_LOOP_REPEAT_TIME_US = 31250, // 31250 us, with TACHO_TAB_SIZE = 32 means tacho_ticks_per_time is tacho_ticks_per_second + MAIN_LOOP_ITERATION_Hz = 1000000 / MAIN_LOOP_REPEAT_TIME_US, + CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings + HANDBRAKE = 0, + FORWARD = 8, + REVERSE = 16, + REGENBRAKE = 24, + PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear + MAX_PWM_TICKS = SystemCoreClock / PWM_HZ; + +/* End of Please Do Not Alter these */ + +/* Global variable declarations */ +uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts + driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot + sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US + AtoD_Semaphore = 0; +bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop +bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec +bool sounder_on = false; +double test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only +/* End of Global variable declarations */ + +Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc +Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop + +// Interrupt Service Routines + +/** void ISR_loop_timer () +* This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above) +* This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop. +* Increments global 'sys_timer', usable anywhere as general measure of elapsed time +*/ +void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US +{ + loop_flag = true; // set flag to allow main programme loop to proceed + sys_timer++; // Just a handy measure of elapsed time for anything to use + if ((sys_timer & 0x03) == 0) + flag_8Hz = true; +} + +/** void ISR_voltage_reader () +* This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds +* +* AtoD_reader() called from convenient point in code to take readings outside of ISRs +*/ + +void ISR_voltage_reader () // This is Ticker Interrupt Service Routine - few us between readings +{ + AtoD_Semaphore++; +} + + +/* +Servo - mutex uses : +0. Unused +1. Input of pwm from model control Rx +2. Output pwm to drive model control servo +*/ +//enum {SERVO_UNUSED, SERVO_PWM_IN, SERVO_PWM_OUT} ; +class Servo +{ + FastPWM * out; + Timer t; + +public: + + bool rx_active; + int32_t clock_old, clock_new; + int32_t pulse_width_us, period_us; + Servo () { // Constructor + pulse_width_us = period_us = 0; + clock_old = clock_new = 0; + out = NULL; + rx_active = false; + } + bool validate_rx () ; + void rise () ; + void fall () ; + void out_pw_set (double); + void period_ticks (uint32_t); + void pulsewidth_ticks (uint32_t); + void set_out (FastPWM *); +} S1, S2; + +bool Servo::validate_rx () +{ + if ((clock() - clock_new) > 4) + rx_active = false; + else + rx_active = true; + return rx_active; +} + +void Servo::rise () +{ + t.stop (); + period_us = t.read_us (); + t.reset (); + t.start (); +} +void Servo::fall () +{ + pulse_width_us = t.read_us (); + clock_old = clock_new; + clock_new = clock(); + if ((clock_new - clock_old) < 4) + rx_active = true; +} +void Servo::out_pw_set (double outpw) +{ + if (outpw > 1.0) + outpw = 1.0; + if (outpw < 0.0) + outpw = 0.0; + outpw *= 1.2; // Change range to 1.2 ms to cover out pulse width range 0.9 to 2.1 ms + outpw += 0.9; // Bias to nom min servo range + pulsewidth_ticks ((uint32_t)(outpw * (SystemCoreClock / 2000))); +} +void Servo::period_ticks (uint32_t pt) +{ + if (out) out->period_ticks (pt); +} +void Servo::pulsewidth_ticks (uint32_t wt) +{ + if (out) out->pulsewidth_ticks (wt); +} +void Servo::set_out (FastPWM * op) +{ + out = op; +} + +void Servo1rise () +{ + S1.rise (); +} +void Servo1fall () +{ + S1.fall (); +} +void Servo2rise () +{ + S2.rise (); +} +void Servo2fall () +{ + S2.fall (); +} +//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; +/* + 5 1 3 2 6 4 SENSOR SEQUENCE + +1 1 1 1 0 0 0 ---___---___ +2 0 0 1 1 1 0 __---___---_ +4 1 0 0 0 1 1 -___---___-- + + UV WV WU VU VW UW OUTPUT SEQUENCE +*/ +const uint16_t A_tabl[] = { + 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake + 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 + 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 + 0, BRA,BRA,BRA,BRA,BRA,BRA,0 // Regenerative Braking +} ; +const uint16_t B_tabl[] = { + 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake + 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 + 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 + 0, BRB,BRB,BRB,BRB,BRB,BRB,0 // Regenerative Braking +} ; + + +class motor +{ + struct currents { + uint32_t max, min, ave; + } I; + uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth + uint32_t Hall_tab_ptr, latest_pulses_per_sec; + const uint16_t * lut; + FastPWM * maxV, * maxI; + PortOut * Motor_Port; +public: + uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored + uint32_t index; + motor () {} ; // Default constructor + motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * ) ; + void set_V_limit (double) ; // Sets max motor voltage + void set_I_limit (double) ; // Sets max motor current + void Hall_change () ; // Called in response to edge on Hall input pin + bool set_mode (int); + void current_calc () ; + uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec +} ; //MotorA, MotorB; + +motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl); +motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl); + +motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr ) // Constructor +{ + maxV = _maxV_; + maxI = _maxI_; + Hall_total = mode = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking + index = 0; + Hall_tab_ptr = 0; + latest_pulses_per_sec = 0; + for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++) + edge_count_table[i] = 0; + if (lutptr != A_tabl && lutptr != B_tabl) + pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n"); + Motor_Port = P; + pc.printf ("In motor constructor, Motor port = %lx\r\n", P); + maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz + maxI->period_ticks (MAX_PWM_TICKS + 1); + maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20); + maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30); +// if (P != PortA && P != PortB) +// pc.printf ("Fatal in 'motor' constructor, Invalid Port\r\n"); +// else +// PortOut Motor_P (P, *mask); // PortA for motor A, PortB for motor B + lut = lutptr; +} + +void motor::current_calc () +{ + I.min = 0x0fffffff; // samples are 16 bit + I.max = 0; + I.ave = 0; + uint16_t sample; + for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) { + sample = current_samples[i]; + I.ave += sample; + if (I.min > sample) + I.min = sample; + if (I.max < sample) + I.max = sample; + } + I.ave /= CURRENT_SAMPLES_AVERAGED; +} + +void motor::set_V_limit (double p) // Sets max motor voltage +{ + if (p < 0.0) + p = 0.0; + if (p > 1.0) + p = 1.0; +// last_pwm = p; + p *= 0.95; // need limit, ffi see MCP1630 data + p = 1.0 - p; // because pwm is wrong way up + maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts +} + +void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level +{ + int a; + if (p < 0.0) + p = 0.0; + if (p > 1.0) + p = 1.0; + a = (int)(p * MAX_PWM_TICKS); + if (a > MAX_PWM_TICKS) + a = MAX_PWM_TICKS; + if (a < 0) + a = 0; + maxI->pulsewidth_ticks (a); // PWM +} + +uint32_t motor::pulses_per_sec () // call this once per main loop pass to keep count = edges per sec +{ + uint32_t tmp = Hall_total; + latest_pulses_per_sec = tmp - edge_count_table[Hall_tab_ptr]; + edge_count_table[Hall_tab_ptr] = tmp; + Hall_tab_ptr++; + if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz) + Hall_tab_ptr = 0; + return latest_pulses_per_sec; +} + +bool motor::set_mode (int m) +{ + if (m != HANDBRAKE && m != FORWARD && m != REVERSE && m !=REGENBRAKE) + return false; + mode = m; + return true; +} + +void motor::Hall_change () +{ + Hall_total++; + mode &= 0x038L; // safety + *Motor_Port = lut[mode + index]; +} +void MAH1r () +{ + MotorA.index = 1; + if (MAH2 != 0) MotorA.index |= 2; + if (MAH3 != 0) MotorA.index |= 4; + MotorA.Hall_change (); +} +void MAH1f () +{ + MotorA.index = 0; + if (MAH2 != 0) MotorA.index |= 2; + if (MAH3 != 0) MotorA.index |= 4; + MotorA.Hall_change (); +} +void MAH2r () +{ + MotorA.index = 2; + if (MAH1 != 0) MotorA.index |= 1; + if (MAH3 != 0) MotorA.index |= 4; + MotorA.Hall_change (); +} +void MAH2f () +{ + MotorA.index = 0; + if (MAH1 != 0) MotorA.index |= 1; + if (MAH3 != 0) MotorA.index |= 4; + MotorA.Hall_change (); +} +void MAH3r () +{ + MotorA.index = 4; + if (MAH1 != 0) MotorA.index |= 1; + if (MAH2 != 0) MotorA.index |= 2; + MotorA.Hall_change (); +} +void MAH3f () +{ + MotorA.index = 0; + if (MAH1 != 0) MotorA.index |= 1; + if (MAH2 != 0) MotorA.index |= 2; + MotorA.Hall_change (); +} + +void MBH1r () +{ + MotorB.index = 1; + if (MBH2 != 0) MotorB.index |= 2; + if (MBH3 != 0) MotorB.index |= 4; + MotorB.Hall_change (); +} +void MBH1f () +{ + MotorB.index = 0; + if (MBH2 != 0) MotorB.index |= 2; + if (MBH3 != 0) MotorB.index |= 4; + MotorB.Hall_change (); +} +void MBH2r () +{ + MotorB.index = 2; + if (MBH1 != 0) MotorB.index |= 1; + if (MBH3 != 0) MotorB.index |= 4; + MotorB.Hall_change (); +} +void MBH2f () +{ + MotorB.index = 0; + if (MBH1 != 0) MotorB.index |= 1; + if (MBH3 != 0) MotorB.index |= 4; + MotorB.Hall_change (); +} +void MBH3r () +{ + MotorB.index = 4; + if (MBH1 != 0) MotorB.index |= 1; + if (MBH2 != 0) MotorB.index |= 2; + MotorB.Hall_change (); +} +void MBH3f () +{ + MotorB.index = 0; + if (MBH1 != 0) MotorB.index |= 1; + if (MBH2 != 0) MotorB.index |= 2; + MotorB.Hall_change (); +} + + +// End of Interrupt Service Routines + +void buggery_fuck () // simulate hall movement to observe port output bits +{ + /* MotorA.index++; + if (MotorA.index > 6) + MotorA.index = 1; + MotorA.Hall_change (); + */ +} +void AtoD_reader () +{ + static uint32_t i = 0, tab_ptr = 0; + if (AtoD_Semaphore > 20) { + pc.printf ("WARNING - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore); + AtoD_Semaphore = 20; + } + while (AtoD_Semaphore > 0) { + AtoD_Semaphore--; + // Code here to sequence through reading voltmeter, demand pot, ammeters + switch (i) { // + case 0: + volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading + volt_reading >>= 1; // Result = Result / 2 + break; // i.e. Very simple digital low pass filter + case 1: + driverpot_reading += Ain_DriverPot.read_u16 (); + driverpot_reading >>= 1; + break; + case 2: + MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); // + break; + case 3: + MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); // + if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter + tab_ptr = 0; + break; + } + i++; // prepare to read the next input in response to the next interrupt + if (i > 3) + i = 0; + } +} + +/** double Read_DriverPot () +* driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine +* ISR also filters signal +* This function returns normalised double (range 0.0 to 1.0) representation of driver pot position +*/ +double Read_DriverPot () +{ +#ifdef KNIFEANDFORK + return test_pot; // may be altered using cli +#else + return (double) driverpot_reading / 65535.0; // Normalise 0.0 <= control pot <= 1.0 +#endif +} + +double Read_BatteryVolts () +{ + return (double) volt_reading / 1800.0; // divisor fiddled to make voltage reading correct ! +} + +double read_volts () // A test function +{ + pc.printf ("pot = %.4f, System Voltage = %.2f\r\n", Read_DriverPot(), Read_BatteryVolts()); + return Read_BatteryVolts(); +} + + +extern void command_line_interpreter () ; +extern int check_24LC64 () ; // Call from near top of main() to init i2c bus +extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ; +extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ; + +int main() +{ + int j = 0; + uint32_t Apps, Bpps; + + MotA = 0; // Motor drive ports A and B + MotB = 0; + MAH1.rise (& MAH1r); + MAH1.fall (& MAH1f); + MAH2.rise (& MAH2r); + MAH2.fall (& MAH2f); + MAH3.rise (& MAH3r); + MAH3.fall (& MAH3f); + + MBH1.rise (& MBH1r); + MBH1.fall (& MBH1f); + MBH2.rise (& MBH2r); + MBH2.fall (& MBH2f); + MBH3.rise (& MBH3r); + MBH3.fall (& MBH3f); + + MAH1.mode (PullUp); + MAH2.mode (PullUp); + MAH3.mode (PullUp); + MBH1.mode (PullUp); + MBH2.mode (PullUp); + MBH3.mode (PullUp); + pc.printf ("\tAbandon Hope %d\r\n", LED ? 0 : 1); + // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc + tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator + loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator + // Done setting up system interrupt timers + + const int TXTBUFSIZ = 36; + char buff[TXTBUFSIZ]; + bool eerom_detected = false; + pc.printf ("RAM test - "); + if (check_24LC64() != 0xa0) // searches for i2c devices, returns address of highest found + pc.printf ("Check for 24LC64 eeprom FAILED\r\n"); + else // i2c.write returned 0, think this means device responded with 'ACK', found it anyway + eerom_detected = true; + if (eerom_detected) { + bool j, k; + pc.printf ("ok\r\n"); + static const char ramtst[] = "I found the man sir!"; + j = wr_24LC64 (0x1240, (char*)ramtst, strlen(ramtst)); + for (int i = 0; i < TXTBUFSIZ; i++) buff[i] = 0; // Clear buffer + // need a way to check i2c busy - YES implemented ack_poll + k = rd_24LC64 (0x1240, buff, strlen(ramtst)); + pc.printf("Ram test returned [%s], wr ret'd [%s], rd ret'd [%s]\r\n", buff, j ? "true" : "false", k ? "true" : "false"); + } + T1 = 0; // As yet unused pins + T2 = 0; + T3 = 0; + T4 = 0; + T5 = 0; + T6 = 0; +// MotorA.set_mode (HANDBRAKE); +// MotorB.set_mode (HANDBRAKE); + MotorA.set_mode (FORWARD); + MotorB.set_mode (FORWARD); + MotorA.set_V_limit (0.1); + MotorB.set_V_limit (0.0); + MotorA.set_I_limit (0.0); + MotorB.set_I_limit (0.0); + + // Setup Complete ! Can now start main control forever loop. + + while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true + while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port + command_line_interpreter () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true + AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts + } + loop_flag = false; // Clear flag set by ticker interrupt handler + Apps = MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second + Bpps = MotorB.pulses_per_sec (); + + buggery_fuck (); + + // do stuff + if (flag_8Hz) { // do slower stuff + flag_8Hz = false; + LED = !LED; // Toggle LED on board, should be seen to fast flash + j++; + if (j > 6) { // Send some status info out of serial port every second and a bit or thereabouts + j = 0; +//double pres = PressureSensor.getPressure (); +//double pres = PressureSensor.getTemperature (); + pc.printf ("Apps %d, Bpps %d, sys_timer %d\r\n", Apps, Bpps, sys_timer); +// pc.printf ("Apps %d, Bpps %d, sys_timer %d, pressure %.2f\r\n", Apps, Bpps, sys_timer, pres); + // pc.printf ("V=%+.1f, I=%+.1f, CtrlOut %.3f, RPM %d, %s\r\n", Read_BatteryVolts(), AmpsReading, ControlOut, ReadEngineRPM(), engine_warm ? "Running mode" : "Startup mode"); + } + } // End of if(flag_8Hz) + } // End of main programme loop +}