Revision 8:93203f473f6e, committed 2018-08-18

Comitter:

JonFreeman

Date:

Sat Aug 18 12:51:35 2018 +0000

Parent:

7:6deaeace9a3e

Child:

9:ac2412df01be

Commit message:

Work underway to drive brushed motors.; Code as supplied to Rob

Changed in this revision

--- a/DualBLS.h	Sun Jun 17 06:59:37 2018 +0000
+++ b/DualBLS.h	Sat Aug 18 12:51:35 2018 +0000
@@ -6,20 +6,23 @@
 const   int     TIMEOUT_SECONDS = 30;
 
 /*  Please Do Not Alter these */
-const   int     VOLTAGE_READ_INTERVAL_US    = 50,       //  Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass
+const   int     PWM_PRESECALER_DEFAULT      = 5,
+                VOLTAGE_READ_INTERVAL_US    = 50,       //  Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass
                 MAIN_LOOP_REPEAT_TIME_US    = 31250,    //  31250 us, with TACHO_TAB_SIZE = 32 means tacho_ticks_per_time is tacho_ticks_per_second
                 MAIN_LOOP_ITERATION_Hz      = 1000000 / MAIN_LOOP_REPEAT_TIME_US,
                 CURRENT_SAMPLES_AVERAGED    = 100,     //  Current is spikey. Reading smoothed by using average of this many latest current readings
-                PWM_HZ              = 16000,    //  chosen to be above cutoff frequency of average human ear
-                MAX_PWM_TICKS       = (SystemCoreClock / PWM_HZ),
+                PWM_HZ              = 15000,    //  chosen to be above cutoff frequency of average human ear
+                MAX_PWM_TICKS       = (SystemCoreClock / (PWM_HZ * PWM_PRESECALER_DEFAULT)),
                 TICKLE_TIMES    =   100 ,
                 WATCHDOG_RELOAD = (TIMEOUT_SECONDS * 8);    //  WatchDog counter ticked down in 8Hz loop
 
 /*  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}  ;  //  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
 struct  optpar  {
     int min, max, def;  //  min, max, default
     const char * t;     //  description
@@ -32,9 +35,15 @@
     {0, 2, 2, "Servo2 0, 1, 2 = Not used, Input, Output"},
     {1, 5, 2, "Command source 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2"},
     {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, //  defaults to '0' before eerom setup for first time
-    {20, 253, 98,  "Wheel diameter mm"},   //  New 01/06/2018
+    {50, 253, 98,  "Wheel diameter mm"},   //  New 01/06/2018
     {10, 253, 27,  "Motor pinion"},   //  New 01/06/2018
-    {20, 253, 85,  "Wheel gear"},   //  New 01/06/2018
+    {50, 253, 85,  "Wheel gear"},   //  New 01/06/2018
+    {1, 20, 4, "Bogie power closest hundreds of Watt"}, //  New 22/06/2018
+    {0, 100, 0, "Future 1"},
+    {0, 100, 0, "Future 2"},
+    {0, 100, 0, "Future 3"},
+    {0, 100, 0, "Future 4"},
+    {0, 100, 0, "Future 5"},
 }   ;
 const int    numof_eeprom_options    = sizeof(option_list) / sizeof (struct optpar);
 

--- a/F401RE.h	Sun Jun 17 06:59:37 2018 +0000
+++ b/F401RE.h	Sat Aug 18 12:51:35 2018 +0000
@@ -91,8 +91,8 @@
 DigitalOut  T3        (PB_15);    //  Pin 36
 //  BufferedSerial com2 pins 37 Tx, 38 Rx
 BufferedSerial  com2          (PC_6, PC_7);    //  Pins 37, 38  tx, rx to XBee module
-FastPWM     A_MAX_V_PWM     (PC_8, 1),  //  Pin 39                  pwm3/3
-            A_MAX_I_PWM     (PC_9, 1); //  pin 40, prescaler value  pwm3/4
+FastPWM     A_MAX_V_PWM     (PC_8, PWM_PRESECALER_DEFAULT),  //  Pin 39                  pwm3/3
+            A_MAX_I_PWM     (PC_9, PWM_PRESECALER_DEFAULT); //  pin 40, prescaler value  pwm3/4
 //InterruptIn MotB_Hall   (PA_8); //  Pin 41
 //  Pin 41  Port_A AWH
 //  BufferedSerial com3 pins 42 Tx, 43 Rx
@@ -126,8 +126,8 @@
 InterruptIn MAH3    (PC_12);    //  Pin 53
 InterruptIn MBH1    (PD_2);     //  Pin 54
 DigitalOut  T6      (PB_3);     //  Pin 55
-FastPWM     B_MAX_V_PWM     (PB_4, 1),  //  Pin 56                  pwm3/3
-            B_MAX_I_PWM     (PB_5, 1); //  pin 57, prescaler value  pwm3/4
+FastPWM     B_MAX_V_PWM     (PB_4, PWM_PRESECALER_DEFAULT),  //  Pin 56                  pwm3/3
+            B_MAX_I_PWM     (PB_5, PWM_PRESECALER_DEFAULT); //  pin 57, prescaler value  pwm3/4
 
 I2C i2c                     (PB_7, PB_6);   //  Pins 58, 59 For 24LC64 eeprom
 //  Pin 60  BOOT0

--- a/F446ZE.h	Sun Jun 17 06:59:37 2018 +0000
+++ b/F446ZE.h	Sat Aug 18 12:51:35 2018 +0000
@@ -1,16 +1,14 @@
 //  5TH JUNE 2018 NONE OF THIS IS RIGHT YET !!
-
 //  Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out
-
 //  Port A -> MotorA, Port B -> MotorB
 const   uint16_t
-AUL = 1 << 0,   //  Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - 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,
+AUL = 1 << 1,   //  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 << 2,   //  These are which port bits connect to which mosfet driver
+AWL = 1 << 3,
 
-AUH = 1 << 1,
-AVH = 1 << 7,
-AWH = 1 << 8,
+AUH = 1 << 4,
+AVH = 1 << 5,
+AWH = 1 << 6,
 
 AUV =   AUH | AVL,  //  Each of 6 possible output energisations made up of one hi and one low
 AVU =   AVH | AUL,
@@ -29,8 +27,8 @@
 BWL = 1 << 2,
 
 BUH = 1 << 10,
-BVH = 1 << 12,
-BWH = 1 << 13,
+BVH = 1 << 11,
+BWH = 1 << 3,
 
 BUV =   BUH | BVL,
 BVU =   BVH | BUL,
@@ -46,14 +44,14 @@
 
 PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH,            //  NEW METHOD FOR DGD21032 MOSFET DRIVERS
 PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH;
-
-PortOut MotA    (PortA, PORT_A_MASK);   //  Activate output ports to motor drivers
-PortOut MotB    (PortB, PORT_B_MASK);
+//  Use Ports E and G - best first guess July 2018
+PortOut MotA    (PortE, PORT_A_MASK);   //  Activate output ports to motor drivers
+PortOut MotB    (PortG, PORT_B_MASK);
 
 //  Pin 1   VBAT    NET +3V3
 
 //DigitalIn   J3         (PC_13, PullUp);//  Pin 2   Jumper pulls to GND, R floats Hi
-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
+InterruptIn   Temperature_pin   (PD_7);//  Pin ??   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
@@ -70,16 +68,16 @@
 //  Pin 14  Port_A AUL
 //  Pin 15  Port_A AUH
 //  Pins 16, 17 BufferedSerial pc
-BufferedSerial  pc          (PA_2, PA_3, 512, 4, NULL);    //  Pins 16, 17    tx, rx to pc via usb lead
+BufferedSerial  pc          (PD_8, PD_9, 512, 4, NULL);    //  Pins 16, 17    tx, rx to pc via usb lead
 //  Pin 18  VSS     NET GND
 //  Pin 19  VDD     NET +3V3
 //  Pin 20  Port_A AWL
 //  Pin 21  DigitalOut led1(LED1);
-DigitalOut  LED           (PA_5); //  Pin 21
+DigitalOut  LED           (PB_0); //  Pin ?? as fitted to Nucleo board. Other leds use PB_7 and PB_14
 //  Pin 22  Port_A AVL
 //  Pin 23  Port_A AVH
-InterruptIn  MBH2      (PC_4); //  Pin 24
-InterruptIn  MBH3      (PC_5); //  Pin 25
+InterruptIn  MBH2      (PD_1); //  Pin ??
+InterruptIn  MBH3      (PD_3); //  Pin ??
 //  Pin 26  Port_B BUL
 //  Pin 27  Port_B BVL
 //  Pin 28  Port_B BWL
@@ -89,17 +87,21 @@
 //  Pin 32  VDD
 //  Pin 33  Port_B BVH
 //  Pin 34  Port_B BWH
-DigitalOut  T4        (PB_14);    //  Pin 35
-DigitalOut  T3        (PB_15);    //  Pin 36
+
+//DigitalOut  T4        (PB_14);    //  Pin **
+//DigitalOut  T3        (PB_15);    //  Pin **
+
 //  BufferedSerial com2 pins 37 Tx, 38 Rx
-BufferedSerial  com2          (PC_6, PC_7);    //  Pins 37, 38  tx, rx to XBee module
-FastPWM     A_MAX_V_PWM     (PC_8, 1),  //  Pin 39                  pwm3/3
-            A_MAX_I_PWM     (PC_9, 1); //  pin 40, prescaler value  pwm3/4
+BufferedSerial  com2          (PC_10, PC_11);    //  Pins 37, 38  tx, rx to XBee module
+FastPWM     A_MAX_V_PWM     (PC_8, PWM_PRESECALER_DEFAULT),  //  Pin 39                  pwm3/3
+            A_MAX_I_PWM     (PC_9, PWM_PRESECALER_DEFAULT); //  pin 40, prescaler value  pwm3/4
 //InterruptIn MotB_Hall   (PA_8); //  Pin 41
 //  Pin 41  Port_A AWH
 //  BufferedSerial com3 pins 42 Tx, 43 Rx
 //InterruptIn tryseredge  (PA_9);
-BufferedSerial  com3        (PA_9, PA_10);    //    Pins 42, 43  tx, rx to any aux module
+
+BufferedSerial  com3        (PC_12, PD_2);    //    Pins 42, 43  tx, rx to any aux module
+
 //  PA_9 is Tx. I wonder, can we also use InterruptIn on this pin to generate interrupts on tx bit transitions ? Let's find out !
 //  No.
 
@@ -111,7 +113,7 @@
 
 //InterruptIn T1  (PA_12);    //  Pin 45 now input counting pulses from LMT01 temperature sensor
 //  InterruptIn DOES NOT WORK ON PA_12. Boards are being made, will have to wire link PA12 to PC13
-DigitalIn   T1    (PA_12);
+//DigitalIn   T1    (PA_12);
 ////InterruptIn T1  (PC_13);    //  Pin 45 now input counting pulses from LMT01 temperature sensor
 
 
@@ -123,21 +125,21 @@
 
 //Was DigitalOut  T5  (PA_15); //  Pin 50
 DigitalIn   T5  (PA_15); //  Pin 50 now fwd/rev from remote control box if fitted
-InterruptIn MAH1    (PC_10);    //  Pin 51
-InterruptIn MAH2    (PC_11);    //  Pin 52
-InterruptIn MAH3    (PC_12);    //  Pin 53
-InterruptIn MBH1    (PD_2);     //  Pin 54
+InterruptIn MAH1    (PD_4);    //  Pin 51
+InterruptIn MAH2    (PD_5);    //  Pin 52
+InterruptIn MAH3    (PD_6);    //  Pin 53
+InterruptIn MBH1    (PD_0);     //  Pin 54
 DigitalOut  T6      (PB_3);     //  Pin 55
-FastPWM     B_MAX_V_PWM     (PB_4, 1),  //  Pin 56                  pwm3/3
-            B_MAX_I_PWM     (PB_5, 1); //  pin 57, prescaler value  pwm3/4
+FastPWM     B_MAX_V_PWM     (PE_13, PWM_PRESECALER_DEFAULT),  //  Pin 56                  pwm3/3
+            B_MAX_I_PWM     (PE_14, PWM_PRESECALER_DEFAULT); //  pin 57, prescaler value  pwm3/4
 
-I2C i2c                     (PB_7, PB_6);   //  Pins 58, 59 For 24LC64 eeprom
+I2C i2c                     (PB_9, PB_8);   //  Pins 58, 59 For 24LC64 eeprom
 //  Pin 60  BOOT0
 
 //  Servo pins, 2 off. Configured as Input to read radio control receiver
 //  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    (PF_14); //  Pin 61  to read output from rc rx
+InterruptIn Servo2_i    (PF_15); //  Pin 62  to read output from rc rx
 
 //  Pin 63  VSS
 //  Pin 64  VDD

--- a/cli_BLS_nortos.cpp	Sun Jun 17 06:59:37 2018 +0000
+++ b/cli_BLS_nortos.cpp	Sat Aug 18 12:51:35 2018 +0000
@@ -7,7 +7,9 @@
 using namespace std;
 
 extern  int I_Am    ()  ;      //  Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
-extern  int WatchDog;
+extern  int     WatchDog;
+extern  bool    WatchDogEnable;
+extern  char    mode_bytes[];
 
 const int   BROADCAST   = '\r';
 const   int MAX_PARAMS = 20;
@@ -31,6 +33,11 @@
 extern  void    read_last_VI    (double * val)  ;   //  only for test from cli
 
 //BufferedSerial * com;
+extern  double  Read_DriverPot  ();
+void    pot_cmd (struct parameters & a)
+{
+    pc.printf   ("Driver's pot %.3f\r\n", Read_DriverPot  ());
+}
 
 void    null_cmd (struct parameters & a)
 {
@@ -38,7 +45,45 @@
         a.com->printf   ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
 }
 
-extern  void    mode_set   (int mode, double val)  ;   //  called from cli to set fw, re, rb, hb
+//    {"wden", "enable watchdog if modes allow", wden_lococmd},
+//    {"wddi", "disable watchdog always", wddi_lococmd},
+
+void    wden_lococmd (struct parameters & a)
+{
+    if  (mode_bytes[COMM_SRC] != 3)     //  When not hand pot control, allow watchdog enable
+        WatchDogEnable  = true;
+}
+void    wden_pccmd (struct parameters & a)
+{
+    wden_lococmd    (a);
+    a.com->printf   ("Watchdog %sabled\r\n", WatchDogEnable ? "En":"Dis");
+}
+
+void    wddi_lococmd (struct parameters & a)
+{
+    WatchDogEnable  = false;
+}
+void    wddi_pccmd (struct parameters & a)
+{
+    wddi_lococmd    (a);
+    a.com->printf   ("Watchdog %sabled\r\n", WatchDogEnable ? "En":"Dis");
+}
+
+extern  void prscfuck   (int);
+void    pf_cmd (struct parameters & a)
+{
+    prscfuck    ((int)a.dbl[0]);
+}
+
+extern  void    report_motor_types  ()  ;
+void    mt_cmd (struct parameters & a)
+{
+    report_motor_types  ();
+//    if  (a.respond) 
+//        a.com->printf   ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
+}
+
+extern  void    mode_set_both_motors   (int mode, double val)  ;   //  called from cli to set fw, re, rb, hb
 extern  void    read_supply_vi   (double * val)  ;
 
 void    rdi_cmd (struct parameters & a)  //  read motor currents
@@ -61,19 +106,19 @@
 
 void    fw_cmd (struct parameters & a)  //  Forward command
 {
-    mode_set   (FORWARD, 0.0);
+    mode_set_both_motors   (FORWARD, 0.0);
 }
 
 void    re_cmd (struct parameters & a)  //  Reverse command
 {
-    mode_set   (REVERSE, 0.0);
+    mode_set_both_motors   (REVERSE, 0.0);
 }
 
 void    rb_cmd (struct parameters & a)      //  Regen brake command
 {
     double b = a.dbl[0] / 100.0;
 //    a.com->printf   ("Applying brake %.3f\r\n", b);
-    mode_set   (REGENBRAKE, b);
+    mode_set_both_motors   (REGENBRAKE, b);
 //    apply_brake (b);
 }
 
@@ -105,7 +150,25 @@
 //            last;
 }   ;
 */
-extern  char mode_bytes[];
+
+//  New 22 June 2018
+//  get bogie bytes - report back to touch controller
+void    gbb_cmd (struct parameters & a)       //  
+{
+    if  (a.target_unit == BROADCAST || !a.resp_always) {
+//        a.com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
+    } else    {
+        pc.printf   ("At gbb\r\n");
+        char    eeprom_contents[36];    //  might need to be unsigned?
+        memset  (eeprom_contents, 0, 36);
+        a.com->printf   ("gbb");
+        rd_24LC64   (0, eeprom_contents, 32);
+        for (int i = 0; i < numof_eeprom_options; i++)
+            a.com->printf (" %d", eeprom_contents[i]);
+        a.com->putc ('\r');
+        a.com->putc ('\n');
+    }
+}
 
 void    mode_cmd (struct parameters & a)       //  With no params, reads eeprom contents. With params sets eeprom contents
 {
@@ -154,7 +217,7 @@
         a.com->printf   ("numof params = %d\r\n", a.numof_dbls);
         a.com->printf   ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
     }
-    mode_set   (HANDBRAKE, 0.0);
+    mode_set_both_motors   (HANDBRAKE, 0.0);
 }
 
 extern  uint32_t    last_temp_count;
@@ -183,9 +246,11 @@
 extern  double  rpm2mph ;
 void    mph_cmd (struct parameters & a) //  to report miles per hour
 {
+    if  (a.respond) {
         uint32_t dest[3];
         read_RPM    (dest);     //  gets rpm for each motor
-        a.com->printf ("mph%c %.3f\r\n", mode_bytes[ID], (double)(dest[0] + dest[1]) * rpm2mph / 2.0);
+        a.com->printf ("mph%c %.3f\r", mode_bytes[ID], (double)(dest[0] + dest[1]) * rpm2mph / 2.0);
+    }
 }
 
 void    menucmd (struct parameters & a);
@@ -240,15 +305,19 @@
     {"v", "set motors V percent RANGE 0 to 100", v_cmd},
     {"i", "set motors I percent RANGE 0 to 100", i_cmd},
     {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
-    {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
+    {"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd},
     {"mode", "read or set params in eeprom", mode_cmd},
     {"erase", "set eeprom contents to all 0xff", erase_cmd},
     {"tem", "report temperature", temperature_cmd},
     {"kd", "kick the dog, reloads WatchDog", kd_cmd},
+    {"wden", "enable watchdog if modes allow", wden_lococmd},
+    {"wddi", "disable watchdog always", wddi_lococmd},
     {"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},
     {"rdi", "read motor currents and power voltage", rdi_cmd},
-    {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
+    {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},    //  OBSOLETE, replaced by 'gbb'
+    {"gbb", "get bogie bytes from eeprom and report", gbb_cmd},
     {"nu", "do nothing", null_cmd},
 };
 
@@ -258,6 +327,9 @@
 struct  kb_command const pc_command_list[] = {
     {"ls", "Lists available commands", menucmd},
     {"?", "Lists available commands, same as ls", menucmd},
+    {"mtypes", "report types of motors found", mt_cmd},
+    {"pf", "try changing FastPWM prescaler values", pf_cmd},
+    {"pot", "read drivers control pot", pot_cmd},
     {"fw", "forward", fw_cmd},
     {"re", "reverse", re_cmd},
     {"rb", "regen brake 0 to 99 %", rb_cmd},
@@ -265,16 +337,19 @@
     {"v", "set motors V percent RANGE 0 to 100", v_cmd},
     {"i", "set motors I percent RANGE 0 to 100", i_cmd},
     {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
-    {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
+    {"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd},
     {"mode", "read or set params in eeprom", mode_cmd},
     {"erase", "set eeprom contents to all 0xff", erase_cmd},
     {"tem", "report temperature", temperature_cmd},
     {"kd", "kick the dog, reloads WatchDog", kd_cmd},
+    {"wden", "enable watchdog if modes allow", wden_pccmd},
+    {"wddi", "disable watchdog always", wddi_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},
     {"rdi", "read motor currents and power voltage", rdi_cmd},
     {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
+    {"gbb", "get bogie bytes from eeprom and report", gbb_cmd},    //  OBSOLETE, replaced by 'gbb'
     {"nu", "do nothing", null_cmd},
 };
 

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sud.cpp	Sat Aug 18 12:51:35 2018 +0000
@@ -0,0 +1,117 @@
+//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);
+}