File content as of revision 10:e40d8724268a:

//  Cloned from 'DualBLS2018_06' on 23 November 2018
#include "mbed.h"
//#include    "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
#include    "stm32f401xe.h"
#include "DualBLS.h"
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "Servo.h"
#include "brushless_motor.h"

/*
Brushless_STM3 board

New 29th May 2018 **
                LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as 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_F401RE)  //  CPU in 64 pin LQFP
#include    "F401RE.h"  //  See here for warnings about Servo InterruptIn not working
#endif
#if defined (TARGET_NUCLEO_F446ZE)  //  CPU in 144 pin LQFP
#include    "F446ZE.h"              //  A thought for future version
#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
bool        WatchDogEnable  = false;    //  Must recieve explicit instruction from pc or controller to enable
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;
bool        eeprom_flag; //  gets set according to 24LC674 being found or not
bool        mode_good_flag  = 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;
double  rpm2mph = 0.0;  //  gets calculated from eeprom mode entries if present
/*  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;
Timer   dc_motor_kicker_timer;
Timeout motors_restore;

//  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
    Checks for __-__ duration 800-2200us
    Checks repetition rate in range 5-25ms
*/
class   RControl_In
{
    //  Read servo style pwm input
    Timer   t;
    int32_t pulse_width_us, period_us, pulse_count;
public:
    RControl_In   ()   {    //  Default Constructor
        pulse_width_us = period_us = pulse_count = 0;
        rx_active = false;
        com2.printf ("Setting up Radio_Control_In\r\n");
    }   ;
    bool    validate_rx ()  ;
    void    rise    ()  ;       //  InterruptIn ISRs redirected to these
    void    fall    ()  ;
    uint32_t    pulsecount    ()  ;
    uint32_t    pulsewidth    ()  ;
    uint32_t    period    ()  ;
    double  normalised    ();   //  Returns 0.0 <= p <= 1.0 or something else when rc not active
    bool    rx_active;
}   ;


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

uint32_t    RControl_In::pulsecount   ()
{
    return  pulse_count;
}

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

bool    RControl_In::validate_rx ()
{   //  Tests for pulse width and repetition rates being believable
    if  ((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200))
    {
        rx_active = false;
        pc.printf   ("RC per=%d, pw=%d\r\n", period_us, pulse_width_us);
    }
    else
        rx_active = true;
    return  rx_active;
}

double  RControl_In::normalised  ()  {
    if  (!validate_rx())
        return  0.0;       //  ** WHAT TO RETURN WHEN RC NOT ACTIVE ? **
    double norm = (double) (pulse_width_us - 1000);  //  left with -200 to + 1200 allowing for some margin
    norm /= 1000.0;
    if  (norm > 1.0)
        norm = 1.0;
    if  (norm < 0.0)
        norm = 0.0;
    return  norm;
}

void    RControl_In::rise    ()     //  These may not work as use of PortB as port may bugger InterruptIn use ** THIS IS SO **
{                                   //  December 2018 - ** FIXED ** by using PC_14 and 15 instead
//    t.stop   ();
    period_us = t.read_us    ();
    t.reset ();
    t.start ();
}
void    RControl_In::fall    ()
{
    pulse_width_us = t.read_us   ();
    pulse_count++;
}
//  end of RControl_In class

    RControl_In RC_chan_1, RC_chan_2;   //  Declare two radio control input channels

//  Radio Control Input Interrupt Handlers
void    RC_chan_1rise   ()  {
    RC_chan_1.rise  ();
}
void    RC_chan_1fall   ()  {
    RC_chan_1.fall  ();
}
void    RC_chan_2rise   ()  {
    RC_chan_2.rise  ();
}
void    RC_chan_2fall   ()  {
    RC_chan_2.fall  ();
}
//  End of Radio Control Input Interrupt Handlers

//Servo * Servos[2];    //  Is possible to create pointers to Servo but no need to.

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

8th July 2018
Added drive to DC brushed motors.
Connect U and W to dc motor, leave V open.

Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors

*/
const   uint16_t    A_tabl[] = {  //  Origial table
    0,  0,  0,  0,  0,  0,  0,  0,  //  Handbrake
    0,  AWHVL,AVHUL,AWHUL,AUHWL,AUHVL,AVHWL,AUHWL,  //  Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,    //  JP, FR, SG, PWM = 1 0 1 1   Forward1
    0,  AVHWL,AUHVL,AUHWL,AWHUL,AVHUL,AWHVL,AWHUL,  //  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,BRA,   //  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,  BWHVL,BVHUL,BWHUL,BUHWL,BUHVL,BVHWL,BUHWL,  //  Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,    //  JP, FR, SG, PWM = 1 0 1 1   Forward1
    0,  BVHWL,BUHVL,BUHWL,BWHUL,BVHUL,BWHVL,BWHUL,  //  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,BRB,   //  Regenerative Braking
    KEEP_L_MASK_B, KEEP_H_MASK_B
}   ;
InterruptIn * BHarr[] = {
    &MBH1,
    &MBH2,
    &MBH3
}   ;


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


void    report_motor_types  ()      //  not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
{
    pc.printf   ("Mot A is %s, Mot B is %s\r\n", MotorA.motor_is_brushless() ? "Brushless":"DC", MotorB.motor_is_brushless() ? "Brushless":"DC");
}

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    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.dc_motor)   {
//        MotorA.low_side_on  ();
//    }
//    else    {
    if  (MotorA.tickleon)
        MotorA.high_side_off    ();
//    }
//    if  (MotorB.dc_motor)   {
//        MotorB.low_side_on  ();
//    }
//    else    {
    if  (MotorB.tickleon)
        MotorB.high_side_off    ();
//    }
    if  (AtoD_Semaphore > 20)   {
        pc.printf   ("WARNING - sema cnt %d\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)   {
//    if  (MotorA.dc_motor || MotorA.tickleon)   {
        MotorA.tickleon--;
        MotorA.motor_set    (); //  Reactivate any high side switches turned off above
    }
    if  (MotorB.tickleon)   {
//    if  (MotorB.dc_motor || MotorB.tickleon)   {
        MotorB.tickleon--;
        MotorB.motor_set    ();
    }
}

/** double  Read_Servo1_In  ()
*   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_Servo1_In  ()
{
    const double    xjoin   = 0.5,
                    yjoin   = 0.35,
                    slope_a = yjoin / xjoin,
                    slope_b = (1.0 - yjoin)/(1.0 - xjoin);
//                    centre = 0.35,  //  For pot, first (1/3)ish in braking area, position = 1/3 drift, > (1/3)ish drive
//                    halfish = 0.5;  //  RC stick in the centre value
                    //  Bottom half of RC stick movement maps to lowest (1/3)ish pot movement
                    //  Higher half of RC stick movement maps to highest (2/3)ish pot movement
    double  t;
    double  demand = RC_chan_1.normalised();
    //  apply demand = 1.0 - demand here to swap stick move polarity
//    return  pow (demand, SERVOIN_PWR_BENDER);
    if  (demand < 0.0)  demand = 0.0;
    if  (demand > 1.0)  demand = 1.0;
    if  (demand < xjoin) {
        demand *= slope_a;
    }
    else    {
        t = yjoin + ((demand - xjoin) * slope_b);
        demand = t;
    }
    return  demand;
}

/** double  Read_DriverPot  ()
*   driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
*   ISR also filters signal by returning average of most recent two readings
*   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_both_motors   (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    setup_comms ()  ;
extern  void    command_line_interpreter_pc    ()  ;
extern  void    command_line_interpreter_loco    ()  ;
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;
}

double  mph (int    rpm)
{
    if  (mode_good_flag)    {
        return  rpm2mph * (double) rpm;
    }
    return  -1.0;
}

void    restorer    ()
{
    motors_restore.detach   ();
    if  (MotorA.dc_motor)   {
        MotorA.set_V_limit  (MotorA.last_V);
        MotorA.set_I_limit  (MotorA.last_I);
        MotorA.motor_set    ();
    }
    if  (MotorB.dc_motor)   {
        MotorB.set_V_limit  (MotorB.last_V);
        MotorB.set_I_limit  (MotorB.last_I);
        MotorB.motor_set    ();
    }
}

//int ttime = 3;    //  resettable via cli 'stt', used to determine suitable force low on period for driving dc motor

void    prscfuck    (int v) {
/*
int prescaler   (   int     value )     

Set the PWM prescaler.

The period of all PWM pins on the same PWM unit have to be reset after using this!

Parameters:
    value   - The required prescaler. Special values: 0 = lock current prescaler, -1 = use dynamic prescaler
    return  - The prescaler which was set (can differ from requested prescaler if not possible)

Definition at line 82 of file FastPWM_common.cpp.
*/
    if  (v < 1)
        v = 1;
    int w = A_MAX_V_PWM.prescaler   (v);
    pc.printf   ("Attempt setting prescaler %d returned %d\r\n", v, w);
}

void    rcin_report ()  {
    double c1 = RC_chan_1.normalised();
    double c2 = RC_chan_2.normalised();
    uint32_t    pc1 = RC_chan_1.pulsecount();
    uint32_t    pc2 = RC_chan_2.pulsecount();
    pc.printf   ("At rcin_report, Ch1=%.3f, Ch2=%.3f, cnt1 %d, cnt2 %d\r\n", c1, c2, pc1, pc2);
//    if  (c1 < -0.0001)
        pc.printf   ("\t1 period %d, pulsewidth %d\r\n", RC_chan_1.period(), RC_chan_1.pulsewidth());
//    if  (c2 < -0.0001)
        pc.printf   ("\t2 period %d, pulsewidth %d\r\n", RC_chan_2.period(), RC_chan_2.pulsewidth());
}

bool    worth_the_bother    (double a, double b)    {
    double c = a - b;
    if  (c < 0.0)
        c = 0.0 - c;
    if  (c > 0.02)
        return  true;
    return  false;
}

void    hand_control_state_machine  (int source)  {     //  if hand control mode '3', get here @ approx 30Hz to read drivers control pot
                                                        //  if servo1 mode '4', reads input from servo1 instead
enum    {   //  states used in hand_control_state_machine()
        HAND_CONT_IDLE,
        HAND_CONT_BRAKE_WAIT,
        HAND_CONT_BRAKE_POT,
        HAND_CONT_STATE_INTO_BRAKING,
        HAND_CONT_STATE_BRAKING,
        HAND_CONT_STATE_INTO_DRIVING,
        HAND_CONT_STATE_DRIVING
        }  ;

    static  int hand_controller_state = HAND_CONT_IDLE;
//    static  int old_hand_controller_direction = T5;              //  Nov 2018 reworked thanks to feedback from Rob and Quentin
    static  double  brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
    static  int dirbuf[5] = {100,100,100,100,100};      //  Initialised with invalid direction such that 'change' is read from switch
    static  int dirbufptr = 0, direction_old = -1, direction_new = -1;
    const   int buflen = sizeof(dirbuf) / sizeof(int);
    const   double      Pot_Brake_Range = 0.35;  //pow   (0.5, SERVOIN_PWR_BENDER); //0.353553 for SERVOIN_PWR_BENDER = 1.5;

    direction_old = direction_new;

//      Test for change in direction switch setting.
//      If whole buffer NEWLY filled with all Fwd or all Rev, state = brake_wait
    int diracc = 0;    
    if  (dirbufptr >= buflen)
        dirbufptr = 0;
    dirbuf[dirbufptr++] = T5;   //  Read direction switch into circular debounce buffer
    for (int i = 0; i < buflen; i++)
        diracc += dirbuf[i];    //  will = 0 or buflen if direction switch stable
    if  (diracc == buflen || diracc == 0)   //  if was all 0 or all 1
        direction_new = diracc / buflen;
    if  (direction_new != direction_old)
        hand_controller_state = HAND_CONT_BRAKE_WAIT;   //  validated change of direction switch

    switch  (source)    {
        case    3:  //  driver pot is control input
            pot_position = Read_DriverPot   ();     //  Only place read in the loop, rteads normalised 0.0 to 1.0
            break;
        case    4:  //  servo 1 input is control input
            break;
        default:
            pot_position = 0.0;
            break;
    }

    switch  (hand_controller_state) {
        case    HAND_CONT_IDLE:         //  Here for first few passes until validated direction switch reading
            break;

        case    HAND_CONT_BRAKE_WAIT:
            pc.printf   ("At HAND_CONT_BRAKE_WAIT\r\n");
            brake_effort = 0.05;    //  Apply braking not too fiercely
            mode_set_both_motors    (REGENBRAKE, brake_effort);  //  Direction change 
            hand_controller_state = HAND_CONT_BRAKE_POT;
            break;

        case    HAND_CONT_BRAKE_POT:
            if  (brake_effort < 0.9)    {
                brake_effort += 0.05;   //  ramp braking up to max over about one sec
                mode_set_both_motors    (REGENBRAKE, brake_effort);  //  Direction change 
                pc.printf   ("Brake effort %.2f\r\n", brake_effort);
            }
            else    {   //  once braking up to quite hard
                if  (pot_position < 0.02)   {   //  Driver has turned pot fully anticlock
                    hand_controller_state = HAND_CONT_STATE_BRAKING;
                }
            }
            break;

        case    HAND_CONT_STATE_INTO_BRAKING:
            brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
            if  (brake_effort < 0.0)
                brake_effort = 0.5;
            mode_set_both_motors    (REGENBRAKE, brake_effort);
            old_pot_position = pot_position;
            hand_controller_state = HAND_CONT_STATE_BRAKING;
            pc.printf   ("Brake\r\n");
            break;

        case    HAND_CONT_STATE_BRAKING:
            if  (pot_position > Pot_Brake_Range)    //  escape braking into drive
                hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
            else    {
                if  (worth_the_bother(pot_position, old_pot_position))  {
                    old_pot_position = pot_position;
                    brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
                    mode_set_both_motors    (REGENBRAKE, brake_effort);
//                    pc.printf   ("Brake %.2f\r\n", brake_effort);
                }
            }
            break;

        case    HAND_CONT_STATE_INTO_DRIVING:
            pc.printf   ("Drive\r\n");
            if  (direction_new == 1)
                mode_set_both_motors   (FORWARD, 0.0);  //  sets both motors
            else
                mode_set_both_motors   (REVERSE, 0.0);
            hand_controller_state = HAND_CONT_STATE_DRIVING;
            break;

        case    HAND_CONT_STATE_DRIVING:
            if  (pot_position < Pot_Brake_Range)    //  escape braking into drive
                hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
            else    {
                if  (worth_the_bother(pot_position, old_pot_position))  {
                    old_pot_position = pot_position;
                    drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range);
                    setVI  (drive_effort, drive_effort);   //  Wind volts and amps up and down together ???
                    pc.printf   ("Drive %.2f\r\n", drive_effort);
                }
            }
            break;

        default:
            pc.printf   ("Unhandled Hand Controller state %d\r\n", hand_controller_state);
            break;
    }       //  endof switch  (hand_controller_state) {
}

int main()
{
    int eighth_sec_count = 0;
    double  servo_angle = 0.0;  //  For testing servo outs

    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);
#ifdef  RC1IN
    RC_1_in.rise    (& RC_chan_1rise)  ;
    RC_1_in.fall    (& RC_chan_1fall)  ;
    RC_1_in.mode    (PullDown);
#endif
#ifdef  RC2IN
    RC_2_in.rise    (& RC_chan_2rise)  ;
    RC_2_in.fall    (& RC_chan_2fall)  ;
    RC_2_in.mode    (PullDown);
#endif
//    Servo1_i.mode   (PullUp); //  These never worked, December 2018 re-trying using PC_14 and 15
//    Servo2_i.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);

    //  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);
    setup_comms ();

    IAm = '0';
    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");
        eeprom_flag = false;
    } else   {      //  Found 24LC64 memory on I2C
        eeprom_flag = true;
        bool k;
        k = rd_24LC64   (0, mode_bytes, 32);
        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);
                pc.printf   ("EEROM error with %s\r\n", option_list[i].t);
                if  (i == ID)   {   //  need to force id to '0'
                    pc.printf   ("Bad board ID value %d, forcing to \'0\'\r\n");
                    mode_bytes[ID] = '0';
                }
//                err++;
            }
//            else
//                com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
        }
        rpm2mph = 0.0;
        if  (mode_bytes[WHEELGEAR] == 0)    //  avoid making div0 error
            mode_bytes[WHEELGEAR]++;
//        if  (err == 0)  {
        mode_good_flag = true;
        MotorA.direction_set    (mode_bytes[MOTADIR]);
        MotorB.direction_set    (mode_bytes[MOTBDIR]);
        IAm = mode_bytes[ID];
        rpm2mph = 60.0                                                          //  to Motor Revs per hour;
                  * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR])  //  Wheel revs per hour
                  * PI * ((double)mode_bytes[WHEELDIA] / 1000.0)                  //  metres per hour
                  * 39.37 / (1760.0 * 36.0);                                      //  miles per hour
//        }
        //  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);

//    prscfuck    (PWM_PRESECALER_DEFAULT);    //  Test fucking with fastpwm prescaler

//    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 !

//  December 2018   ** NEED TO PROVE SERVO OUT WORKS ** YES, DONE.
    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);
    dc_motor_kicker_timer.start   ();
    int old_hand_controller_direction = T5;
    if  (mode_bytes[COMM_SRC] == 3)  {      //  Read fwd/rev switch 'T5', PA15 on 401
        pc.printf   ("Pot control\r\n");
        if  (T5)
            mode_set_both_motors   (FORWARD, 0.0);  //  sets both motors
        else
            mode_set_both_motors   (REVERSE, 0.0);
    }

    pc.printf   ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]);
//    const   double  pwr = 1.5;SERVOIN_PWR_BENDER
//    for (double i = 0.0; i < 1.05; i+= 0.1)
//    pc.printf   ("%f ^ %f = %f\r\n", i, SERVOIN_PWR_BENDER, pow(i, SERVOIN_PWR_BENDER));

//    pc.printf   ("PortA=%lx\r\n", PortA);

    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_pc    ()  ;   //  Proceed beyond here once loop_timer ticker ISR has set loop_flag true
            command_line_interpreter_loco    ()  ;   //  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. WHEN LAST CHECKED this was about every 32ms

        RC_chan_1.validate_rx();
        RC_chan_2.validate_rx();

        switch  (mode_bytes[COMM_SRC])  {   //  Look to selected source of driving command, act on commands from wherever
            case    0:  //  Invalid
                break;
            case    1:  //  COM1    -   Primarily for testing, bypassed by command line interpreter
                break;
            case    2:  //  COM2    -   Primarily for testing, bypassed by command line interpreter
                break;
            case    3:  //  Put all hand controller input stuff here
                hand_control_state_machine  (3);
                break;  //  endof hand controller stuff
            case    4:  //  Servo1
                hand_control_state_machine  (4);
                break;
            case    5:  //  Servo2
                break;
            default:
                pc.printf   ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]);
                break;
        }   //  endof   switch  (mode_bytes[COMM_SRC])  {

        dc_motor_kicker_timer.reset   ();
        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  (MotorA.dc_motor)   {
            MotorA.raw_V_pwm    (1);
            MotorA.low_side_on  ();
        }
        if  (MotorB.dc_motor)   {
            MotorB.raw_V_pwm    (1);
            MotorB.low_side_on  ();
        }
        if  (MotorA.dc_motor || MotorB.dc_motor)    {
//            motors_restore.attach_us    (&restorer, ttime);
            motors_restore.attach_us    (&restorer, 25);
        }

        if  (flag_8Hz)  {   //  do slower stuff
            flag_8Hz    = false;
            LED = !LED;                   // Toggle LED on board, should be seen to fast flash
            if  (WatchDogEnable)    {
                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);
            }
            
            //  servo out test here     December 2018
            servo_angle += 0.01;
            if  (servo_angle > TWOPI)
                servo_angle -= TWOPI;
            Servo1 = ((sin(servo_angle)+1.0) / 2.0);
            Servo2 = ((cos(servo_angle)+1.0) / 2.0);
            //  Yep!    Both servo outs work lovely December 2018
            
        }   //  End of if(flag_8Hz)
    }       //  End of main programme loop
}