Diff: main.cpp

Revision:

10:e40d8724268a

Parent:

9:ac2412df01be

Child:

11:bfb73f083009

--- a/main.cpp	Sat Nov 10 17:08:21 2018 +0000
+++ b/main.cpp	Tue Jan 15 09:03:57 2019 +0000
@@ -1,3 +1,4 @@
+//  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"
@@ -5,10 +6,13 @@
 #include "BufferedSerial.h"
 #include "FastPWM.h"
 #include "Servo.h"
+#include "brushless_motor.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 as InterruptIn on T1 (ports A and B I think) not workable
+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
 */
 
 
@@ -35,7 +39,7 @@
 #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"
+#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
@@ -106,47 +110,74 @@
 }
 
 
+/**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 clock_old, clock_new;
-    int32_t pulse_width_us, period_us;
+    int32_t pulse_width_us, period_us, pulse_count;
 public:
-    RControl_In   ()   {
-        pulse_width_us = period_us = 0;
-        com2.printf ("Setting up Radio_Congtrol_In\r\n");
+    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    ()  ;
+    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 ()
-{
-    if  ((clock() - clock_new) > 4)
+{   //  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;
 }
 
-void    RControl_In::rise    ()     //  These may not work as use of PortB as port may bugger InterruptIn use
-{
-    t.stop   ();
+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 ();
@@ -154,14 +185,28 @@
 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;
+    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];
+//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;
 /*
@@ -182,8 +227,8 @@
 */
 const   uint16_t    A_tabl[] = {  //  Origial table
     0,  0,  0,  0,  0,  0,  0,  0,  //  Handbrake
-    0,  AWV,AVU,AWU,AUW,AUV,AVW,AUW,  //  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,AWU,  //  Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,    //  JP, FR, SG, PWM = 1 1 0 1   Reverse1
+    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]
 }   ;
@@ -194,8 +239,8 @@
 }   ;
 const   uint16_t    B_tabl[] = {
     0,  0,  0,  0,  0,  0,  0,  0,  //  Handbrake
-    0,  BWV,BVU,BWU,BUW,BUV,BVW,BUW,  //  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,BWU,  //  Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,    //  JP, FR, SG, PWM = 1 1 0 1   Reverse1
+    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
 }   ;
@@ -205,256 +250,9 @@
     &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:
-    bool    dc_motor;
-    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
-    bool    motor_is_brushless  ();
-    void    high_side_off   ()  ;
-    void    low_side_on     ()  ;
-    void    raw_V_pwm   (int);
-}   ;   //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;
-    int x = read_Halls  ();
-    if  (x == 7)
-        dc_motor = true;
-    else
-        dc_motor = false;
-}
-
-bool    motor::motor_is_brushless   ()
-{
-    /*    int x = read_Halls  ();
-        if  (x < 1 || x > 6)
-            return  false;
-        return  true;
-        */
-    return  !dc_motor;
-}
-
-/**
-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::low_side_on   ()
-{
-//    uint16_t    p = *Motor_Port;
-//    p &= lut[31];   //  KEEP_L_MASK_A or B
-    *Motor_Port = lut[31];
-}
-
-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::raw_V_pwm    (int    v)
-{
-    if  (v < 1) v = 1;
-    if  (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
-    maxV->pulsewidth_ticks  (v);
-}
-
-void    motor::set_V_limit (double p)  //  Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
-{
-    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  (dc_motor)
-        return  0;
-    if  (ppstmp == Hall_total)  {
-//    if  (dc_motor || 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];
-}
+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
@@ -499,12 +297,6 @@
 
 //  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
@@ -606,9 +398,40 @@
     }
 }
 
+/** 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
+*   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  ()
@@ -689,16 +512,6 @@
 }
 
 //int ttime = 3;    //  resettable via cli 'stt', used to determine suitable force low on period for driving dc motor
-bool    worth_the_bother    (double a, double b)    {
-    double c = a - b;
-    if  (c < 0.0)
-        c = 0.0 - c;
-    if  (c > 0.02)  {
-//        pc.printf   ("%.3f\r\n", c);
-        return  true;
-    }
-    return  false;
-}
 
 void    prscfuck    (int v) {
 /*
@@ -720,54 +533,110 @@
     pc.printf   ("Attempt setting prescaler %d returned %d\r\n", v, w);
 }
 
-enum    {   HAND_CONT_STATE_BEGIN,
-            HAND_CONT_STATE_POWER_CYCLE_TO_EXIT, 
-            HAND_CONT_STATE_INTO_BRAKING,
-            HAND_CONT_STATE_INTO_DRIVING,
-            HAND_CONT_STATE_BRAKING,
-            HAND_CONT_STATE_DRIVING
-            }  ;
+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());
+}
 
-void    hand_control_state_machine  ()  {
-    static  int new_hand_controller_state = HAND_CONT_STATE_BEGIN;
-//    static  int old_hand_controller_state = HAND_CONT_STATE_BEGIN;
-    static  int old_hand_controller_direction = T5, t = 0;              //  Nov 2018 confirms Rob and Quentin obs, direction read at powerup
+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;
-    if  (T5 != old_hand_controller_direction)   {   //  1 Forward, 0 Reverse
-        pc.printf   ("Direction change! Power off then on again to resume\r\n");
-        mode_set_both_motors    (REGENBRAKE, 1.0);
-//        old_hand_controller_state = new_hand_controller_state;
-        new_hand_controller_state = HAND_CONT_STATE_POWER_CYCLE_TO_EXIT;
+    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;
     }
-    pot_position = Read_DriverPot   ();     //  gets to here on first pass before pot has been read
-    switch  (new_hand_controller_state) {
-        case    HAND_CONT_STATE_BEGIN:
-            pot_position = Read_DriverPot   ();
-            if  (t++ > 2 && pot_position < 0.02)    {
-                pc.printf   ("In BEGIN, pot %.2f\r\n", pot_position);
-                new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+
+    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_POWER_CYCLE_TO_EXIT:
-            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;
-            new_hand_controller_state = HAND_CONT_STATE_BRAKING;
+            hand_controller_state = HAND_CONT_STATE_BRAKING;
             pc.printf   ("Brake\r\n");
             break;
-        case    HAND_CONT_STATE_INTO_DRIVING:
-            new_hand_controller_state = HAND_CONT_STATE_DRIVING;
-            pc.printf   ("Drive\r\n");
-            if  (T5)
-                mode_set_both_motors   (FORWARD, 0.0);  //  sets both motors
-            else
-                mode_set_both_motors   (REVERSE, 0.0);
-            break;
+
         case    HAND_CONT_STATE_BRAKING:
             if  (pot_position > Pot_Brake_Range)    //  escape braking into drive
-                new_hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
+                hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
             else    {
                 if  (worth_the_bother(pot_position, old_pot_position))  {
                     old_pot_position = pot_position;
@@ -777,9 +646,19 @@
                 }
             }
             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
-                new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+                hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
             else    {
                 if  (worth_the_bother(pot_position, old_pot_position))  {
                     old_pot_position = pot_position;
@@ -789,7 +668,9 @@
                 }
             }
             break;
+
         default:
+            pc.printf   ("Unhandled Hand Controller state %d\r\n", hand_controller_state);
             break;
     }       //  endof switch  (hand_controller_state) {
 }
@@ -797,6 +678,7 @@
 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;
@@ -805,6 +687,18 @@
 
     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);
@@ -826,8 +720,6 @@
     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
@@ -898,15 +790,17 @@
 
 //    prscfuck    (PWM_PRESECALER_DEFAULT);    //  Test fucking with fastpwm prescaler
 
-    Servos[0] = Servos[1] = NULL;
+//    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;
+//    Servos[0] = & Servo1;
     Servo   Servo2  (PB_9)  ;
-    Servos[1] = & Servo2;
+//    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
@@ -945,6 +839,11 @@
     }
 
     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
@@ -952,7 +851,11 @@
             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
+        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;
@@ -961,9 +864,10 @@
             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  ();
+                hand_control_state_machine  (3);
                 break;  //  endof hand controller stuff
             case    4:  //  Servo1
+                hand_control_state_machine  (4);
                 break;
             case    5:  //  Servo2
                 break;
@@ -1015,6 +919,15 @@
                                 }*/
 //                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
 }