Revision 4:21d91465e4b1, committed 2018-04-26

Comitter:

JonFreeman

Date:

Thu Apr 26 08:23:04 2018 +0000

Parent:

3:ecb00e0e8d68

Child:

5:ca86a7848d54

Commit message:

Adding setup in eeprom code, responses to be aligned to controller;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Thu Apr 26 08:23:04 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07

--- a/cli_nortos.cpp	Sun Mar 18 08:17:56 2018 +0000
+++ b/cli_nortos.cpp	Thu Apr 26 08:23:04 2018 +0000
@@ -23,6 +23,8 @@
 //extern  void    set_V_limit (double p)  ;   //  Sets max motor voltage
 extern  void    send_test   ()  ;
 extern  void    setVI   (double v, double i)  ;
+extern  void    setV    (double v)  ;
+extern  void    setI    (double i)  ;
 
 BufferedSerial * com;
 
@@ -30,12 +32,6 @@
     com->printf   ("At null_cmd, parameters : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
 }
 
-/*extern  void    tickleboth  ()  ;
-void    ti_cmd (struct parameters & a)   {
-    com->printf   ("At tickle\r\n");
-    tickleboth  ();
-}*/
-
 extern  void    mode_test   (int mode, double val)  ;
 
 void    fw_cmd (struct parameters & a)   {
@@ -141,10 +137,28 @@
     setVI   (a.dbl[0] / 100.0, a.dbl[1] / 100.0);
 }
 
+void    v_cmd (struct parameters & a)
+{
+    com->printf   ("In setV, setting V to %.2f\r\n", a.dbl[0]);
+    setV   (a.dbl[0] / 100.0);
+}
+
+void    i_cmd (struct parameters & a)
+{
+    com->printf   ("In setI, setting I to %.2f\r\n", a.dbl[0]);
+    setI   (a.dbl[0] / 100.0);
+}
+
 void    kd_cmd (struct parameters & a)  //  kick the watchdog
 {
 }
 
+/*extern  void    seredgetest ()  ;
+void    s_cmd (struct parameters & a)  //  test use of InterruptIn on serialout
+{
+    seredgetest ()  ;
+}*/
+
 void    who_cmd (struct parameters & a)
 {
     int i = I_Am    ();
@@ -161,11 +175,13 @@
 struct  kb_command const command_list[] = {
     {"ls", "Lists available commands", menucmd},
     {"?", "Lists available commands, same as ls", menucmd},
-//    {"ti", "tickle to try to get mosfet driver charge pump primed", ti_cmd},
+//    {"s", "seredgetest", s_cmd},
     {"fw", "forward", fw_cmd},
     {"re", "reverse", re_cmd},
     {"rb", "regen brake 0 to 99 %", rb_cmd},
     {"hb", "hand brake", hb_cmd},
+    {"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},
     {"mode", "read or set params in eeprom", mode_cmd},

--- a/main.cpp	Sun Mar 18 08:17:56 2018 +0000
+++ b/main.cpp	Thu Apr 26 08:23:04 2018 +0000
@@ -2,11 +2,10 @@
 #include "DualBLS.h"
 #include "BufferedSerial.h"
 #include "FastPWM.h"
+#include "Servo.h"
 /*  STM32F401RE - compile using NUCLEO-F401RE
 //  PROJECT -   Dual Brushless Motor Controller -   March 2018.
 
-DigitalIn   nFault1 (); needs pullup
-DigitalIn   nFault2 (); needs pullup
 AnalogIn to read each motor current
 
 ____________________________________________________________________________________
@@ -24,15 +23,15 @@
 */
 
 //  Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out
-#define SERVO1_IN
+//#define SERVO1_IN
 //#define SERVO1_OUT
 //#define SERVO2_IN
-#define SERVO2_OUT
+//#define SERVO2_OUT
 
 
 //  Port A -> MotorA, Port B -> MotorB
 const   uint16_t
-AUL = 1 << 0,   //  Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side)
+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,
 AWL = 1 << 4,
 
@@ -122,7 +121,10 @@
 //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
+//  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.
 
 //  Feb 2018 Pins 44 and 45 now liberated, could use for serial or other uses
 //BufferedSerial  extra_ser   (PA_11, PA_12);    //  Pins 44, 45  tx, rx to XBee module
@@ -144,19 +146,10 @@
 I2C i2c                     (PB_7, PB_6);   //  Pins 58, 59 For 24LC64 eeprom
 //  Pin 60  BOOT0
 
-#ifdef  SERVO1_IN
+//  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
-#endif
-#ifdef  SERVO1_OUT
-FastPWM     Servo1_o    (PB_8, 2);  //Prescaler 2. This is pwm 4/3
-#endif
-
-#ifdef  SERVO2_IN
 InterruptIn Servo2_i    (PB_9); //  Pin 62  to read output from rc rx
-#endif
-#ifdef  SERVO2_OUT
-FastPWM     Servo2_o    (PB_9, 2); //  Prescaler 2. This is pwm 4/4
-#endif
 
 //  Pin 63  VSS
 //  Pin 64  VDD
@@ -168,7 +161,8 @@
                 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;
+                MAX_PWM_TICKS       = SystemCoreClock / PWM_HZ,
+                TICKLE_TIMES    =   100 ;
 
 /*  End of Please Do Not Alter these */
 
@@ -180,9 +174,8 @@
             AtoD_Semaphore = 0;
 bool        loop_flag   = false;    //  made true in ISR_loop_timer, picked up and made false again in main programme loop
 bool        flag_8Hz    = false;    //  As loop_flag but repeats 8 times per sec
-bool        sounder_on  = false;
 
-double      angle = 0.0,    angle_step = 0.000005,  sinv, cosv;
+double      angle = 0.0,    angle_step = 0.00005,  sinv, cosv;
 
 //double      test_pot = 0.0, test_amps = 0.0;    //  These used in knifeandfork code testing only
 /*  End of Global variable declarations */
@@ -192,6 +185,14 @@
 
 //  Interrupt Service Routines
 
+/*uint32_t    edgeintcnt = 0;
+void    seredgerise ()  {   edgeintcnt++;   }
+void    seredgefall ()  {   edgeintcnt++;   }
+
+void    seredgetest ()  {
+    com2.printf ("edgeintcnt = %d\r\n", edgeintcnt);
+    com3.printf ("%c", 0x55);
+}*/
 /** void    ISR_loop_timer  ()
 *   This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
 *   This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
@@ -224,33 +225,35 @@
 1.  Input of pwm from model control Rx
 2.  Output pwm to drive model control servo
 */
-//enum    {SERVO_UNUSED, SERVO_PWM_IN, SERVO_PWM_OUT}  ;
-class   Servo
-{
-    FastPWM * out;
+enum    {SERVO_UNUSED, SERVO_IN, SERVO_OUT}  ;
+
+class   RControl_In
+{  //  Read servo style pwm input
     Timer   t;
-
-public:
-
-    bool    rx_active;
     int32_t clock_old, clock_new;
     int32_t pulse_width_us, period_us;
-    Servo   ()  {   //  Constructor
+public:
+    RControl_In   ()   {
         pulse_width_us = period_us = 0;
-        clock_old = clock_new = 0;
-        out = NULL;
-        rx_active = false;
-    }
+        com2.printf ("Setting up Radio_Congtrol_In\r\n");
+    }   ;
     bool    validate_rx ()  ;
     void    rise    ()  ;
     void    fall    ()  ;
-    void    out_pw_set         (double);
-    void    period_ticks        (uint32_t);
-    void    pulsewidth_ticks    (uint32_t);
-    void    set_out (FastPWM *);
-}   S1, S2;
+    uint32_t    pulsewidth    ()  ;
+    uint32_t    period    ()  ;
+    bool    rx_active;
+}   ;
 
-bool    Servo::validate_rx ()
+uint32_t    RControl_In::pulsewidth   ()  {
+    return  pulse_width_us;
+}
+
+uint32_t    RControl_In::period   ()  {
+    return  period_us;
+}
+
+bool    RControl_In::validate_rx ()
 {
     if  ((clock() - clock_new) > 4)
         rx_active = false;
@@ -259,14 +262,14 @@
     return  rx_active;
 }
 
-void    Servo::rise    ()
+void    RControl_In::rise    ()
 {
     t.stop   ();
     period_us = t.read_us    ();
     t.reset ();
     t.start ();
 }
-void    Servo::fall    ()
+void    RControl_In::fall    ()
 {
     pulse_width_us = t.read_us   ();
     clock_old = clock_new;
@@ -274,45 +277,10 @@
     if  ((clock_new - clock_old) < 4)
         rx_active = true;
 }
-void    Servo::out_pw_set         (double outpw)
-{
-    if  (outpw > 1.0)
-        outpw = 1.0;
-    if  (outpw < 0.0)
-        outpw = 0.0;
-    outpw *= 1.2;       //  Change range to 1.2 ms to cover out pulse width range 0.9 to 2.1 ms
-    outpw += 0.9;       //  Bias to nom min servo range
-    pulsewidth_ticks ((uint32_t)(outpw * (SystemCoreClock / 2000)));
-}
-void    Servo::period_ticks        (uint32_t pt)
-{
-    if  (out)   out->period_ticks   (pt);
-}
-void    Servo::pulsewidth_ticks    (uint32_t wt)
-{
-    if  (out)   out->pulsewidth_ticks   (wt);
-}
-void    Servo::set_out (FastPWM * op)
-{
-    out = op;
-}
+
 
-void    Servo1rise  ()
-{
-    S1.rise   ();
-}
-void    Servo1fall  ()
-{
-    S1.fall ();
-}
-void    Servo2rise  ()
-{
-    S2.rise   ();
-}
-void    Servo2fall  ()
-{
-    S2.fall ();
-}
+Servo * Servos[2];
+
 //uint32_t    HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
 /*
     5   1   3   2   6   4  SENSOR SEQUENCE
@@ -328,47 +296,26 @@
     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
-    KEEP_L_MASK_A, KEEP_H_MASK_A,   //  [32 and 33]
-    (uint16_t)((uint32_t)&MAH1 >> 16), (uint16_t)((uint32_t)&MAH1 & 0xffff),
-    (uint16_t)((uint32_t)&MAH2 >> 16), (uint16_t)((uint32_t)&MAH2 & 0xffff),
-    (uint16_t)((uint32_t)&MAH3 >> 16), (uint16_t)((uint32_t)&MAH3 & 0xffff)
-//    ((uint32_t)&MAH1),
-//    ((uint32_t)&MAH2),
-//    ((uint32_t)&MAH3)
-//    (uint16_t)((uint32_t)&MAH1 >> 16), (uint16_t)((uint32_t)&MAH1 & 0xffff),
+    KEEP_L_MASK_A, KEEP_H_MASK_A   //  [32 and 33]
 }   ;
-const   uint32_t    A_t2[] = {
-    (uint32_t)&MAH1,
-    (uint32_t)&MAH2,
-    (uint32_t)&MAH3
+InterruptIn * AHarr[] = {
+    &MAH1,
+    &MAH2,
+    &MAH3
 }   ;
-/*const   uint16_t    A_tabl[] = {    //  Table revised advancing magnetic pull angle - WORKS, but sucks more power for no apparent advantage
-    0,  0,  0,  0,  0,  0,  0,  0,  //  Handbrake
-    0,  AWU,AVW,AVU,AUV,AWV,AUW,  0,  //  Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,    //  JP, FR, SG, PWM = 1 0 1 1   Forward1
-    0,  AVU,AUW,AVW,AWV,AWU,AUV,  0,  //  Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,    //  JP, FR, SG, PWM = 1 1 0 1   Reverse1
-    0,  BRA,BRA,BRA,BRA,BRA,BRA,0   //  Regenerative Braking
-}   ;*/
 const   uint16_t    B_tabl[] = {
     0,  0,  0,  0,  0,  0,  0,  0,  //  Handbrake
     0,  BWV,BVU,BWU,BUW,BUV,BVW,  0,  //  Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,    //  JP, FR, SG, PWM = 1 0 1 1   Forward1
     0,  BVW,BUV,BUW,BWU,BVU,BWV,  0,  //  Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,    //  JP, FR, SG, PWM = 1 1 0 1   Reverse1
     0,  BRB,BRB,BRB,BRB,BRB,BRB,0,   //  Regenerative Braking
-    KEEP_L_MASK_B, KEEP_H_MASK_B,
-    (uint16_t)((uint32_t)&MBH1 >> 16), (uint16_t)((uint32_t)&MBH1 & 0xffff),
-    (uint16_t)((uint32_t)&MBH2 >> 16), (uint16_t)((uint32_t)&MBH2 & 0xffff),
-    (uint16_t)((uint32_t)&MBH3 >> 16), (uint16_t)((uint32_t)&MBH3 & 0xffff)
-//    ((uint32_t)&MBH1),
-//    ((uint32_t)&MBH2),
-//    ((uint32_t)&MBH3)
+    KEEP_L_MASK_B, KEEP_H_MASK_B
 }   ;
-const   uint32_t    B_t2[] = {
-    (uint32_t)&MBH1,
-    (uint32_t)&MBH2,
-    (uint32_t)&MBH3
+InterruptIn * BHarr[] = {
+    &MBH1,
+    &MBH2,
+    &MBH3
 }   ;
 
-//   void * vp[] = {(void*)MAH1, (void*)MAH2};
-
 class   motor
 {
     uint32_t    Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; //  to contain one seconds worth
@@ -386,7 +333,7 @@
     uint32_t    current_samples[CURRENT_SAMPLES_AVERAGED];  //  Circular buffer where latest current readings get stored
     uint32_t    Hindex[2], tickleon, encoder_error_cnt;
     motor   ()  {}  ;   //  Default constructor
-    motor   (PortOut * , FastPWM * , FastPWM * , const uint16_t *, const uint32_t *)  ;
+    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
@@ -400,12 +347,12 @@
     void    high_side_off   ()  ;
 }   ;   //MotorA, MotorB, or even Motor[2];
 
-motor   MotorA  (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, A_t2);
-motor   MotorB  (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, B_t2);
+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, const uint32_t * lut32ptr)        //  Constructor
+motor::motor    (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall)        //  Constructor
 {   //  Constructor
     maxV = _maxV_;
     maxI = _maxI_;
@@ -434,12 +381,9 @@
     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 = (InterruptIn *)(((uint32_t)lut[34] << 16) | (uint32_t)lut[35]);
-    Hall2 = (InterruptIn *)(((uint32_t)lut[36] << 16) | (uint32_t)lut[37]);
-    Hall3 = (InterruptIn *)(((uint32_t)lut[38] << 16) | (uint32_t)lut[39]);
-//    Hall1 = (InterruptIn *)lut32ptr[0];
-//    Hall1 = (InterruptIn *)lut32ptr[1];
-//    Hall1 = (InterruptIn *)lut32ptr[2];
+    Hall1 = Hall[0];
+    Hall2 = Hall[1];
+    Hall3 = Hall[2];
 }
 
 void    motor::direction_set   (int dir)  {
@@ -510,7 +454,7 @@
 {           //  Can also test for motor running or not here
     if  (ppstmp == Hall_total)  {
         moving_flag  = false;       //  Zero Hall transitions since previous call - motor not moving
-        tickleon    = 100;
+        tickleon    = TICKLE_TIMES;
     }
     else    {
         moving_flag  = true;
@@ -597,17 +541,25 @@
 }
 
 void    setVI   (double v, double i)  {
-//    void    set_V_limit (double)    ;  //  Sets max motor voltage
-//    void    set_I_limit (double)    ;  //  Sets max motor current
+    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)  {
     MotorA.set_V_limit  (v);
+    MotorB.set_V_limit  (v);
+}
+void    setI    (double i)  {
     MotorA.set_I_limit  (i);
-    MotorB.set_V_limit  (v);
     MotorB.set_I_limit  (i);
 }
 
 void    sincostest  ()  {
     sinv = sin(angle);  //  to set speed and direction of MotorA
     cosv = cos(angle);  //  to set speed and direction of MotorB
+        Servos[0]->write  ((sinv + 1.0) / 2.0);
+        Servos[1]->write  ((cosv + 1.0) / 2.0);
     angle += angle_step;
     if  (angle > TWOPI)
         angle -= TWOPI;
@@ -617,21 +569,21 @@
         MotorA.set_mode (REVERSE);
         sinv = -sinv;
     }
-    MotorA.set_V_limit  (0.01 + (sinv / 8.0));
+    MotorA.set_V_limit  (0.01 + (sinv / 1.3));
     if  (cosv > 0.0)
         MotorB.set_mode (FORWARD);
     else    {
         MotorB.set_mode (REVERSE);
         cosv = -cosv;
     }
-    MotorB.set_V_limit  (0.01 + (cosv / 8.0));
+    MotorB.set_V_limit  (0.01 + (cosv / 1.3));
 }
 
 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;
 
-    sincostest  ();
+//    sincostest  ();
 
     if  (MotorA.tickleon)
         MotorA.high_side_off    ();
@@ -727,17 +679,21 @@
     return  i | '0';
 }
 
+
 int main()
 {
-    int j = 0;
+    int eighth_sec_count = 0;
     uint32_t    Apps, Bpps;
 
-    MotA   = 0;     //  Motor drive ports A and B
+    MotA   = 0;     //  Output all 0s to Motor drive ports A and B
     MotB   = 0;
-    MotPtr[0] = &MotorA;
+    MotPtr[0] = &MotorA;    //  Pointers to motor class objects
     MotPtr[1] = &MotorB;
 
-    MAH1.rise   (& MAH_isr);
+//    tryseredge.rise (&seredgerise);
+//    tryseredge.fall (&seredgefall);
+
+    MAH1.rise   (& MAH_isr);    //  Set up interrupt vectors
     MAH1.fall   (& MAH_isr);
     MAH2.rise   (& MAH_isr);
     MAH2.fall   (& MAH_isr);
@@ -750,20 +706,16 @@
     MBH2.fall   (& MBH_isr);
     MBH3.rise   (& MBH_isr);
     MBH3.fall   (& MBH_isr);
-/*
-    MAH1.rise   (& MAH1r);
-    MAH1.fall   (& MAH1f);
-    MAH2.rise   (& MAH2r);
-    MAH2.fall   (& MAH2f);
-    MAH3.rise   (& MAH3r);
-    MAH3.fall   (& MAH3f);
-*/
+
     MAH1.mode   (PullUp);
     MAH2.mode   (PullUp);
     MAH3.mode   (PullUp);
     MBH1.mode   (PullUp);
     MBH2.mode   (PullUp);
     MBH3.mode   (PullUp);
+    Servo1_i.mode   (PullUp);
+    Servo2_i.mode   (PullUp);
+
     pc.printf   ("\tAbandon Hope %d\r\n", LED ? 0 : 1);
     //  Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
     tick_vread.attach_us    (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US);    //  Start periodic interrupt generator
@@ -774,8 +726,8 @@
     char    buff[TXTBUFSIZ];
     bool    eerom_detected = false;
     pc.baud     (9600);
-    com3.baud   (9600);
-    com2.baud   (9600);
+    com3.baud   (1200);
+    com2.baud   (19200);
     
     pc.printf   ("RAM test - ");
     if  (check_24LC64() != 0xa0)  //  searches for i2c devices, returns address of highest found
@@ -801,19 +753,26 @@
 //    MotPtr[0]->set_mode (REGENBRAKE);
     MotorA.set_mode (REGENBRAKE);
     MotorB.set_mode (REGENBRAKE);
-//    MotorA.set_mode (FORWARD);
-//    MotorB.set_mode (FORWARD);
     MotorA.set_V_limit  (0.9);
     MotorB.set_V_limit  (0.9); 
     MotorA.set_I_limit  (0.5);
     MotorB.set_I_limit  (0.5);
-
+    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 !
+    Servo   Servo1  (PB_8)  ;
+    Servos[0] = & Servo1;
+    Servo   Servo2  (PB_9)  ;
+    Servos[1] = & Servo2;
+/*
     //  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
@@ -829,6 +788,12 @@
             break;
     }
     */
+    MotorA.set_mode (FORWARD);
+    MotorB.set_mode (REVERSE);
+    MotorA.set_V_limit  (0.2);
+    MotorB.set_V_limit  (0.2); 
+    MotorA.set_I_limit  (0.5);
+    MotorB.set_I_limit  (0.5);
 
     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
@@ -843,15 +808,17 @@
         if  (flag_8Hz)  {   //  do slower stuff
             flag_8Hz    = false;
             LED = !LED;                   // Toggle LED on board, should be seen to fast flash
-            j++;
-            if  (j > 6)    {   //  Send some status info out of serial port every second and a bit or thereabouts
-                j = 0;
+            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 ();
             MotorB.current_calc ();
+//            Apps += Bpps;   //  to kill compiler warning
 //                pc.printf   ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IA %d, IB %d, Arpm %d, Brpm %d\r\n", Read_BatteryVolts(), Read_DriverPot(), MotorA.read_Halls  (), MotorB.read_Halls  (), MotorA.I.ave, MotorB.I.ave, (Apps * 60) / 24, (Bpps * 60) / 24);
-                //pc.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);
-                pc.printf   ("\tAangle_cnt %d\tAencoder_error_cnt %d", MotorA.angle_cnt, MotorA.encoder_error_cnt);
-                pc.printf   ("\tBangle_cnt %d\tBencoder_error_cnt %d, J3 %d\r\n", MotorB.angle_cnt, MotorB.encoder_error_cnt, J3 == 0 ? 0 : 1);
+                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);
+//                pc.printf   ("\tAangle_cnt %d\tAencoder_error_cnt %d", MotorA.angle_cnt, MotorA.encoder_error_cnt);
+//                pc.printf   ("\tBangle_cnt %d\tBencoder_error_cnt %d, J3 %d\r\n", MotorB.angle_cnt, MotorB.encoder_error_cnt, J3 == 0 ? 0 : 1);
+//                com2.printf ("RCI1 pw %d, RCI2 pw %d, 1per %d, 2per %d\r\n", RCI1.pulsewidth(), RCI2.pulsewidth(), RCI1.period(), RCI2.period());
             }
         }   //  End of if(flag_8Hz)
     }       //  End of main programme loop