Revision 10:e40d8724268a, committed 2019-01-15

Comitter:

JonFreeman

Date:

Tue Jan 15 09:03:57 2019 +0000

Parent:

9:ac2412df01be

Child:

11:bfb73f083009

Commit message:

Buggered serial comms to TS controller

Changed in this revision

--- a/DualBLS.h	Sat Nov 10 17:08:21 2018 +0000
+++ b/DualBLS.h	Tue Jan 15 09:03:57 2019 +0000
@@ -19,7 +19,6 @@
 /*  End of Please Do Not Alter these */
 const   double      PI      = 4.0 * atan(1.0),
                     TWOPI   = 8.0 * atan(1.0);
-const   double      Pot_Brake_Range = 0.33;
 
 //enum    {MOTADIR, MOTBDIR, GANG, SVO1, SVO2, COMM_SRC, ID, WHEELDIA, MOTPIN, WHEELGEAR}  ;  //  Identical in TS and DualBLS
 enum    {MOTADIR, MOTBDIR, GANG, SVO1, SVO2, COMM_SRC, ID, WHEELDIA, MOTPIN, WHEELGEAR, BOGHUNWAT, FUT1, FUT2, FUT3, FUT4, FUT5}  ;  //  Identical in TS and DualBLS
@@ -50,3 +49,5 @@
 struct  single_bogie_options   {
     char    motoradir, motorbdir, gang, svo1, svo2, comm_src, id, wheeldia, motpin, wheelgear, spare;
 }   ;
+
+//const   double  SERVOIN_PWR_BENDER = 1.5;   //  Used to change servo_in stick at centre position to match pot approx 1/3 braking 2/3 driving

--- a/F401RE.h	Sat Nov 10 17:08:21 2018 +0000
+++ b/F401RE.h	Tue Jan 15 09:03:57 2019 +0000
@@ -1,41 +1,63 @@
-//  Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out
+//  Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - GOOD, works well with auto-tickle of high side drivers
+
+//  Jan 2019    Trying to add two Radio Control inputs on PC_14 and PC_15, previously connected to unused LF Xtal. 
+//              Problem - Appears to conflict with serial port used for comms with controller
+//              Earlier efforts to use 'Servo' ports as 'you choose' between I/O failed as pins not capable of use as 'InterruptIn'
+
+//                  Experiment disabling RC inputs to see if clearing serial conflict is possible
+
+//#define RC1IN
+//#define RC2IN
+
+
 
 //  Port A -> MotorA, Port B -> MotorB
 const   uint16_t
-AUL = 1 << 0,   //  Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - GOOD, works well with auto-tickle of high side drivers
-AVL = 1 << 6,   //  These are which port bits connect to which mosfet driver
-AWL = 1 << 4,
+//  This is where port bits get assigned to motor output phase switches.
+//  Phases are U, V and W.
+//  Each phase uses two bits, one for the low side switch, one for the high side switch.
+//MotorN_port_bits[] =  {UL, VL, WL, UH, VH, WH},   //  Order must be as shown - 3 low side switches U,V,W followed by 3 high side switches U,V,W
+MotorA_port_bits[] =    {0,  6,  4,  1,  7,  8},    //  List of port A bits used to drive motor A UL, VL, WL, UH, VH, WH
+MotorB_port_bits[] =    {0,  1,  2, 10, 12, 13},    //  List of port B bits used to drive motor B UL, VL, WL, UH, VH, WH
+//  Using port bit info in the two lines above, the compiler sorts all this into creation of lookup table
+//  to provide correct energisation sequencing as motors rotate.
+//  You need concern yourself no further about any of this.
+
 
-AUH = 1 << 1,
-AVH = 1 << 7,
-AWH = 1 << 8,
+AUL = (1 << MotorA_port_bits[0]),
+AVL = (1 << MotorA_port_bits[1]),   //  These are which port bits connect to which mosfet driver
+AWL = (1 << MotorA_port_bits[2]),
 
-AUV =   AUH | AVL,  //  Each of 6 possible output energisations made up of one hi and one low
-AVU =   AVH | AUL,
-AUW =   AUH | AWL,
-AWU =   AWH | AUL,
-AVW =   AVH | AWL,
-AWV =   AWH | AVL,
+AUH = (1 << MotorA_port_bits[3]),
+AVH = (1 << MotorA_port_bits[4]),
+AWH = (1 << MotorA_port_bits[5]),
+
+AUHVL =   AUH | AVL,  //  Each of 6 possible output energisations made up of one hi and one low
+AVHUL =   AVH | AUL,
+AUHWL =   AUH | AWL,
+AWHUL =   AWH | AUL,
+AVHWL =   AVH | AWL,
+AWHVL =   AWH | AVL,
 
 KEEP_L_MASK_A   = AUL | AVL | AWL,
 KEEP_H_MASK_A   = AUH | AVH | AWH,
 
 BRA = AUL | AVL | AWL,  //  All low side switches on (and all high side off) for braking
 
-BUL = 1 << 0,   //  Likewise for MotorB but different port bits on different port
-BVL = 1 << 1,
-BWL = 1 << 2,
+BUL = (1 << MotorB_port_bits[0]),   //  Likewise for MotorB but different port bits on different port
+BVL = (1 << MotorB_port_bits[1]),
+BWL = (1 << MotorB_port_bits[2]),
 
-BUH = 1 << 10,
-BVH = 1 << 12,
-BWH = 1 << 13,
+BUH = (1 << MotorB_port_bits[3]),
+BVH = (1 << MotorB_port_bits[4]),
+BWH = (1 << MotorB_port_bits[5]),
 
-BUV =   BUH | BVL,
-BVU =   BVH | BUL,
-BUW =   BUH | BWL,
-BWU =   BWH | BUL,
-BVW =   BVH | BWL,
-BWV =   BWH | BVL,
+BUHVL =   BUH | BVL,
+BVHUL =   BVH | BUL,
+BUHWL =   BUH | BWL,
+BWHUL =   BWH | BUL,
+BVHWL =   BVH | BWL,
+BWHVL =   BWH | BVL,
 
 KEEP_L_MASK_B   = BUL | BVL | BWL,
 KEEP_H_MASK_B   = BUH | BVH | BWH,
@@ -54,7 +76,14 @@
 InterruptIn   Temperature_pin   (PC_13);//  Pin 2   June 2018 - taken for temperature sensor - hard wired to T1 due to wrong thought T1 could be InterruptIn
 
 
-//  Pin 3   PC14-OSC32_IN   NET O32I
+#ifdef  RC1IN
+InterruptIn     RC_1_in (PC_14);    //New Dec 2018 tryiing to find pins to use for servo in
+#endif
+#ifdef  RC2IN
+InterruptIn     RC_2_in (PC_15);    //Yes, PC_14 and PC_15 do work
+#endif
+
+//  Pin 3   PC14-OSC32_IN   NET O32I    Xtal chucked off these pins, now needed for RC inputs
 //  Pin 4   PC15-OSC32_OUT  NET O32O
 //  Pin 5   PH0-OSC_IN      NET PH1
 //  Pin 6   PH1-OSC_OUT     NET PH1
@@ -133,9 +162,12 @@
 //  Pin 60  BOOT0
 
 //  Servo pins, 2 off. Configured as Input to read radio control receiver
+//      ** Update December 2018 **
+//  These pins can not be used as InterruptIn.
+//  Can be used as outputs by 'Servo'
 //  If used as servo output, code gives pin to 'Servo' - seems to work
-InterruptIn Servo1_i    (PB_8); //  Pin 61  to read output from rc rx
-InterruptIn Servo2_i    (PB_9); //  Pin 62  to read output from rc rx
+//InterruptIn Servo1_i    (PB_8); //  Pin 61  to read output from rc rx
+//InterruptIn Servo2_i    (PB_9); //  Pin 62  to read output from rc rx
 //  *** NOTE *** Above InterruptIn Servo using PB pins seems not to work, probably due to other Port B pins used as PortOut (try PortInOut?)
 //  Nov 2018 - Yet to try using PC14, PC15, free now as 32k768 xtal not fitted
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/brushless_motor.cpp	Tue Jan 15 09:03:57 2019 +0000
@@ -0,0 +1,223 @@
+//  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_motor::brushless_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    brushless_motor::motor_is_brushless   ()
+{
+    /*    int x = read_Halls  ();
+        if  (x < 1 || x > 6)
+            return  false;
+        return  true;
+        */
+    return  !dc_motor;
+}
+
+/**
+void    brushless_motor::direction_set   (int dir)  {
+Used to set direction according to mode data from eeprom
+*/
+void    brushless_motor::direction_set   (int dir)
+{
+    if  (dir != 0)
+        dir = FORWARD | REVERSE;  //  bits used in eor
+    direction = dir;
+}
+
+int     brushless_motor::read_Halls  ()
+{
+    int x = 0;
+    if  (*Hall1 != 0)    x |= 1;
+    if  (*Hall2 != 0)    x |= 2;
+    if  (*Hall3 != 0)    x |= 4;
+    return  x;
+}
+
+void    brushless_motor::high_side_off   ()
+{
+    uint16_t    p = *Motor_Port;
+    p &= lut[32];   //  KEEP_L_MASK_A or B
+    *Motor_Port = p;
+}
+
+void    brushless_motor::low_side_on   ()
+{
+//    uint16_t    p = *Motor_Port;
+//    p &= lut[31];   //  KEEP_L_MASK_A or B
+    *Motor_Port = lut[31];
+}
+
+void    brushless_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    brushless_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    brushless_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    brushless_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    brushless_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    brushless_motor::is_moving ()
+{
+    return  moving_flag;
+}
+
+/**
+bool    brushless_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    brushless_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    brushless_motor::Hall_change  ()
+{
+    const   int32_t delta_theta_lut[] = {   //  Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
+        0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 0
+        0, 0, 0,-1, 0, 1, 0, 0,     //  Previous Hindex was 1
+        0, 0, 0, 1, 0, 0,-1, 0,     //  Previous Hindex was 2
+        0, 1,-1, 0, 0, 0, 0, 0,     //  Previous Hindex was 3
+        0, 0, 0, 0, 0,-1, 1, 0,     //  Previous Hindex was 4
+        0,-1, 0, 0, 1, 0, 0, 0,     //  Previous Hindex was 5
+        0, 0, 1, 0,-1, 0, 0, 0,     //  Previous Hindex was 6
+        0, 0, 0, 0, 0, 0, 0, 0,     //  Previous Hindex was 7
+    }  ;
+    int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
+    if  (delta_theta == 0)
+        encoder_error_cnt++;
+    else
+        angle_cnt += delta_theta;
+    *Motor_Port = lut[inner_mode | Hindex[0]];  //  changed mode to inner_mode 27/04/18
+    Hall_total++;
+    Hindex[1] = Hindex[0];
+}
+
+void    brushless_motor::motor_set  ()
+{
+    Hindex[0]  = read_Halls    ();
+    *Motor_Port = lut[inner_mode | Hindex[0]];
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/brushless_motor.h	Tue Jan 15 09:03:57 2019 +0000
@@ -0,0 +1,73 @@
+/*  mbed brushless_motor library
+    Jon Freeman
+    December 2018
+*/
+#ifndef MBED_BRUSHLESSMOTOR_H
+#define MBED_BRUSHLESSMOTOR_H
+
+#include "mbed.h"
+
+class   brushless_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;
+    uint16_t    ttabl[34];
+    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;
+    brushless_motor   ()  {}  ;   //  Default constructor
+/**
+    brushless_motor   (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **)  ;
+    
+    PortOut * is &MotA where MotA has been declared as below where PORT_A_MASK identifies which 6 bits of 16 bit port are used to energise motor
+    PortOut MotA    (PortA, PORT_A_MASK);   //  Activate output ports to motor drivers
+    
+    FastPWM * are e.g. &A_MAX_V_PWM, &A_MAX_I_PWM
+    
+    const uint16_t * is e.g. A_tabl defining which port bits set to which level 
+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,  BRA,BRA,BRA,BRA,BRA,BRA,BRA,   //  Regenerative Braking
+    KEEP_L_MASK_A, KEEP_H_MASK_A   //  [32 and 33]
+}   ;
+
+InterruptIn ** is e.g. AHarr pointer to array of addresses of InterruptIns connected to Hall sensors
+InterruptIn * AHarr[] = {
+    &MAH1,
+    &MAH2,
+    &MAH3
+}   ;
+
+*/
+    brushless_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];
+
+#endif

--- a/cli_BLS_nortos.cpp	Sat Nov 10 17:08:21 2018 +0000
+++ b/cli_BLS_nortos.cpp	Tue Jan 15 09:03:57 2019 +0000
@@ -289,6 +289,12 @@
         a.com->printf ("who%c\r\n", a.target_unit);
 }
 
+extern  void    rcin_report ()  ;
+void    rcin_pccmd (struct parameters & a)
+{
+    rcin_report ();
+}
+
 struct kb_command  {
     const char * cmd_word;         //  points to text e.g. "menu"
     const char * explan;
@@ -353,6 +359,7 @@
     {"kd", "kick the dog, reloads WatchDog", kd_cmd},
     {"wden", "enable watchdog if modes allow", wden_pccmd},
     {"wddi", "disable watchdog always", wddi_pccmd},
+    {"rcin", "Report Radio Control Input stuff", rcin_pccmd},
     {"rpm", "read motor pair speeds", rpm_cmd},
     {"mph", "read loco speed miles per hour", mph_cmd},
     {"rvi", "read most recent values sent to pwms", rvi_cmd},
@@ -381,7 +388,7 @@
 void    menucmd (struct parameters & a)
 {
     if  (a.respond) {
-        a.com->printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
+        a.com->printf("\r\n\nDual BLDC ESC type STM3 2018\r\nAt menucmd function - listing commands:-\r\n");
         for(int i = 0; i < a.numof_menu_items; i++)
             a.com->printf("[%s]\t\t%s\r\n", a.command_list[i].cmd_word, a.command_list[i].explan);
         a.com->printf("End of List of Commands\r\n");

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

--- a/sud.cpp	Sat Nov 10 17:08:21 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,117 +0,0 @@
-//sudoku
-#include "mbed.h"
-#include "BufferedSerial.h"
-#include "DualBLS.h"
-extern  BufferedSerial com2, pc;
-using namespace std;
-
-struct  cell    {
-    uint16_t    confirmed;
-    uint16_t    dominoes;
-    uint16_t    must_be;
-}    matrix[82];
-/*
-Cell Map
-
-0  1  2    3  4  5    6  7  8
-9  10 11   12 13 14   15 16 17
-18 19 20   21 22 23   24 25 26
-
-27 28 29   30 31 32   33 34 35
-36 37 38   39 40 41   42 43 44
-45 46 47   48 49 50   51 52 53
-
-54 55 56   57 58 59   60 61 62
-63 64 65   66 67 68   69 70 71
-72 73 74   75 76 77   78 79 80
-
-*/
-
-static  const   uint16_t
-    box1[] = { 0,  1,  2,  9, 10, 11, 18, 19, 20},
-    box2[] = { 3,  4,  5, 12, 13, 14, 21, 22, 23},
-    box3[] = { 6,  7,  8, 15, 16, 17, 24, 25, 26},
-    box4[] = {27, 28, 29, 36, 37, 38, 45, 46, 47},
-    box5[] = {30, 31, 32, 39, 40, 41, 48, 49, 50},
-    box6[] = {33, 34, 35, 42, 43, 44, 51, 52, 53},
-    box7[] = {54, 55, 56, 63, 64, 65, 72, 73, 74},
-    box8[] = {57, 58, 59, 66, 67, 68, 75, 76, 77},
-    box9[] = {60, 61, 62, 69, 70, 71, 78, 79, 80},
-
-    col1[] = { 0,  9, 18, 27, 36, 45, 54, 63, 72},
-    col2[] = { 1, 10, 19, 28, 37, 46, 55, 64, 73},
-    col3[] = { 2, 11, 20, 29, 38, 47, 56, 65, 74},
-    col4[] = { 3, 12, 21, 30, 39, 48, 57, 66, 75},
-    col5[] = { 4, 13, 22, 31, 40, 49, 58, 67, 76},
-    col6[] = { 5, 14, 23, 32, 41, 50, 59, 68, 77},
-    col7[] = { 6, 15, 24, 33, 42, 51, 60, 69, 78},
-    col8[] = { 7, 16, 25, 34, 43, 52, 61, 70, 79},
-    col9[] = { 8, 17, 26, 35, 44, 53, 62, 71, 80},
-
-    row1[] = { 0,  1,  2,  3,  4,  5,  6,  7,  8},
-    row2[] = { 9, 10, 11, 12, 13, 14, 15, 16, 17},
-    row3[] = {18, 19, 20, 21, 22, 23, 24, 25, 26},
-    row4[] = {27, 28, 29, 30, 31, 32, 33, 34, 35},
-    row5[] = {36, 37, 38, 39, 40, 41, 42, 43, 44},
-    row6[] = {45, 46, 47, 48, 49, 50, 51, 52, 53},
-    row7[] = {54, 55, 56, 57, 58, 59, 60, 61, 62},
-    row8[] = {63, 64, 65, 66, 67, 68, 69, 70, 71},
-    row9[] = {72, 73, 74, 75, 76, 77, 78, 79, 80},
-
-    *box[] = {box1, box2, box3, box4, box5, box6, box7, box8, box9},
-    *row[] = {row1, row2, row3, row4, row5, row6, row7, row8, row9},
-    *col[] = {col1, col2, col3, col4, col5, col6, col7, col8, col9},
-    
-    examp1[] =  {5,0,0,0,9,0,7,0,0, //  Hard puzzle 2,706,560,598
-                 0,0,8,2,0,0,5,3,0,
-                 0,6,0,8,0,0,0,0,0,
-                 0,0,0,3,7,9,0,0,2,
-                 0,2,0,0,0,0,0,9,0,
-                 3,0,0,5,8,2,0,0,0,
-                 0,0,0,0,0,6,0,8,0,
-                 0,7,4,0,0,8,3,0,0,
-                 0,0,6,0,1,0,0,0,7};
-
-void    clear_matrix    ()  {
-    for (int i = 0; i < 81; i++)    {
-        matrix[i].confirmed = matrix[i].must_be = 0;
-        matrix[i].dominoes = 0x3fe;
-    }
-}
-void    load_matrix (const uint16_t * p)  {
-    for (int i = 0; i < 81; i++)
-        matrix[i].confirmed = p[i];
-}
-bool    ranged  (uint16_t a)    {
-    if  ((a > 0) && (a < 10))
-        return  true;
-    return  false;
-}
-void    knock_down_dominoes (const uint16_t * brc)  {   //  param points to list of 9 matrix offsets. May refer to row, col, or box
-    int d, mask;
-    for (int i = 0; i < 9; i++)    {
-        d = matrix[brc[i]].confirmed;
-        pc.printf   ("matrix cell %d\r\n", brc[i]);
-        if  (ranged(d))  {
-            mask = (~(1 << d)) & 0x3fe;
-            for (int j = 0; j < 9; j++) {
-                if  (i == j)
-                    matrix[brc[j]].dominoes |= 1 << d;
-                else
-                    matrix[brc[j]].dominoes &= mask;
-            }
-        }
-    }
-}
-void    update_matrix   ()  {
-    for (int i = 0; i < 9; i++) {
-        knock_down_dominoes (row[i]);
-        knock_down_dominoes (col[i]);
-        knock_down_dominoes (box[i]);
-    }
-}
-void    sudo  ()  {
-    clear_matrix    ();
-    load_matrix     (examp1);
-    pc.printf   ("Row 3 col 7 = %d\r\n", matrix[row[3][7]].dominoes);
-}