Diff: main.cpp

Revision:

0:435bf84ce48a

Child:

1:0fabe6fdb55b

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