File content as of revision 6:f289a49c1eae:

#include "mbed.h"
#include "DualBLS.h"
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "Servo.h"

/*
New 29th May 2018 - YET TO CODE FOR - Fwd/Rev line from possible remote hand control box has signal routed to T5
                Also new LMT01 temperature sensor routed to T1 - and rerouted to PC_13 at InterruptIn on T1 (ports A and B I think) not workable
*/


/*  STM32F401RE - compile using NUCLEO-F401RE
//  PROJECT -   Dual Brushless Motor Controller -   Jon Freeman     June 2018.

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 ...

____________________________________________________________________________________


*/
//#if defined (TARGET_NUCLEO_F446ZE)
#if defined (TARGET_NUCLEO_F401RE)

//  Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and 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) - GOOD, works well with auto-tickle of high side drivers
AVL = 1 << 6,   //  These are which port bits connect to which mosfet driver
AWL = 1 << 4,

AUH = 1 << 1,
AVH = 1 << 7,
AWH = 1 << 8,

AUV =   AUH | AVL,  //  Each of 6 possible output energisations made up of one hi and one low
AVU =   AVH | AUL,
AUW =   AUH | AWL,
AWU =   AWH | AUL,
AVW =   AVH | AWL,
AWV =   AWH | AVL,

KEEP_L_MASK_A   = AUL | AVL | AWL,
KEEP_H_MASK_A   = AUH | AVH | AWH,

BRA = AUL | AVL | AWL,  //  All low side switches on (and all high side off) for braking

BUL = 1 << 0,   //  Likewise for MotorB but different port bits on different port
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,

KEEP_L_MASK_B   = BUL | BVL | BWL,
KEEP_H_MASK_B   = BUH | BVH | BWH,

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);   //  Activate output ports to motor drivers
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
InterruptIn   Temperature_pin   (PC_13);//  Pin 2   June 2018 - taken for temperature sensor - hard wired to T1 due to wrong thought T1 could be InterruptIn


//  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
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 com2 pins 37 Tx, 38 Rx
BufferedSerial  com2          (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 com3 pins 42 Tx, 43 Rx
//InterruptIn tryseredge  (PA_9);
BufferedSerial  com3        (PA_9, PA_10);    //    Pins 42, 43  tx, rx to any aux module
//  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 !
//  No.

//  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
// was DigitalOut  T1  (PA_12);    //  Pin 45


//InterruptIn T1  (PA_12);    //  Pin 45 now input counting pulses from LMT01 temperature sensor
//  InterruptIn DOES NOT WORK ON PA_12. Boards are being made, will have to wire link PA12 to PC13
DigitalIn   T1    (PA_12);
////InterruptIn T1  (PC_13);    //  Pin 45 now input counting pulses from LMT01 temperature sensor



//  Pin 46  SWDIO
//  Pin 47  VSS
//  Pin 48  VDD
//  Pin 49  SWCLK

//Was DigitalOut  T5  (PA_15); //  Pin 50
DigitalIn   T5  (PA_15); //  Pin 50 now fwd/rev from remote control box if fitted
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

//  Servo pins, 2 off. Configured as Input to read radio control receiver
//  If used as servo output, code gives pin to 'Servo' - seems to work
InterruptIn Servo1_i    (PB_8); //  Pin 61  to read output from rc rx
InterruptIn Servo2_i    (PB_9); //  Pin 62  to read output from rc rx

//  Pin 63  VSS
//  Pin 64  VDD
//  SYSTEM CONSTANTS

#endif
#if defined (TARGET_NUCLEO_F446ZE)
#endif
/*  Global variable declarations */
volatile    uint32_t    fast_sys_timer      = 0;    //  gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
int         WatchDog    = WATCHDOG_RELOAD + 80; //  Allow extra few seconds at powerup
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;
int         IAm;
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        temp_sensor_exists = false;
char        mode_bytes[36];

uint32_t    temp_sensor_count = 0,  //  incremented by every rising edge from LMT01
            last_temp_count = 0;  //  global updated approx every 100ms after each LMT01 conversion completes
//    struct  single_bogie_options    bogie;    
/*  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
Ticker  temperature_find_ticker;
Timer   temperature_timer;

//  Interrupt Service Routines
void    ISR_temperature_find_ticker ()  {   //  every 960 us, i.e. slightly faster than once per milli sec
    static  bool    readflag = false;
    int t = temperature_timer.read_ms   ();
    if  ((t == 5) && (!readflag))    {
        last_temp_count = temp_sensor_count;
        temp_sensor_count = 0;
        readflag = true;
    }
    if  (t == 6)
        readflag = false;
}

/** 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 ; VOLTAGE_READ_INTERVAL_US    = 50
{
    AtoD_Semaphore++;
    fast_sys_timer++;        //  Just a handy measure of elapsed time for anything to use
}


class   RControl_In
{  //  Read servo style pwm input
    Timer   t;
    int32_t clock_old, clock_new;
    int32_t pulse_width_us, period_us;
public:
    RControl_In   ()   {
        pulse_width_us = period_us = 0;
        com2.printf ("Setting up Radio_Congtrol_In\r\n");
    }   ;
    bool    validate_rx ()  ;
    void    rise    ()  ;
    void    fall    ()  ;
    uint32_t    pulsewidth    ()  ;
    uint32_t    period    ()  ;
    bool    rx_active;
}   ;

uint32_t    RControl_In::pulsewidth   ()  {
    return  pulse_width_us;
}

uint32_t    RControl_In::period   ()  {
    return  period_us;
}

bool    RControl_In::validate_rx ()
{
    if  ((clock() - clock_new) > 4)
        rx_active = false;
    else
        rx_active = true;
    return  rx_active;
}

void    RControl_In::rise    ()     //  These may not work as use of PortB as port may bugger InterruptIn use
{
    t.stop   ();
    period_us = t.read_us    ();
    t.reset ();
    t.start ();
}
void    RControl_In::fall    ()
{
    pulse_width_us = t.read_us   ();
    clock_old = clock_new;
    clock_new = clock();
    if  ((clock_new - clock_old) < 4)
        rx_active = true;
}


Servo * Servos[2];

//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  ---___---___ Hall1
2   0   0   1   1   1   0  __---___---_ Hall2
4   1   0   0   0   1   1  -___---___-- Hall3

    UV  WV  WU  VU  VW  UW  OUTPUT SEQUENCE
*/
const   uint16_t    A_tabl[] = {  //  Origial table
    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
    KEEP_L_MASK_A, KEEP_H_MASK_A   //  [32 and 33]
}   ;
InterruptIn * AHarr[] = {
    &MAH1,
    &MAH2,
    &MAH3
}   ;
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
    KEEP_L_MASK_B, KEEP_H_MASK_B
}   ;
InterruptIn * BHarr[] = {
    &MBH1,
    &MBH2,
    &MBH3
}   ;

class   motor
{
    uint32_t    Hall_total, visible_mode, inner_mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; //  to contain one seconds worth
    uint32_t    latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp;
    bool    moving_flag;
    const   uint16_t * lut;
    FastPWM * maxV, * maxI;
    PortOut * Motor_Port;
    InterruptIn * Hall1, * Hall2, * Hall3;
public:
    struct  currents    {
        uint32_t    max, min, ave;
    }  I;
    int32_t     angle_cnt;
    uint32_t    current_samples[CURRENT_SAMPLES_AVERAGED];  //  Circular buffer where latest current readings get stored
    uint32_t    Hindex[2], tickleon, encoder_error_cnt;
    uint32_t    RPM, PPS;
    double      last_V, last_I;
    motor   ()  {}  ;   //  Default constructor
    motor   (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **)  ;
    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
    void    motor_set   ()  ;           //  Energise Port with data determined by Hall sensors
    void    direction_set   (int)  ;    //  sets 'direction' with bit pattern to eor with FORWARD or REVERSE in set_mode
    bool    set_mode    (int);          //  sets mode to HANDBRAKE, FORWARD, REVERSE or REGENBRAKE
    bool    is_moving   ()  ;           //  Returns true if one or more Hall transitions within last 31.25 milli secs
    void    current_calc    ()  ;       //  Updates 3 uint32_t I.min, I.ave, I.max
    uint32_t    pulses_per_sec   ()  ;  //  call this once per main loop pass to keep count = edges per sec
    int     read_Halls  ()  ;           //  Returns 3 bits of latest Hall sensor outputs
    void    high_side_off   ()  ;
}   ;   //MotorA, MotorB, or even Motor[2];

motor   MotorA  (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
motor   MotorB  (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr);

motor * MotPtr[8];  //  Array of pointers to some number of motor objects

motor::motor    (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall)        //  Constructor
{   //  Constructor
    maxV = _maxV_;
    maxI = _maxI_;
    Hall_total = 0;  //  mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
    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");
    Hall_tab_ptr = 0;
    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);
    visible_mode    = REGENBRAKE;
    inner_mode      = REGENBRAKE;
    lut = lutptr;
    Hindex[0] = Hindex[1]  = read_Halls    ();
    ppstmp  = 0;
    tickleon    = 0;
    direction   = 0;
    angle_cnt   = 0;        //  Incremented or decremented on each Hall event according to actual measured direction of travel
    encoder_error_cnt = 0;  //  Incremented when Hall transition not recognised as either direction
    Hall1 = Hall[0];
    Hall2 = Hall[1];
    Hall3 = Hall[2];
    PPS     = 0;
    RPM     = 0;
    last_V = last_I = 0.0;
}

/**
void    motor::direction_set   (int dir)  {
Used to set direction according to mode data from eeprom
*/
void    motor::direction_set   (int dir)  {
    if  (dir != 0)
        dir = FORWARD | REVERSE;  //  bits used in eor
    direction = dir;
}

int     motor::read_Halls  ()  {
    int x = 0;
    if  (*Hall1 != 0)    x |= 1;
    if  (*Hall2 != 0)    x |= 2;
    if  (*Hall3 != 0)    x |= 4;
    return  x;
}

void    motor::high_side_off   ()  {
    uint16_t    p = *Motor_Port;
    p &= lut[32];   //  KEEP_L_MASK_A or B
    *Motor_Port = p;
}

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_V = p;     //  for read by diagnostics
    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;
    last_I = p;
    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_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
{           //  Can also test for motor running or not here
    if  (ppstmp == Hall_total)  {
        moving_flag  = false;       //  Zero Hall transitions since previous call - motor not moving
        tickleon    = TICKLE_TIMES;
    }
    else    {
        moving_flag  = true;
        ppstmp = Hall_total;
    }
    latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
    edge_count_table[Hall_tab_ptr] = ppstmp;
    Hall_tab_ptr++;
    if  (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
        Hall_tab_ptr = 0;
    PPS = latest_pulses_per_sec;
    RPM = (latest_pulses_per_sec * 60) / 24;
    return  latest_pulses_per_sec;
}

bool    motor::is_moving ()
{
    return  moving_flag;
}

/**
bool    motor::set_mode (int m)
Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
If this causes change of mode, also sets V and I to zero.
*/
bool    motor::set_mode (int m)
{
    if  ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE))  {
        pc.printf   ("Error in set_mode, invalid mode %d\r\n", m);
        return  false;
    }
    if  (visible_mode != m) {   //  Mode change, kill volts and amps to be safe
        set_V_limit (0.0);
        set_I_limit (0.0);
        visible_mode = m;
    }
    if  (m == FORWARD || m == REVERSE)
        m ^= direction;
    inner_mode = m;     //  idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
    return  true;
}

void    motor::Hall_change  ()
{
    const   int32_t delta_theta_lut[] =     //  Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
            {
                0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 0
                0, 0, 0,-1, 0, 1, 0, 0,     //  Previous Hindex was 1
                0, 0, 0, 1, 0, 0,-1, 0,     //  Previous Hindex was 2
                0, 1,-1, 0, 0, 0, 0, 0,     //  Previous Hindex was 3
                0, 0, 0, 0, 0,-1, 1, 0,     //  Previous Hindex was 4
                0,-1, 0, 0, 1, 0, 0, 0,     //  Previous Hindex was 5
                0, 0, 1, 0,-1, 0, 0, 0,     //  Previous Hindex was 6
                0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 7
            }  ;
    int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
    if  (delta_theta == 0)
        encoder_error_cnt++;
    else
        angle_cnt += delta_theta;
    *Motor_Port = lut[inner_mode | Hindex[0]];  //  changed mode to inner_mode 27/04/18
    Hall_total++;
    Hindex[1] = Hindex[0];
}


void    temp_sensor_isr ()  {   //  got rising edge from LMT01. ALMOST CERTAIN this misses some
    int t = temperature_timer.read_us   (); //  Must be being overrun by something, most likely culprit A-D reading ?
    temperature_timer.reset ();
    temp_sensor_count++;
    if  (t > 18)            //  Yes proved some interrupts get missed, this fixes temperature reading
        temp_sensor_count++;
//    T2 = !T2;   //  scope hanger
}

void    MAH_isr    ()
{
    uint32_t x = 0;
    MotorA.high_side_off    ();
    T3 = !T3;
    if  (MAH1 != 0) x |= 1;
    if  (MAH2 != 0) x |= 2;
    if  (MAH3 != 0) x |= 4;
    MotorA.Hindex[0] = x;       //  New in [0], old in [1]
    MotorA.Hall_change  ();
}

void    MBH_isr    ()
{
    uint32_t x = 0;
    MotorB.high_side_off    ();
    if  (MBH1 != 0) x |= 1;
    if  (MBH2 != 0) x |= 2;
    if  (MBH3 != 0) x |= 4;
    MotorB.Hindex[0] = x;
    MotorB.Hall_change  ();
}


//  End of Interrupt Service Routines

void    motor::motor_set  ()
{
    Hindex[0]  = read_Halls    ();
    *Motor_Port = lut[inner_mode | Hindex[0]];
}

void    setVI   (double v, double i)  {
    MotorA.set_V_limit  (v);  //  Sets max motor voltage
    MotorA.set_I_limit  (i);  //  Sets max motor current
    MotorB.set_V_limit  (v);
    MotorB.set_I_limit  (i);
}
void    setV    (double v)  {
    MotorA.set_V_limit  (v);
    MotorB.set_V_limit  (v);
}
void    setI    (double i)  {
    MotorA.set_I_limit  (i);
    MotorB.set_I_limit  (i);
}

void    read_RPM    (uint32_t * dest)  {
    dest[0] = MotorA.RPM;
    dest[1] = MotorB.RPM;
}

void    read_PPS    (uint32_t * dest)  {
    dest[0] = MotorA.PPS;
    dest[1] = MotorB.PPS;
}

void    read_last_VI    (double * d)  {   //  only for test from cli
    d[0]    = MotorA.last_V;
    d[1]    = MotorA.last_I;
    d[2]    = MotorB.last_V;
    d[3]    = MotorB.last_I;
}


/**
void    AtoD_reader ()  //  Call to here every VOLTAGE_READ_INTERVAL_US    = 50 once loop responds to flag set in isr
Not part of ISR
*/
void    AtoD_reader ()  //  Call to here every VOLTAGE_READ_INTERVAL_US    = 50 once loop responds to flag set in isr
{
    static uint32_t i = 0, tab_ptr = 0;

    if  (MotorA.tickleon)
        MotorA.high_side_off    ();
    if  (MotorB.tickleon)
        MotorB.high_side_off    ();
    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;
    }   //  end of while (AtoD_Semaphore > 0)    {
    if  (MotorA.tickleon)   {
        MotorA.tickleon--;
        MotorA.motor_set    (); //  Reactivate any high side switches turned off above
    }
    if  (MotorB.tickleon)   {
        MotorB.tickleon--;
        MotorB.motor_set    ();
    }
}

/** 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  ()
{
    return ((double) driverpot_reading) / 65536.0;     //  Normalise 0.0 <= control pot <= 1.0
}

double  Read_BatteryVolts   ()
{
    return  ((double) volt_reading) / 951.0;    //  divisor fiddled to make voltage reading correct !
}

void    read_supply_vi   (double * val)  {  //  called from cli
    val[0] = MotorA.I.ave;
    val[1] = MotorB.I.ave;
    val[2] = Read_BatteryVolts  ();
}

void    mode_set   (int mode, double val)  {   //  called from cli to set fw, re, rb, hb
    MotorA.set_mode (mode);
    MotorB.set_mode (mode);
    if  (mode == REGENBRAKE)    {
        if  (val > 1.0)
            val = 1.0;
        if  (val < 0.0)
            val = 0.0;
        val *= 0.9;    //  set upper limit, this is essential
        val = sqrt  (val);  //  to linearise effect
        setVI  (val, 1.0);
    }
}

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)   ;

/*struct  motorpairoptions    {   //  This to be user settable in eeprom, 32 bytes
    uint8_t MotA_dir,   //  0 or 1
            MotB_dir,   //  0 or 1
            gang,       //  0 for separate control (robot mode), 1 for ganged loco bogie mode
            serv1,      //  0, 1, 2 = Not used, Input, Output
            serv2,      //  0, 1, 2 = Not used, Input, Output
            cmd_source, //  0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
            last;
}   ;
*/
int I_Am    ()  {   //  Returns boards id number as ASCII char
    return  IAm;
}


int main()
{
    int eighth_sec_count = 0;

    MotA   = 0;     //  Output all 0s to Motor drive ports A and B
    MotB   = 0;
    MotPtr[0] = &MotorA;    //  Pointers to motor class objects
    MotPtr[1] = &MotorB;
    
    Temperature_pin.fall (&temp_sensor_isr);
    Temperature_pin.mode (PullUp);

    MAH1.rise   (& MAH_isr);    //  Set up interrupt vectors
    MAH1.fall   (& MAH_isr);
    MAH2.rise   (& MAH_isr);
    MAH2.fall   (& MAH_isr);
    MAH3.rise   (& MAH_isr);
    MAH3.fall   (& MAH_isr);

    MBH1.rise   (& MBH_isr);
    MBH1.fall   (& MBH_isr);
    MBH2.rise   (& MBH_isr);
    MBH2.fall   (& MBH_isr);
    MBH3.rise   (& MBH_isr);
    MBH3.fall   (& MBH_isr);

    MAH1.mode   (PullUp);
    MAH2.mode   (PullUp);
    MAH3.mode   (PullUp);
    MBH1.mode   (PullUp);
    MBH2.mode   (PullUp);
    MBH3.mode   (PullUp);
    Servo1_i.mode   (PullUp);
    Servo2_i.mode   (PullUp);

    //  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
    temperature_find_ticker.attach_us   (&ISR_temperature_find_ticker, 960);
    //  Done setting up system interrupt timers
    temperature_timer.start ();

//    const   int TXTBUFSIZ = 36;
//    char    buff[TXTBUFSIZ];
    pc.baud     (9600);
    com3.baud   (1200);
    com2.baud   (19200);

    if  (check_24LC64() != 0xa0)  { //  searches for i2c devices, returns address of highest found
        pc.printf   ("Check for 24LC64 eeprom FAILED\r\n");
        com2.printf   ("Check for 24LC64 eeprom FAILED\r\n");
    }
    else   {        //  Found 24LC64 memory on I2C
        bool k;
//        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");
//        com2.printf("Ram test returned [%s], wr ret'd [%s], rd ret'd [%s]\r\n", buff, j ? "true" : "false", k ? "true" : "false");
        k = rd_24LC64   (0, mode_bytes, 32);
//        if  (k)
//            com2.printf ("Good read from eeprom\r\n");
        if  (!k)
            com2.printf ("Error reading from eeprom\r\n");

        int err = 0;
        for (int i = 0; i < numof_eeprom_options; i++) {
            if  ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max))  {
                com2.printf ("EEROM error with %s\r\n", option_list[i].t);
                err++;
            }
//            else
//                com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
        }
        IAm = '0';
        if  (err == 0)  {
            MotorA.direction_set    (mode_bytes[MOTADIR]);
            MotorB.direction_set    (mode_bytes[MOTBDIR]);
            IAm = mode_bytes[ID];
        }
           //  Alternative ID 1 to 9
//        com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
    }
//    T1 = 0;   Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
    T2 = 0; //  T2, T3, T4 As yet unused pins
    T3 = 0;
    T4 = 0;
//    T5 = 0; now input from fw/re on remote control box
    T6 = 0;
//    MotPtr[0]->set_mode (REGENBRAKE);
    MotorA.set_mode (REGENBRAKE);
    MotorB.set_mode (REGENBRAKE);
    setVI  (0.9, 0.5);

    Servos[0] = Servos[1] = NULL;
    //  NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
    //  Only works with unconditional inline code
    //  However, setup code for Input by default,
    //  This can be used to monitor Servo output also !
    Servo   Servo1  (PB_8)  ;
    Servos[0] = & Servo1;
    Servo   Servo2  (PB_9)  ;
    Servos[1] = & Servo2;
    
    pc.printf   ("last_temp_count = %d\r\n", last_temp_count);  //  Has had time to do at least 1 conversion
    if  ((last_temp_count > 160) && (last_temp_count < 2400))   //  in range -40 to +100 degree C
        temp_sensor_exists  = true;
/*
    //  Setup Complete ! Can now start main control forever loop.
    //  March 16th 2018 thoughts !!!
    //  Control from one of several sources and types as selected in options bytes in eeprom.
    //  Control could be from e.g. Pot, Com2, Servos etc.
    //  Suggest e.g.
*/    /*
    switch  (mode_byte) {   //  executed once only upon startup
        case    POT:
            while   (1) {   //  forever loop
                call    common_stuff    ();
                ...
            }
            break;
        case    COM2:
            while   (1) {   //  forever loop
                call    common_stuff    ();
                ...
            }
            break;
    }
    */
    pc.printf   ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
    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
        MotorA.pulses_per_sec   ();   //  Needed to keep table updated to give reading in Hall transitions per second
        MotorB.pulses_per_sec   ();   //  Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
        T4 = !T4;   //  toggle to hang scope on to verify loop execution
        //  do stuff
        if  (flag_8Hz)  {   //  do slower stuff
            flag_8Hz    = false;
            LED = !LED;                   // Toggle LED on board, should be seen to fast flash
            WatchDog--;
            if  (WatchDog == 0) {   //  Deal with WatchDog timer timeout here
                setVI  (0.0, 0.0);  //  set motor volts and amps to zero
                com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f));   //  Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
            }                       //  End of dealing with WatchDog timer timeout
            if  (WatchDog < 0)
                WatchDog = 0;
            eighth_sec_count++;
            if  (eighth_sec_count > 6)    {   //  Send some status info out of serial port every second and a bit or thereabouts
                eighth_sec_count = 0;
                MotorA.current_calc ();     //  Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
                MotorB.current_calc ();
                if  (temp_sensor_exists)    {
                    double  tmprt = (double) last_temp_count;
                    tmprt /= 16.0;
                    tmprt -= 50.0;
                    pc.printf   ("Temp %.2f\r\n", tmprt);
                }
//                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);
            }
        }   //  End of if(flag_8Hz)
    }       //  End of main programme loop
}