Diff: main.cpp

Revision:

8:93203f473f6e

Parent:

7:6deaeace9a3e

Child:

9:ac2412df01be

--- a/main.cpp	Sun Jun 17 06:59:37 2018 +0000
+++ b/main.cpp	Sat Aug 18 12:51:35 2018 +0000
@@ -1,4 +1,6 @@
 #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"
@@ -28,6 +30,7 @@
 
 
 */
+
 #if defined (TARGET_NUCLEO_F401RE)  //  CPU in 64 pin LQFP
 #include    "F401RE.h"
 #endif
@@ -37,6 +40,7 @@
 /*  Global variable declarations */
 volatile    uint32_t    fast_sys_timer      = 0;    //  gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
 int         WatchDog    = WATCHDOG_RELOAD + 80; //  Allow extra few seconds at powerup
+bool        WatchDogEnable  = false;    //  Must recieve explicit instruction from pc or controller to enable
 uint32_t    volt_reading        = 0,    //  Global updated by interrupt driven read of Battery Volts
             driverpot_reading   = 0,    //  Global updated by interrupt driven read of Drivers Pot
             sys_timer           = 0,    //  gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
@@ -51,17 +55,20 @@
 
 uint32_t    temp_sensor_count = 0,  //  incremented by every rising edge from LMT01
             last_temp_count = 0;  //  global updated approx every 100ms after each LMT01 conversion completes
-//    struct  single_bogie_options    bogie;    
-    double  rpm2mph = 0.0;  //  gets calculated from eeprom mode entries if present
+//    struct  single_bogie_options    bogie;
+double  rpm2mph = 0.0;  //  gets calculated from eeprom mode entries if present
 /*  End of Global variable declarations */
 
 Ticker  tick_vread;     //  Device to cause periodic interrupts, used to time voltage readings etc
 Ticker  loop_timer;     //  Device to cause periodic interrupts, used to sync iterations of main programme loop
 Ticker  temperature_find_ticker;
 Timer   temperature_timer;
+Timer   dc_motor_kicker_timer;
+Timeout motors_restore;
 
 //  Interrupt Service Routines
-void    ISR_temperature_find_ticker ()  {   //  every 960 us, i.e. slightly faster than once per milli sec
+void    ISR_temperature_find_ticker ()      //  every 960 us, i.e. slightly faster than once per milli sec
+{
     static  bool    readflag = false;
     int t = temperature_timer.read_ms   ();
     if  ((t == 5) && (!readflag))    {
@@ -100,7 +107,8 @@
 
 
 class   RControl_In
-{  //  Read servo style pwm input
+{
+    //  Read servo style pwm input
     Timer   t;
     int32_t clock_old, clock_new;
     int32_t pulse_width_us, period_us;
@@ -117,11 +125,13 @@
     bool    rx_active;
 }   ;
 
-uint32_t    RControl_In::pulsewidth   ()  {
+uint32_t    RControl_In::pulsewidth   ()
+{
     return  pulse_width_us;
 }
 
-uint32_t    RControl_In::period   ()  {
+uint32_t    RControl_In::period   ()
+{
     return  period_us;
 }
 
@@ -162,12 +172,19 @@
 4   1   0   0   0   1   1  -___---___-- Hall3
 
     UV  WV  WU  VU  VW  UW  OUTPUT SEQUENCE
+
+8th July 2018
+Added drive to DC brushed motors.
+Connect U and W to dc motor, leave V open.
+
+Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors
+
 */
 const   uint16_t    A_tabl[] = {  //  Origial table
     0,  0,  0,  0,  0,  0,  0,  0,  //  Handbrake
-    0,  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
+    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 * AHarr[] = {
@@ -177,9 +194,9 @@
 }   ;
 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
+    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,  BRB,BRB,BRB,BRB,BRB,BRB,BRB,   //  Regenerative Braking
     KEEP_L_MASK_B, KEEP_H_MASK_B
 }   ;
 InterruptIn * BHarr[] = {
@@ -198,6 +215,7 @@
     PortOut * Motor_Port;
     InterruptIn * Hall1, * Hall2, * Hall3;
 public:
+    bool    dc_motor;
     struct  currents    {
         uint32_t    max, min, ave;
     }  I;
@@ -218,7 +236,10 @@
     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);
@@ -227,7 +248,8 @@
 //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
+{
+    //  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
@@ -258,19 +280,36 @@
     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)  {
+void    motor::direction_set   (int dir)
+{
     if  (dir != 0)
         dir = FORWARD | REVERSE;  //  bits used in eor
     direction = dir;
 }
 
-int     motor::read_Halls  ()  {
+int     motor::read_Halls  ()
+{
     int x = 0;
     if  (*Hall1 != 0)    x |= 1;
     if  (*Hall2 != 0)    x |= 2;
@@ -278,12 +317,20 @@
     return  x;
 }
 
-void    motor::high_side_off   ()  {
+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
@@ -301,7 +348,15 @@
     I.ave /= CURRENT_SAMPLES_AVERAGED;
 }
 
-void    motor::set_V_limit (double p)  //  Sets max motor voltage
+
+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;
@@ -330,12 +385,15 @@
 }
 
 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
+{
+    //  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    {
+    } else    {
         moving_flag  = true;
         ppstmp = Hall_total;
     }
@@ -378,17 +436,16 @@
 
 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
-            }  ;
+    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++;
@@ -400,7 +457,13 @@
 }
 
 
-void    temp_sensor_isr ()  {   //  got rising edge from LMT01. ALMOST CERTAIN this misses some
+void    report_motor_types  ()      //  not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
+{
+    pc.printf   ("Mot A is %s, Mot B is %s\r\n", MotorA.motor_is_brushless() ? "Brushless":"DC", MotorB.motor_is_brushless() ? "Brushless":"DC");
+}
+
+void    temp_sensor_isr ()      //  got rising edge from LMT01. ALMOST CERTAIN this misses some
+{
     int t = temperature_timer.read_us   (); //  Must be being overrun by something, most likely culprit A-D reading ?
     temperature_timer.reset ();
     temp_sensor_count++;
@@ -409,11 +472,12 @@
 //    T2 = !T2;   //  scope hanger
 }
 
+
 void    MAH_isr    ()
 {
     uint32_t x = 0;
     MotorA.high_side_off    ();
-    T3 = !T3;
+//    T3 = !T3;
     if  (MAH1 != 0) x |= 1;
     if  (MAH2 != 0) x |= 2;
     if  (MAH3 != 0) x |= 4;
@@ -441,32 +505,38 @@
     *Motor_Port = lut[inner_mode | Hindex[0]];
 }
 
-void    setVI   (double v, double i)  {
+void    setVI   (double v, double i)
+{
     MotorA.set_V_limit  (v);  //  Sets max motor voltage
     MotorA.set_I_limit  (i);  //  Sets max motor current
     MotorB.set_V_limit  (v);
     MotorB.set_I_limit  (i);
 }
-void    setV    (double v)  {
+void    setV    (double v)
+{
     MotorA.set_V_limit  (v);
     MotorB.set_V_limit  (v);
 }
-void    setI    (double i)  {
+void    setI    (double i)
+{
     MotorA.set_I_limit  (i);
     MotorB.set_I_limit  (i);
 }
 
-void    read_RPM    (uint32_t * dest)  {
+void    read_RPM    (uint32_t * dest)
+{
     dest[0] = MotorA.RPM;
     dest[1] = MotorB.RPM;
 }
 
-void    read_PPS    (uint32_t * dest)  {
+void    read_PPS    (uint32_t * dest)
+{
     dest[0] = MotorA.PPS;
     dest[1] = MotorB.PPS;
 }
 
-void    read_last_VI    (double * d)  {   //  only for test from cli
+void    read_last_VI    (double * d)      //  only for test from cli
+{
     d[0]    = MotorA.last_V;
     d[1]    = MotorA.last_I;
     d[2]    = MotorB.last_V;
@@ -481,13 +551,22 @@
 void    AtoD_reader ()  //  Call to here every VOLTAGE_READ_INTERVAL_US    = 50 once loop responds to flag set in isr
 {
     static uint32_t i = 0, tab_ptr = 0;
-
+//    if  (MotorA.dc_motor)   {
+//        MotorA.low_side_on  ();
+//    }
+//    else    {
     if  (MotorA.tickleon)
         MotorA.high_side_off    ();
+//    }
+//    if  (MotorB.dc_motor)   {
+//        MotorB.low_side_on  ();
+//    }
+//    else    {
     if  (MotorB.tickleon)
         MotorB.high_side_off    ();
+//    }
     if  (AtoD_Semaphore > 20)   {
-        pc.printf   ("WARNING - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore);
+        pc.printf   ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
         AtoD_Semaphore = 20;
     }
     while   (AtoD_Semaphore > 0)    {
@@ -516,10 +595,12 @@
             i = 0;
     }   //  end of while (AtoD_Semaphore > 0)    {
     if  (MotorA.tickleon)   {
+//    if  (MotorA.dc_motor || MotorA.tickleon)   {
         MotorA.tickleon--;
         MotorA.motor_set    (); //  Reactivate any high side switches turned off above
     }
     if  (MotorB.tickleon)   {
+//    if  (MotorB.dc_motor || MotorB.tickleon)   {
         MotorB.tickleon--;
         MotorB.motor_set    ();
     }
@@ -540,13 +621,15 @@
     return  ((double) volt_reading) / 951.0;    //  divisor fiddled to make voltage reading correct !
 }
 
-void    read_supply_vi   (double * val)  {  //  called from cli
+void    read_supply_vi   (double * val)     //  called from cli
+{
     val[0] = MotorA.I.ave;
     val[1] = MotorB.I.ave;
     val[2] = Read_BatteryVolts  ();
 }
 
-void    mode_set   (int mode, double val)  {   //  called from cli to set fw, re, rb, hb
+void    mode_set_both_motors   (int mode, double val)      //  called from cli to set fw, re, rb, hb
+{
     MotorA.set_mode (mode);
     MotorB.set_mode (mode);
     if  (mode == REGENBRAKE)    {
@@ -577,17 +660,140 @@
             last;
 }   ;
 */
-int I_Am    ()  {   //  Returns boards id number as ASCII char
+int I_Am    ()      //  Returns boards id number as ASCII char
+{
     return  IAm;
 }
 
-double  mph (int    rpm)    {
+double  mph (int    rpm)
+{
     if  (mode_good_flag)    {
         return  rpm2mph * (double) rpm;
     }
     return  -1.0;
 }
 
+void    restorer    ()
+{
+    motors_restore.detach   ();
+    if  (MotorA.dc_motor)   {
+        MotorA.set_V_limit  (MotorA.last_V);
+        MotorA.set_I_limit  (MotorA.last_I);
+        MotorA.motor_set    ();
+    }
+    if  (MotorB.dc_motor)   {
+        MotorB.set_V_limit  (MotorB.last_V);
+        MotorB.set_I_limit  (MotorB.last_I);
+        MotorB.motor_set    ();
+    }
+}
+
+//int ttime = 3;    //  resettable via cli 'stt', used to determine suitable force low on period for driving dc motor
+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) {
+/*
+int prescaler   (   int     value )     
+
+Set the PWM prescaler.
+
+The period of all PWM pins on the same PWM unit have to be reset after using this!
+
+Parameters:
+    value   - The required prescaler. Special values: 0 = lock current prescaler, -1 = use dynamic prescaler
+    return  - The prescaler which was set (can differ from requested prescaler if not possible)
+
+Definition at line 82 of file FastPWM_common.cpp.
+*/
+    if  (v < 1)
+        v = 1;
+    int w = A_MAX_V_PWM.prescaler   (v);
+    pc.printf   ("Attempt setting prescaler %d returned %d\r\n", v, w);
+}
+
+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    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;
+    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;
+    }
+    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;
+            }
+            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;
+            mode_set_both_motors    (REGENBRAKE, brake_effort);
+            old_pot_position = pot_position;
+            new_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;
+            else    {
+                if  (worth_the_bother(pot_position, old_pot_position))  {
+                    old_pot_position = pot_position;
+                    brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
+                    mode_set_both_motors    (REGENBRAKE, brake_effort);
+//                    pc.printf   ("Brake %.2f\r\n", brake_effort);
+                }
+            }
+            break;
+        case    HAND_CONT_STATE_DRIVING:
+            if  (pot_position < Pot_Brake_Range)    //  escape braking into drive
+                new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+            else    {
+                if  (worth_the_bother(pot_position, old_pot_position))  {
+                    old_pot_position = pot_position;
+                    drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range);
+                    setVI  (drive_effort, drive_effort);   //  Wind volts and amps up and down together ???
+                    pc.printf   ("Drive %.2f\r\n", drive_effort);
+                }
+            }
+            break;
+        default:
+            break;
+    }       //  endof switch  (hand_controller_state) {
+}
+
 int main()
 {
     int eighth_sec_count = 0;
@@ -596,7 +802,7 @@
     MotB   = 0;
 //    MotPtr[0] = &MotorA;    //  Pointers to motor class objects
 //    MotPtr[1] = &MotorB;
-    
+
     Temperature_pin.fall (&temp_sensor_isr);
     Temperature_pin.mode (PullUp);
 
@@ -642,41 +848,47 @@
         pc.printf   ("Check for 24LC64 eeprom FAILED\r\n");
         com2.printf   ("Check for 24LC64 eeprom FAILED\r\n");
         eeprom_flag = false;
-    }
-    else   {        //  Found 24LC64 memory on I2C
+    } else   {      //  Found 24LC64 memory on I2C
         eeprom_flag = true;
         bool k;
         k = rd_24LC64   (0, mode_bytes, 32);
         if  (!k)
             com2.printf ("Error reading from eeprom\r\n");
 
-        int err = 0;
+//        int err = 0;
         for (int i = 0; i < numof_eeprom_options; i++) {
             if  ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max))  {
                 com2.printf ("EEROM error with %s\r\n", option_list[i].t);
-                err++;
+                pc.printf   ("EEROM error with %s\r\n", option_list[i].t);
+                if  (i == ID)   {   //  need to force id to '0'
+                    pc.printf   ("Bad board ID value %d, forcing to \'0\'\r\n");
+                    mode_bytes[ID] = '0';
+                }
+//                err++;
             }
 //            else
 //                com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
         }
         rpm2mph = 0.0;
-        if  (err == 0)  {
-            mode_good_flag = true;
-            MotorA.direction_set    (mode_bytes[MOTADIR]);
-            MotorB.direction_set    (mode_bytes[MOTBDIR]);
-            IAm = mode_bytes[ID];
-            rpm2mph = 60.0                                                          //  to Motor Revs per hour;
-                    * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR])  //  Wheel revs per hour
-                    * PI * ((double)mode_bytes[WHEELDIA] / 1000.0)                  //  metres per hour
-                    * 39.37 / (1760.0 * 36.0);                                      //  miles per hour
-        }
-           //  Alternative ID 1 to 9
+        if  (mode_bytes[WHEELGEAR] == 0)    //  avoid making div0 error
+            mode_bytes[WHEELGEAR]++;
+//        if  (err == 0)  {
+        mode_good_flag = true;
+        MotorA.direction_set    (mode_bytes[MOTADIR]);
+        MotorB.direction_set    (mode_bytes[MOTBDIR]);
+        IAm = mode_bytes[ID];
+        rpm2mph = 60.0                                                          //  to Motor Revs per hour;
+                  * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR])  //  Wheel revs per hour
+                  * PI * ((double)mode_bytes[WHEELDIA] / 1000.0)                  //  metres per hour
+                  * 39.37 / (1760.0 * 36.0);                                      //  miles per hour
+//        }
+        //  Alternative ID 1 to 9
 //        com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
     }
 //    T1 = 0;   Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
     T2 = 0; //  T2, T3, T4 As yet unused pins
-    T3 = 0;
-    T4 = 0;
+//    T3 = 0;
+//    T4 = 0;
 //    T5 = 0; now input from fw/re on remote control box
     T6 = 0;
 //    MotPtr[0]->set_mode (REGENBRAKE);
@@ -684,6 +896,8 @@
     MotorB.set_mode (REGENBRAKE);
     setVI  (0.9, 0.5);
 
+//    prscfuck    (PWM_PRESECALER_DEFAULT);    //  Test fucking with fastpwm prescaler
+
     Servos[0] = Servos[1] = NULL;
     //  NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
     //  Only works with unconditional inline code
@@ -693,17 +907,17 @@
     Servos[0] = & Servo1;
     Servo   Servo2  (PB_9)  ;
     Servos[1] = & Servo2;
-    
+
 //    pc.printf   ("last_temp_count = %d\r\n", last_temp_count);  //  Has had time to do at least 1 conversion
     if  ((last_temp_count > 160) && (last_temp_count < 2400))   //  in range -40 to +100 degree C
         temp_sensor_exists  = true;
-/*
-    //  Setup Complete ! Can now start main control forever loop.
-    //  March 16th 2018 thoughts !!!
-    //  Control from one of several sources and types as selected in options bytes in eeprom.
-    //  Control could be from e.g. Pot, Com2, Servos etc.
-    //  Suggest e.g.
-*/    /*
+    /*
+        //  Setup Complete ! Can now start main control forever loop.
+        //  March 16th 2018 thoughts !!!
+        //  Control from one of several sources and types as selected in options bytes in eeprom.
+        //  Control could be from e.g. Pot, Com2, Servos etc.
+        //  Suggest e.g.
+    */    /*
     switch  (mode_byte) {   //  executed once only upon startup
         case    POT:
             while   (1) {   //  forever loop
@@ -720,7 +934,18 @@
     }
     */
 //    pc.printf   ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
-    pc.printf   ("About to start!\r\n");
+    dc_motor_kicker_timer.start   ();
+    int old_hand_controller_direction = T5;
+    if  (mode_bytes[COMM_SRC] == 3)  {      //  Read fwd/rev switch 'T5', PA15 on 401
+        pc.printf   ("Pot control\r\n");
+        if  (T5)
+            mode_set_both_motors   (FORWARD, 0.0);  //  sets both motors
+        else
+            mode_set_both_motors   (REVERSE, 0.0);
+    }
+
+    pc.printf   ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]);
+
     while   (1) {      //  Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
         while   (!loop_flag)  {         //  Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
             command_line_interpreter_pc    ()  ;   //  Proceed beyond here once loop_timer ticker ISR has set loop_flag true
@@ -728,31 +953,66 @@
             AtoD_reader ();                     //  Performs A to D conversions at rate set by ticker interrupts
         }
         loop_flag = false;              //  Clear flag set by ticker interrupt handler
+        switch  (mode_bytes[COMM_SRC])  {   //  Look to selected source of driving command, act on commands from wherever
+            case    0:  //  Invalid
+                break;
+            case    1:  //  COM1    -   Primarily for testing, bypassed by command line interpreter
+                break;
+            case    2:  //  COM2    -   Primarily for testing, bypassed by command line interpreter
+                break;
+            case    3:  //  Put all hand controller input stuff here
+                hand_control_state_machine  ();
+                break;  //  endof hand controller stuff
+            case    4:  //  Servo1
+                break;
+            case    5:  //  Servo2
+                break;
+            default:
+                pc.printf   ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]);
+                break;
+        }   //  endof   switch  (mode_bytes[COMM_SRC])  {
+
+        dc_motor_kicker_timer.reset   ();
         MotorA.pulses_per_sec   ();   //  Needed to keep table updated to give reading in Hall transitions per second
         MotorB.pulses_per_sec   ();   //  Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
-        T4 = !T4;   //  toggle to hang scope on to verify loop execution
+//        T4 = !T4;   //  toggle to hang scope on to verify loop execution
         //  do stuff
+        if  (MotorA.dc_motor)   {
+            MotorA.raw_V_pwm    (1);
+            MotorA.low_side_on  ();
+        }
+        if  (MotorB.dc_motor)   {
+            MotorB.raw_V_pwm    (1);
+            MotorB.low_side_on  ();
+        }
+        if  (MotorA.dc_motor || MotorB.dc_motor)    {
+//            motors_restore.attach_us    (&restorer, ttime);
+            motors_restore.attach_us    (&restorer, 25);
+        }
+
         if  (flag_8Hz)  {   //  do slower stuff
             flag_8Hz    = false;
             LED = !LED;                   // Toggle LED on board, should be seen to fast flash
-            WatchDog--;
-            if  (WatchDog == 0) {   //  Deal with WatchDog timer timeout here
-                setVI  (0.0, 0.0);  //  set motor volts and amps to zero
-                com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f));   //  Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
-            }                       //  End of dealing with WatchDog timer timeout
-            if  (WatchDog < 0)
-                WatchDog = 0;
+            if  (WatchDogEnable)    {
+                WatchDog--;
+                if  (WatchDog == 0) {   //  Deal with WatchDog timer timeout here
+                    setVI  (0.0, 0.0);  //  set motor volts and amps to zero
+                    com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f));   //  Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
+                }                       //  End of dealing with WatchDog timer timeout
+                if  (WatchDog < 0)
+                    WatchDog = 0;
+            }
             eighth_sec_count++;
             if  (eighth_sec_count > 6)    {   //  Send some status info out of serial port every second and a bit or thereabouts
                 eighth_sec_count = 0;
                 MotorA.current_calc ();     //  Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
                 MotorB.current_calc ();
-/*                if  (temp_sensor_exists)    {
-                    double  tmprt = (double) last_temp_count;
-                    tmprt /= 16.0;
-                    tmprt -= 50.0;
-                    pc.printf   ("Temp %.2f\r\n", tmprt);
-                }*/
+                /*                if  (temp_sensor_exists)    {
+                                    double  tmprt = (double) last_temp_count;
+                                    tmprt /= 16.0;
+                                    tmprt -= 50.0;
+                                    pc.printf   ("Temp %.2f\r\n", tmprt);
+                                }*/
 //                com2.printf   ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IAmin %d, IAave %d, IAmax %d, IB %d, Arpm %d, Brpm %d\r\n", Read_BatteryVolts(), Read_DriverPot(), MotorA.read_Halls  (), MotorB.read_Halls  (), MotorA.I.min, MotorA.I.ave, MotorA.I.max, MotorB.I.ave, (Apps * 60) / 24, (Bpps * 60) / 24);
             }
         }   //  End of if(flag_8Hz)