Diff: main.cpp

Revision:

3:7aaf0072cc22

Parent:

2:b3c668ec43ac

--- a/main.cpp	Thu Feb 20 09:27:18 2014 +0000
+++ b/main.cpp	Fri Mar 14 14:14:55 2014 +0000
@@ -2,51 +2,62 @@
 #include "rtos.h"
 #include "MODSERIAL.h"
 #include "cnc.h"
-using namespace std;
+extern  void    i2c_handler    (void const *);
 extern  void    command_line_interpreter    (void const *) ;
-//extern  void    lissajous   (void const *)  ;
-extern  double  feed_rate, spindle_rpm;
-extern  void    more_setup  ()  ;
-extern  struct  Gparams    last_position;
+extern  fl_typ  feed_rate;      //  float type is 'float'
+extern  signed long spindle_rpm;
 
 const   int BAUD = 38400;
-MODSERIAL  pc(USBTX, USBRX);        //  tx, rx to pc
-//Serial  pc(USBTX, USBRX);        //  tx, rx to pc
-Ticker  NCO_gen;                 // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz)
+MODSERIAL  pc(USBTX, USBRX);    //  tx, rx to pc via usb lead
+Ticker  NCO_gen;                // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz)
+Ticker  msec;                   //  Ticker updating global millisecs counter
+
+bool    running         = false,
+        new_run_pending = false,
+        idle            = false,
+        move_ended      = false;
 
-bool    running     = false,    new_run_pending = false,    idle = false,   move_ended = false;
-volatile unsigned long ticks    = 0L;  //  32 bit count of "interrupt_period_us" interrupts from time t=0
-unsigned long          tickrun  = 0L;  //  32 bit effectively stores time in future when current movement to stop
-unsigned long          ticks_next  = 0L;  //  32 bit effectively stores time in future when current movement to stop
-unsigned long          millisecs = 0L;        //  32 bit
-unsigned long   pir_a = 0L,         //  Phase Increment Registers
-                pir_x = 0L,
-                pir_y = 0L,
-                pir_z = 0L,
-                pir_a_next  = 0L,   //  Data for next move assembled here
-                pir_x_next  = 0L,   //  during a move.
-                pir_y_next  = 0L,   //  This way, next move can start immediately
-                pir_z_next  = 0L,   //  on end of current move - minimised jerking
-                pir_spin    = 0L;     //  Referenced only in command_interpreter as spindle speed setting
+volatile unsigned long  ticks    = 0L;  //  32 bit count of "interrupt_period_us" interrupts from time t=0
+unsigned long           tickrun  = 0L,  //  32 bit effectively stores time in future when current movement to stop
+                        ticks_next  = 0L,  //  32 bit effectively stores time in future when current movement to stop
+                        millisecs = 0L;        //  32 bit
+signed long
+#if defined Fourth_Axis
+        pir_a_next  = 0L,   //  Data for next move assembled here
+#endif
+        pir_x_next  = 0L,   //  during a move.
+        pir_y_next  = 0L,   //  This way, next move can start immediately
+        pir_z_next  = 0L,   //  on end of current move - minimised jerking
+#if defined Fourth_Axis
+        inc_a_next  = 1L,
+#endif
+        inc_x_next  = 1L,
+        inc_y_next  = 1L,
+        inc_z_next  = 1L,
 
+        dir_bits_next   = 0L,
+        pir_spin    = 0L;     //  Referenced only in command_interpreter as spindle speed setting
+
+struct  Gparams    last_position;   //
 
 
 #if defined (TARGET_KL25Z)
-    const char Target[] = "KL25Z";
-    DigitalOut intled               (PTE1);     //J2p20
-    DigitalOut charge_pumpD25pin1   (PTE0);     //J2p18    
+    const char Target[] = "KL25Z";  //  Note need PTE0 (sda) and PTE1 (scl)
+    DigitalOut intled               (PTD7); //(PTE1);     //J2p19, was 20
+    DigitalOut charge_pumpD25pin1   (PTD6); //(PTE0);     //J2p17, was 18    
 //    InterruptIn     D25pin10_EStop    (PTE20);    //j10p1   KL25 J10 is KL46 j4
     DigitalIn     D25pin10_EStop    (PTE20);    //j10p1   KL25 J10 is KL46 j4
     DigitalIn       D25pin11_XLim     (PTE21);    //j10p3
     DigitalIn       D25pin12_YLim     (PTE22);    //j10p5
     DigitalIn       D25pin13_ZLim     (PTE23);    //j10p7
     DigitalIn       D25pin15_unkn     (PTE30);    //j10p11
-    //SPISlave spidevice(PTD3, PTD2, PTD1, PTD0); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC)
-    SPISlave spidevice(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC)
+#if defined I2C_Enable
+    I2CSlave slave(PTE0, PTE1); //  PTE0 sda, (yellow) PTE1 scl (blue)
+#endif
+#if defined SPI_Enable
+    SPISlave spidevice(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk (uses p11, p12, p13 on mbed LPC1768)
+#endif
     //                 J2p08,J2p10,J2p12, J2p06
-    //SPI spidevice(PTD2, PTD3, PTD1); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC)
-    //SPI spidevice(PTD3, PTD2, PTD1); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC)
-    //NOTE doubt possibly miso mosi in wrong order here, PTD3 and PTD2
     #define STEPPER_PORT    PortC
     const   int PortBitXSt   = 3,    //  Port bit num X Step    J1P05   D25pin 2
                 PortBitXDi   = 4,    //  Port bit num X Dir     J1P07   D25pin 3
@@ -74,7 +85,12 @@
     DigitalIn       D25pin12_YLim     (PTE22);  // j4p5 checked
     DigitalIn       D25pin13_ZLim     (PTE23);  // j4p7 checked
     DigitalIn       D25pin15_unkn     (PTE30);  // j4p11 checked
+#if defined I2C_Enable
+    I2CSlave slave(p9, p10);
+#endif
+#if defined SPI_Enable
     SPISlave spidevice(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC)
+#endif
     //                 J2p13, J2p15, J2p11, J2p09
                 // Easy way to allocate port bits for
                 // output of stepper motor Step and DIR sigs
@@ -93,15 +109,20 @@
 #if defined (TARGET_MBED_LPC1768)
     const char Target[] = "MBED LPC1768";
     DigitalOut intled(LED2);                    //  Correct
-    DigitalOut charge_pumpD25pin1      (p25);    //J2p18    Following 5 inputs all wrong - TO DO sort which pins
+    DigitalOut charge_pumpD25pin1      (p25);    //
 //    InterruptIn D25pin10_EStop  (p26);    //P2.0
     DigitalIn D25pin10_EStop  (p26);    //P2.0
     DigitalIn   D25pin11_XLim   (p24);    //P2.2
     DigitalIn   D25pin12_YLim   (p23);    //P2.3
     DigitalIn   D25pin13_ZLim   (p19);    //P1.30
     DigitalIn   D25pin15_unkn   (p20);    //P1.31
+#if defined I2C_Enable
+    I2CSlave slave(p9, p10);
+#endif
+#if defined SPI_Enable
     SPISlave spidevice(p5, p6, p7, p8);
-                // Easy way to allocate port bits for           ***  N O T  CHECKED for MBED_LPC1768 ***
+#endif
+                // Easy way to allocate port bits
                 // output of stepper motor Step and DIR sigs
     #define STEPPER_PORT    Port0
     /* Port 0 bits routed to DIP pins as follows:-
@@ -133,29 +154,37 @@
 #endif
 
 const   long    //  Assemble mask bits from now known port bit positions
-            XSt =   1 << PortBitXSt,    //  X axis Step signal
-            XDi =   1 << PortBitXDi,    //  X axis Direction signal
-            YSt =   1 << PortBitYSt,    //  Y axis Step, etc
-            YDi =   1 << PortBitYDi,
-            ZSt =   1 << PortBitZSt,    //  Z axis
-            ZDi =   1 << PortBitZDi,
-            ASt =   1 << PortBitASt,    //  A axis, not implemented in full, for e.g. rotary axis
-            ADi =   1 << PortBitADi,
-            SDi =  1 << PortBitSDi,     //  Spindle, also driven by Step and Dir signals up to 5kHz
-            SSt =  1 << PortBitSSt,     //  for 5000 RPM
+        XSt =   1 << PortBitXSt,    //  X axis Step signal
+        XDi =   1 << PortBitXDi,    //  X axis Direction signal
+        YSt =   1 << PortBitYSt,    //  Y axis Step, etc
+        YDi =   1 << PortBitYDi,
+        ZSt =   1 << PortBitZSt,    //  Z axis
+        ZDi =   1 << PortBitZDi,
+        ASt =   1 << PortBitASt,    //  A axis, not implemented in full, for e.g. rotary axis
+        ADi =   1 << PortBitADi,
+        SDi =   1 << PortBitSDi,     //  Spindle, also driven by Step and Dir signals up to 5kHz
+        SSt =   1 << PortBitSSt,     //  for 5000 RPM
 
-            SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt);
+        SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt),
+        direction_swappers = XDi | YDi | ZDi | SDi; //  include bit to swap direction
 
     PortOut Steppers    (STEPPER_PORT, SM_MASK);
-    const   long    direction_swappers = XDi | YDi | ZDi | SDi; //  include bit to swap direction
-/*
-    long    read    ()  {
-        return  mysteppers ^ direction_swappers;
-    }
-    void    write   (long val)  {
-        mysteppers  = val ^ direction_swappers;
-    }
-  */  
+
+void    target_cmd (struct singleGparam * a)   {
+    pc.printf("Computer is %s\r\n", Target);
+}
+
+void    grain_clr   (struct singleGparam & g)  {
+    g.flt = 0.0;
+    g.ul = 0L;
+    g.i = g.c = 0;
+    g.changed = false;
+}
+void    Gparams_clr    (struct Gparams & p)   {
+    grain_clr   (p.x);    grain_clr   (p.y);    grain_clr   (p.z);    grain_clr   (p.i);    grain_clr   (p.j);
+    grain_clr   (p.r);    grain_clr   (p.a);    grain_clr   (p.b);    grain_clr   (p.c);    grain_clr   (p.d);
+}
+
 class digital_readout_stuff  {   //  class does not need to be named here
     private:
     char *  readout (char * txt, long p)         //  p has running subtotal of all pulses issued to stepper driver
@@ -184,16 +213,17 @@
     }
         public:
     signed long x, y, z, a;    //  Could easily expand up to six or more dros
-    bool    dro_output;             //  To enabe / disable output to terminal
+//    bool    dro_output;             //  To enabe / disable output to terminal
     void    init    ()  {
         x = y = z = a = 0;   //  These dro registers count pulses delivered to stepper motor driver
-        dro_output = true;
+//        dro_output = true;
     }
     void    update  ()  {
         static  long    t = 300;    //  Prevent display immediately upon startup
         if  (millisecs < t)
             return;
-        if(!idle && dro_output)  {
+//        if(!idle && dro_output)  {
+        if(!idle)  {
             char    txt[12];
             pc.printf("dros X %s,", readout(txt, x));    //  dro.n has running subtotal of all pulses issued to stepper driver.n
             pc.printf(" Y %s, Z ", readout(txt, y));
@@ -202,8 +232,17 @@
             t = millisecs + 350;    //  Schedule next update after this non-blocking delay
         }
     }
-}   dro_out ;
+}   dro_out ;   //  single instance of class digital_readout_stuff
 
+/**
+class   circbuff    {   public functions
+    void    init    ()  {
+    int     On_Q    ()  {
+    bool    readable    ()  {return !buffempty; }
+    bool    writeable   ()  {return !bufffull; }
+    bool    read    (pirbufgrain & g)   {
+    bool    write   (pirbufgrain & g)   {
+*/
 const int PIRBUFFSIZE = 40; //  pirbufgrain are 40 bytes each
 class   circbuff    {
     private:
@@ -216,10 +255,10 @@
         buffempty = true;
     }
     void    grain_copy  (pirbufgrain & src, pirbufgrain & dest) {
-        dest.x = src.x; 
-        dest.y = src.y; 
-        dest.z = src.z; 
-        dest.c = src.c; 
+        dest.x  = src.x; 
+        dest.y  = src.y; 
+        dest.z  = src.z; 
+        dest.distance_code = src.distance_code; 
         dest.f_rate = src.f_rate;   //  int feed rate mm per min * 1000
     }
     public:
@@ -261,167 +300,79 @@
             bufffull = true;
         return  true;
     }
-}   CircBuff;
+}   CircBuff;   //  single instance of class   circbuff
     
-int PutMoveOnList   (struct pirbufgrain & s)    {
-    while   (!CircBuff.writeable())
-        osThreadYield();
-    CircBuff.write  (s);    //    pc.printf("CircBuff, contains %d\r\n", CircBuff.On_Q());
-    return  0;
-}
 
-const   double  duration_multiplier =   60000000.0 /  interrupt_period_us;
-
+/**
 void    move_to_XYZ   (struct pirbufgrain & ins)   {
+    Takes floating point x, y, z and feed_rate as input.
+    Finds distances from 'last_position' global,
+    copies structure containing floating point values for x, y, z, distance_multiplier and feed_rate
+    onto a circular buffer.
+    If buffer full, executes 'osThreadYield()' until space is created on buffer
+*/
+void    move_to_XYZ   (struct pirbufgrain & ins)   {
+static const   fl_typ  duration_multiplier =   60000000.0 /  interrupt_period_us;
     struct  pirbufgrain outs;
-    double  distx   = ins.x - last_position.x.dbl,
-            disty   = ins.y - last_position.y.dbl,
-            distz   = ins.z - last_position.z.dbl,
+    fl_typ  distx   = ins.x - last_position.x.flt,
+            disty   = ins.y - last_position.y.flt,
+            distz   = ins.z - last_position.z.flt,
             distT   = sqrt ((distx * distx) + (disty * disty) + (distz * distz)), // 3D Pythag !
             temp    = n_for_onemmpermin / distT;
     if  (distT < 0.01)  {
         pc.printf("Very small move %.4f, Ignoring!\r\n", distT);
-        return;
+        return;     //  Return without updating last_position as it was not changed
     }
-    last_position.x.dbl   = ins.x;     //  Update global last_position record
-    last_position.y.dbl   = ins.y;
-    last_position.z.dbl   = ins.z;
+    last_position.x.flt   = ins.x;     //  Update global last_position record
+    last_position.y.flt   = ins.y;
+    last_position.z.flt   = ins.z;
     outs.f_rate = ins.f_rate;
-    outs.c  = duration_multiplier * distT;   //  Duration ticks subject to feed rate compo
+    outs.distance_code  = duration_multiplier * distT;   //  Duration ticks subject to feed rate compo
     outs.x = temp * distx;
     outs.y = temp * disty;
     outs.z = temp * distz;  //  Have assembled data ready to put onto queue of move instructions
-    PutMoveOnList   (outs);
+    while   (!CircBuff.writeable())
+        osThreadYield();
+    CircBuff.write  (outs);    //    Move details put on circular buffer
+}
+
+/**
+*   Interrupt Service Routines
+void    millisec_update_ISR ()  { self explanatory
+
+void    Numerically_Controlled_Oscillators_ISR ()  {   
+    services Ticker 'NCO_gen' generated interrupts ***ISR***
+    Does all of the stepper motor driving.
+    At end of movement, fetches and starts next move to run from circular buffer
+    If nothing buffered, stops x, y and z, leaves spindle unaltered
+*/
+void    millisec_update_ISR ()  {
+    millisecs++;
 }
 
-void    target_cmd (struct singleGparam * a)   {
-    pc.printf("Computer is %s\r\n", Target);
-}
-/*
-*   Interrupt Service Routine
-*/
-/*  **** UNBROKEN VERSION 6th Feb 2014 ******************
+#define STEP_IDLE_HI    //  Choose IDLE_HI or LO to suit any power save function of stepper motor drive units
+//#define STEP_IDLE_LO
 void    Numerically_Controlled_Oscillators_ISR ()  {   // services Ticker 'NCO_gen' generated interrupts ***ISR***
-    static  const   int millisec_countdown = 1000 / interrupt_period_us;
-    const   long    bit_lutx[4] = {XSt0 | XDi0, XSt0 | XDi1, XSt1 | XDi1, XSt1 | XDi0},  //  Used to look-up 'clk' and 'dir' signals from accum MSBs
-                    bit_luty[4] = {YSt0 | YDi0, YSt0 | YDi1, YSt1 | YDi1, YSt1 | YDi0},  //  Used to look-up 'clk' and 'dir' signals from accum MSBs
-                    bit_lutz[4] = {ZSt0 | ZDi0, ZSt0 | ZDi1, ZSt1 | ZDi1, ZSt1 | ZDi0},  //  Used to look-up 'clk' and 'dir' signals from accum MSBs
-                    bit_luta[4] = {ASt0 | ADi0, ASt0 | ADi1, ASt1 | ADi1, ASt1 | ADi0},  //  Used to look-up 'clk' and 'dir' signals from accum MSBs
-                    bits2shift  = (sizeof (long) << 3) - 2,
-    static  unsigned long
-//        acc_s = 0L, //  For Spindle motor, probably not needed as may be pwm
-        acc_a = 0L,
-        acc_x = 0L,
-        acc_y = 0L,
-        acc_z = 0L;
-    static  int obitz = 0, mscount = millisec_countdown;
-    int oldbitz, acts;
+    static  const   long    step_mask   = ASt | XSt | YSt | ZSt,    //  Added 6th Feb 14 Mask Does NOT include spindle bits
+                            dir_mask    = ADi | XDi | YDi | ZDi;    //  Added 6th Feb 14 Mask Does NOT include spindle bits
+    static  signed  long     //  27 Feb 14 changed from unsigned
+#if defined Fourth_Axis
+        acc_a   = 0L,        pir_a   = 0L,
+#endif
+        acc_x       = 0L,   //  acc Accumuloators
+        pir_x       = 0L,   //  pir Phase Increment Registers
+        acc_y       = 0L,        pir_y   = 0L,
+        acc_z       = 0L,        pir_z   = 0L,
+        acc_spin    = 0L,  //  separate acc for spindle rotation NCO
+        inc_x       = 1L,   //  inc_x, y, z for updating DRO registers
+        inc_y       = 1L,        inc_z   = 1L,
+        dir_bits    = 0L,  //  direction flags for up to four axes
+        oldSteps    = 0L;  //  
+    long tmp, newSteps = 0L;
 
     intled = 1;     //  LED on for duration of interrupt service - point for scope probing
-    ticks++;        //  count of interrupts serviced
-    if(!--mscount)  {   //  Maintain global counter of elapsed milli seconds
-        mscount = millisec_countdown;
-        millisecs++;
-    }
-    if  (running)   {
-        acc_x += pir_x;     //  Update phase of signals in accumulators
-        acc_y += pir_y;
-        acc_z += pir_z;
-        acc_a += pir_a;   //  not yet implemented
-    //    acc_s += pir_s;   //  pir_s used for spindle speed
-        oldbitz = obitz;    //  pin output levels as determined during previous interrut
-        obitz = bit_lutx[acc_x >> bits2shift] | bit_luty[acc_y >> bits2shift] | bit_lutz[acc_z >> bits2shift] | bit_luta[acc_a >> bits2shift];
-    
-        mysteppers = obitz; //  Output signals to stepper motor drivers, next look for _- pos clk events on 'Step' outputs
-    
-        acts = (~oldbitz & obitz);  //  get pos clk edge triggers 'Step' bits
-        acts |= (obitz & (XDi1 | YDi1 | ZDi1));      //  get axis X, Y and Z Direction bits
-        if(acts & XSt1)    {           //  got pos clk edge for axis X
-            if  (acts & XDi1)
-                dro.x++;
-            else    dro.x--;
-        }
-        if(acts & YSt1)    {           //  got pos clk edge for axis Y
-            if  (acts & YDi1)
-                dro.y++;
-            else    dro.y--;
-        }
-        if(acts & ZSt1)   {           //  got pos clk edge for axis Z
-            if  (acts & ZDi1)
-                dro.z++;
-            else    dro.z--;
-        }
-        if  (tickrun <= ticks)   {   //  End of a machine movement detected, start next move here if possible
-            running = false;
-            move_ended = true;
-            pir_x = 0L; //  stop all stepper motors
-            pir_y = 0L;
-            pir_z = 0L;
-            pir_a = 0L;
-//          ticks = 0L; //  Simply to avoid having to think about overflow problems
-        }
-    }
-    else    {   //  Not running. Grab next data here when or if available
-        if  (new_run_pending)  {        //  Pick up on flag set elsewhere
-            pir_a   = pir_a_next;
-            pir_x   = pir_x_next;
-            pir_y   = pir_y_next;
-            pir_z   = pir_z_next;
-            tickrun = ticks + ticks_next;
-            running = true;     //  Start the new run
-            new_run_pending = false;    //  Clear the flag which initiated this update
-            idle = false;
-        }
-    }
-    charge_pump = ticks & 0x02;
-    intled = 0;         //  LED off
-}           //  end of interrupt handler
-*/
-/*
-*   End of Interrupt Service Routine
-*/
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-/*
-*   Interrupt Service Routine
-*/
-#define STEP_IDLE_HI
-//#define STEP_IDLE_LO
-void    Numerically_Controlled_Oscillators_ISR ()  {   // services Ticker 'NCO_gen' generated interrupts ***ISR***
-    static  const   int millisec_countdown = 1000 / interrupt_period_us;
-    const   long    step_mask   = ASt | XSt | YSt | ZSt,    //  Added 6th Feb 14 Mask Does NOT include spindle bits
-                    dir_mask    = ADi | XDi | YDi | ZDi;    //  Added 6th Feb 14 Mask Does NOT include spindle bits
-    static  unsigned long
-        acc_spin = 0L,
-        acc_a = 0L,
-        acc_x = 0L,
-        acc_y = 0L,
-        acc_z = 0L;
-    static  long    mscount = millisec_countdown;
-    static  long    dir_bits = 0L, oldSteps = 0L;   //  Added 6th Feb 14
-    long acts, tmp, newSteps, stbits;
-
-    intled = 1;     //  LED on for duration of interrupt service - point for scope probing
-    ticks++;        //  count of interrupts serviced
-    if(!--mscount)  {   //  Maintain global counter of elapsed milli seconds
-        mscount = millisec_countdown;
-        millisecs++;
-    }
-    acc_spin += pir_spin;
+    ticks++;        //  count of interrupts serviced, vital to time end of movement
+    charge_pumpD25pin1 = ticks & 0x01;  //  Can use 0x01 or 0x02 here to alter charge pump freq
     tmp = Steppers ^ direction_swappers;
 #if defined STEP_IDLE_LO
     tmp &= ~step_mask;   //  Step bits prepared for idle lo
@@ -429,96 +380,62 @@
 #if defined STEP_IDLE_HI
     tmp |= step_mask;   //  Step bits prepared for idle hi
 #endif
-    if  (acc_spin & 0x80000000) {   tmp |= SSt;    }
-    else    {                       tmp &= ~SSt;   }
-//    mysteppers = tmp;
-    if  (running)   {
-        acc_x += pir_x;     //  Update phase of signals in accumulators
-        acc_y += pir_y;
-        acc_z += pir_z;
-        acc_a += pir_a;   //  not yet implemented
-        
-        newSteps = 0L;   //  Added 6th Feb 14
-        if  (acc_a & 0x80000000)    newSteps |= ASt;//  Added 6th Feb 14
-        if  (acc_x & 0x80000000)    newSteps |= XSt;//  Added 6th Feb 14
-        if  (acc_y & 0x80000000)    newSteps |= YSt;//  Added 6th Feb 14
-        if  (acc_z & 0x80000000)    newSteps |= ZSt;//  Added 6th Feb 14
-        stbits  = newSteps ^ oldSteps;  //  Any bit of stbits set to initiate a Step pulse
-        acts     = dir_bits | stbits; //
-        oldSteps    = newSteps;   //  Added 6th Feb 14
-//        tmp         = acts ^ step_mask;   //  Invert clock - Arc Euro stepp motor driver only goes into half current mode this way
-        tmp ^= stbits;
+    acc_spin += pir_spin;   //  Spindle NCO
+    if  (acc_spin < 0)  tmp |= SSt;
+    else                tmp &= ~SSt;
+    if  (!running)      Steppers = tmp ^ direction_swappers;    //  Axes not moving, spindle may be turning or not
+    else    {   //  running == true, Further manipulation of tmp follows, prior to rewriting to 'Steppers' IO Port
+//        newSteps = 0L;   //  Added 6th Feb 14
+#if defined Fourth_Axis
+        acc_a   += pir_a;
+        if  (acc_a < 0)    newSteps |= ASt;//  Added 6th Feb 14
+#endif
+        acc_x   += pir_x;     //  Update phase of signals in accumulators
+        if  (acc_x < 0)    newSteps |= XSt;//  Added 6th Feb 14
+        acc_y   += pir_y;
+        if  (acc_y < 0)    newSteps |= YSt;//  Added 6th Feb 14
+        acc_z   += pir_z;
+        if  (acc_z < 0)    newSteps |= ZSt;//  Added 6th Feb 14
+        //  newSteps has copy of all 4 'acc' MSBs shifted into port bit positions
+        oldSteps    ^= newSteps;  //  Any bit of stbits set to initiate a Step pulse
+        tmp         ^= oldSteps;
         Steppers = tmp ^ direction_swappers; //  Output signals to stepper motor drivers, next update dros from 'clocked' bits CLOCK IDLES HIGH
-
-        if(acts & XSt)  {       //  got clk edge for axis X
-            if  (acts & XDi)
-                dro_out.x--;
-            else    dro_out.x++;
-        }
-        if(acts & YSt)  {       //  got clk edge for axis Y
-            if  (acts & YDi)
-                dro_out.y--;
-            else    dro_out.y++;
-        }
-        if(acts & ZSt)  {       //  got clk edge for axis Z
-            if  (acts & ZDi)
-                dro_out.z--;
-            else    dro_out.z++;
-        }
-        if  (tickrun <= ticks)   {   //  End of a machine movement detected, start next move here if possible
-            if  (new_run_pending)  {
-                pir_a   = pir_a_next;
-                pir_x   = pir_x_next;
-                pir_y   = pir_y_next;
-                pir_z   = pir_z_next;
-                dir_bits = 0;           //  Added 6th Feb 14
-                if  (pir_a & 0x80000000)    dir_bits |= ADi;//  Added 6th Feb 14 read sign bits
-                if  (pir_x & 0x80000000)    dir_bits |= XDi;//  Added 6th Feb 14
-                if  (pir_y & 0x80000000)    dir_bits |= YDi;//  Added 6th Feb 14
-                if  (pir_z & 0x80000000)    dir_bits |= ZDi;//  Added 6th Feb 14
-                acts = Steppers ^ direction_swappers;  //  read output lines
-                acts &= ~dir_mask;
-                acts |= dir_bits;
-                Steppers = acts ^ direction_swappers;
-                tickrun = ticks + ticks_next;
-                running = true;     //  Start the new run
-                new_run_pending = false;    //  Clear the flag which initiated this update
-                idle = false;
-            }           //  End of if   (new_run_pending)   {
-            else    {   //  End of machine movement AND no new_run_pending
+        if(oldSteps & XSt)  dro_out.x += inc_x;       //  got clk edge for axis X
+        if(oldSteps & YSt)  dro_out.y += inc_y;       //  got clk edge for axis Y
+        if(oldSteps & ZSt)  dro_out.z += inc_z;       //  got clk edge for axis Z
+        oldSteps    = newSteps;   //  Added 6th Feb 14
+        if  (tickrun <= ticks & !new_run_pending)   {   //  End of a machine movement detected, start next move here if possible
                 running = false;
                 move_ended = true;
                 pir_x = 0L; //  stop all stepper motors
                 pir_y = 0L;
                 pir_z = 0L;
+#if defined Fourth_Axis
                 pir_a = 0L;
+#endif
     //          ticks = 0L; //  Simply to avoid having to think about overflow problems
-            }
-        }
-    }
-    else    {   //  Not running. Grab next data here when or if available
+        }       //  end of if   (tickrun <= ticks)  {
+    }           //  end of else is   (running)   {
+    if  (!running & new_run_pending)  { //  Start axis movement
+        dir_bits= dir_bits_next;
+#if defined Fourth_Axis
+        pir_a   = pir_a_next;
+#endif
+        pir_x   = pir_x_next;
+        pir_y   = pir_y_next;
+        pir_z   = pir_z_next;
+        inc_x   = inc_x_next;
+        inc_y   = inc_y_next;
+        inc_z   = inc_z_next;
+        tmp     = Steppers ^ direction_swappers;  //  read output lines
+        tmp     &= ~dir_mask;
+        tmp     |= dir_bits;
         Steppers = tmp ^ direction_swappers;
-        if  (new_run_pending)  {        //  Pick up on flag set elsewhere
-            pir_a   = pir_a_next;
-            pir_x   = pir_x_next;
-            pir_y   = pir_y_next;
-            pir_z   = pir_z_next;
-            dir_bits = 0;           //  Added 6th Feb 14
-            if  (pir_a & 0x80000000)    dir_bits |= ADi;//  Added 6th Feb 14 read sign bits
-            if  (pir_x & 0x80000000)    dir_bits |= XDi;//  Added 6th Feb 14
-            if  (pir_y & 0x80000000)    dir_bits |= YDi;//  Added 6th Feb 14
-            if  (pir_z & 0x80000000)    dir_bits |= ZDi;//  Added 6th Feb 14
-            acts = Steppers ^ direction_swappers;  //  read output lines
-            acts &= ~dir_mask;
-            acts |= dir_bits;
-            Steppers = acts ^ direction_swappers;
-            tickrun = ticks + ticks_next;
-            running = true;     //  Start the new run
-            new_run_pending = false;    //  Clear the flag which initiated this update
-            idle = false;
-        }   //  end of if  (new_run_pending)  {        //  Pick up on flag set elsewhere
+        tickrun = ticks + ticks_next;
+        running = true;     //  Start the new run
+        new_run_pending = false;    //  Clear the flag which initiated this update
+        idle = false;
     }       //  end of     else    {   //  Not running. Grab next data here when or if available
-    charge_pumpD25pin1 = ticks & 0x01;  //  Can use 0x01 or 0x02 here to alter charge pump freq
     intled = 0;         //  LED off
 }           //  end of interrupt handler
 
@@ -526,47 +443,33 @@
 *   End of Interrupt Service Routine
 */
 bool    spindle_running ()  {
-    return  pir_spin;
+    if  (pir_spin == 0) return  false;
+    return  true;
 }
-void    spindle_control (double ss)  {
+void    spindle_control (signed long ss)  {
     long t, p;
-    pir_spin = (signed long) (ss * spindle_factor);
+    pir_spin = ss * spindle_factor;
     t = ticks;
     while   (t == ticks)    {}  //  wait until just after next interrupt
     p = Steppers ^ direction_swappers;
-    if  (pir_spin & 0x80000000)
-        p |= SDi;
-    else
-        p &= ~SDi;
+    if  (pir_spin & 0x80000000) p |= SDi;
+    else                        p &= ~SDi;
     Steppers = p ^ direction_swappers;
-    pc.printf("Done setting spindle speed %.0f, delay = %d\r\n", ss, ticks - t);
+    pc.printf("Done setting spindle speed %d, pir_spin %d, delay = %d\r\n", ss, pir_spin, ticks - t);
 }
 
-
-/*
-void    scmd (struct singleGparam * a)   {
-    long t, p;
-    if  (fabs(a[1].dbl) > spindle_max)    {
-        pc.printf   ("Errror setting spindle RPM, can't set to %.0f, ignoring request\r\n", a[1].dbl);
-        return;
-    }
-    pc.printf   ("Setting spindle RPM to %.0f Can set Pos or Neg for fwd/rev\r\n", a[1].dbl);
-    pir_spin = (signed long) (a[1].dbl * spindle_factor);
-    t = ticks;
-    while   (t == ticks)    {}  //  wait until just after next interrupt
-    p = mysteppers;
-    if  (pir_spin & 0x80000000)
-        p |= SDi;
-    else
-        p &= ~SDi;
-    mysteppers = p;
-    pc.printf("Done setting spindle, delay = %d", ticks - t);
-}
+/**
+void    pir_updater_task (void const * name)   {
+    A task handed to the RTOS Round Robin
+    ISR controls certain flags.
+    This task responds after 'move_ended' flag asserted by NCO ISR
+    If CircBuff not empty, set of floating point data for next point is fetched,
+    remaining calcs performed and translated into signed longs, results placed in
+    set of '_next' locations to be picked up by future NCO ISR
 */
-void    newpir_updater (void const * name)   {
+void    pir_updater_task (void const * name)   {
     static  long    x, y, z;//, count = 0;
     struct pirbufgrain outs;
-//    pc.printf("Arrived at newpir_updater\r\n");
     while   (true)  {
 //        while   (!move_ended  || !CircBuff.readable())   { ** TO DO ** Solve problem with empty flag
         while   (!move_ended  || CircBuff.On_Q() == 0)   {
@@ -576,16 +479,28 @@
         x = (long)(outs.f_rate * outs.x);  //  These take much CPU time !!
         y = (long)(outs.f_rate * outs.y);
         z = (long)(outs.f_rate * outs.z);
-        ticks_next = (unsigned long)(outs.c / outs.f_rate);
+        ticks_next = (unsigned long)(outs.distance_code / outs.f_rate);
         pir_x_next = x;
         pir_y_next = y;
         pir_z_next = z;
+        dir_bits_next = 0;
+        //if  (pir_a & 0x80000000)    dir_bits_next |= ADi;//  Added 6th Feb 14 read sign bits
+        inc_x_next = inc_y_next = inc_z_next = 1L;
+        if  (x < 0) {
+            dir_bits_next   |= XDi;
+            inc_x_next      = -1L;
+        }
+        if  (y < 0) {
+            dir_bits_next   |= YDi;
+            inc_y_next      = -1L;
+        }
+        if  (z < 0) {
+            dir_bits_next   |= ZDi;
+            inc_z_next      = -1L;
+        }
         move_ended = idle = false;
         new_run_pending = true; //  cleared and 'running' flag set in interrupt handler
-//        idle = false;
-//        count++;
-//        pc.printf("CircB tot %d\r\n", count);
-    }
+    }   //  end of while    (true)  {
 }
 
 
@@ -598,32 +513,6 @@
     pc.printf("On CircBuff %d\r\n", CircBuff.On_Q());
 }
 
-/*void    taskone    (void const * name)
-{
-    static int i = 0;
-    while   (true)  {
-        pc.printf("%s %d\r\n", name, i++);
-        Thread::wait(9500);
-        osThreadYield();
-    }
-}
-
-void    tasktwo    (void const * name)
-{
-        pc.printf("Task Two runs once and exits\r\n");
-        Thread::wait(700);
-        osThreadYield();
-}
-
-void    taskthree    (void const * name)
-{
-    static int i = 0;
-    while   (true)  {
-        pc.printf("%s %d\r\n", name, i++);
-        Thread::wait(3500);
-        osThreadYield();
-    }
-}*/
 /*
 #define ESTOP   0x100
 #define XLIM    1
@@ -658,10 +547,15 @@
 
 //void    Emergency_Stop_Interrupt  ()  {
 //    pc.printf("Emergency Stop Activated !!\r\n");
-//    spindle_control (0.0);  //  Stop spindle rotation
+//    spindle_control (0);  //  Stop spindle rotation
 //}
 
+/**
 int main() {
+
+*/
+int main() {
+    long    ins, ins_old, ins_changed = 0;
     pc.baud(BAUD); //  comms to 'PuTTY' serial terminal via mbed usb
 //    D25pin11_XLim.mode  (PullDown);   External resistors now fitted
 //    D25pin12_YLim.mode  (PullDown);
@@ -670,24 +564,31 @@
 //    D25pin10_EStop.mode (PullDown);
 //    D25pin10_EStop.rise (& Emergency_Stop_Interrupt);
 //    D25pin10_EStop.fall (& Emergency_Stop_Interrupt);
-    
-    more_setup  ()  ;   //  Zeros one 'pirs' structure 'last_position'
-    dro_out.init();
+
+    Gparams_clr     (last_position);
+    dro_out.init    ();
     CircBuff.init   ();
     Inputs_From_Machine.init    ();
+#if defined SPI_Enable
     spidevice.format(8, 0);    //  8 bits mode 0,  // p11 mosi, p12 miso, p13 sclk ** ONLY 8 BIT **
     spidevice.frequency(12000000);  //  12MHz bit rate
+#endif
     pc.printf("\r\n*\n*\nFound Computer %s\r\n", Target);
     pc.printf("Welcome to the CNC tester\r\nStep pulses required to move 1.0mm = %9.0f\r\n", pulses_per_mm);
-    pc.printf("PIR 'n' for 1mm per min = %9.0f\r\ntop speed = %6.1f mm per min\r\n\n", n_for_onemmpermin, max_mm_per_min);
-    NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, (long)interrupt_period_us);// Have setup timed interrupts, let other code deal
-//    Thread threadnametaskone   (taskone, (void *)"task one stuff");
-//    Thread t8   (tasktwo, (void *)"task two");
-    Thread tsr2   (newpir_updater, (void *)"read from CircBuff and move");
-//    Thread tthree   (taskthree, (void *)"task three");
-//    Thread patterngen (lissajous,   (void *)"Loading Lissajous")  ;
+    pc.printf("PIR 'n' for 1mm per min = %9.0f\r\ntop speed = %d mm per min\r\n\n", n_for_onemmpermin, feed_rate_max);
+
+
+//    NVIC_SetPriority(TIMER3_IRQn, 255); // set mbed tickers to lower priority than other things ONLY COMPILES FOR LPC1768
+
+
+    NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, interrupt_period_us);// Have setup timed interrupts, let other code deal
+    msec.attach_us(&millisec_update_ISR, 1001);
+
+    Thread tsr2   (pir_updater_task, (void *)"read from CircBuff and move");
     Thread comlin (command_line_interpreter,    (void *)"cli"); //  Read any instructions arriving via serial port and act upon them
-    long    ins, ins_old, ins_changed = 0;
+#if defined I2C_Enable
+    Thread i2cstuff (i2c_handler, (void *)"i2c thing");
+#endif
     ins = ins_old = Inputs_From_Machine.read    ();
     move_ended = true;  //  Needed to kickstart system
     
@@ -698,7 +599,6 @@
         ins_old = ins;
         if  (ins_changed)
             pc.printf("Inputs Have Changed 0x%x, read 0x%x\r\n", ins_changed, ins);
-//        pc.printf(".");
         osThreadYield();                //
     }   //  end of Round Robin loop
 }       //  end of int main()