File content as of revision 0:435bf84ce48a:

#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
}