Code to drive a CNC machine via a PC LPT port lookalike 25 pin 'D', experiment in 'PC/Mach3' replacement. Designed to compile and run on mbed LPC1768, Freescale KL25Z and Freescale KL46Z. Proved on LPC1768 and KL25Z, problem with serial port on KL46Z. Reads subset of 'G Codes' through usb/serial port and drives 3 stepper/servo drives for X, Y and Z, also similar Step/Dir outputs for spindle motor control. Emulates PC LPT, outputs 'charge pump', proved driving Seig KX3 CNC mill

Dependencies:   MODSERIAL mbed

Files at this revision

API Documentation at this revision

Comitter:
JonFreeman
Date:
Fri Mar 14 14:14:55 2014 +0000
Parent:
2:b3c668ec43ac
Commit message:
CNC Machine driver, emulates PC LPT port, docs are wip

Changed in this revision

arith.cpp Show diff for this revision Revisions of this file
cnc.h Show annotated file Show diff for this revision Revisions of this file
command_interpreter.cpp Show annotated file Show diff for this revision Revisions of this file
lissajous.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/arith.cpp	Thu Feb 20 09:27:18 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,37 +0,0 @@
-#include "mbed.h"
-#include "cnc.h"
-using namespace std;
-
-const   double  ball_screw_pitch_mm   =   4.0,    //  KX3 has 4mm ball screws
-                motor_step_per_rev  =   200.0,    //  KX3 has 200 step per rev steppers
-                micro_steps         =   10.0,      //  Arc Eurotrade choice 2,4,5,8,10,16,20,25,32,40,50,64,100,125,128
-                pulses_per_mm       =   micro_steps * motor_step_per_rev / ball_screw_pitch_mm,
-
-                interrupt_period_us =   32.0,//24.0, //16 is possible with Mbed LPC1768
-                interrupt_freq_Hz   =   1000000.0 / interrupt_period_us,  //  Serious limit when doing all in software, no real limit with FPGA
-                max_pulse_freq_Hz   =   interrupt_freq_Hz / 3.25,  //  strictly 2.0, but allow a good margin
-                max_mm_per_min      =   60.0 * max_pulse_freq_Hz / pulses_per_mm,
-//                n_for_onemmpermin   = pulses_per_mm * interrupt_period_us * pow(2.0,32) / 60000000.0, //  n pir to produce 1mm/min travel
-                n_for_onemmpermin   = pulses_per_mm * interrupt_period_us * pow(2.0,31) / 60000000.0, //  n pir to produce 1mm/min travel
-                //  note power reduced from 32to 31 as interrupt handler now issued step pulse on both edges
-                feed_rate_max       = max_mm_per_min,
-                spindle_factor      = interrupt_period_us * 4280,
-                spindle_max         = 5000.0;
-//The output frequency F<sub>out</sub> = 'Kernel Speed (Hz)' * n / (2 to the power of 32)
-    
-struct  Gparams    last_position;
-void    grain_clr   (struct singleGparam & g)  {
-    g.dbl = 0.0;
-    g.ul = 0L;
-    g.i = g.c = 0;
-    g.changed = false;
-}
-void    pirs_clr2    (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);
-}
-void    more_setup  ()  {
-    pirs_clr2    (last_position);
-}
-
-
--- a/cnc.h	Thu Feb 20 09:27:18 2014 +0000
+++ b/cnc.h	Fri Mar 14 14:14:55 2014 +0000
@@ -1,20 +1,61 @@
-#define ESTOP   0x100   //  bits used in input reading
+/**
+Programme   "cnc__machine_driver_3"
+Author      Jon Freeman
+
+Accepts subset of 'G Code' CNC machine instructions as input from USB serial
+Generates 'Step' and 'Direction' signals for X, Y and Z axes, signals suit
+standard stepper / servo drive systems.
+Also generates similar Step and Dir for spindle
+Intended to connect to 25 pin 'D' connector to emulate old PC 'LPT' port
+for easy swap with PC/Mach driven CNC systems.
+Tested driving Sieg KX3 CNC mill.
+
+Designed to compile and run on:
+    Mbed LPC1768
+    Freescale KL25Z
+    Freescale KL46Z
+*/
+using namespace std;
+
+typedef float  fl_typ;  //  
+const   signed long
+                motor_step_per_rev  =   200,    //  KX3 has 200 step per rev steppers
+                micro_steps         =   10,      //  Arc Eurotrade choice 2,4,5,8,10,16,20,25,32,40,50,64,100,125,128
+                interrupt_period_us =   25, //32, //24.0, //16 is possible with Mbed LPC1768
+                interrupt_freq_Hz   =   1000000 / interrupt_period_us,  //  Serious limit when doing all in software, no real limit with FPGA
+                spindle_factor      =   interrupt_period_us * 4280, //  Derived empirically
+                spindle_max         =   5000;                       //  Stated in KX3 spec
+
+const   fl_typ  ball_screw_pitch_mm =   4.0,    //  KX3 has 4mm pitch ball screws
+                pulses_per_mm       =   micro_steps * motor_step_per_rev / ball_screw_pitch_mm,
+                n_for_onemmpermin   =   pulses_per_mm * interrupt_period_us * pow(2.0,31) / 60000000.0; //  n pir to produce 1mm/min travel
+
+const   signed long
+                max_pulse_freq_Hz   =   interrupt_freq_Hz * 4 / 13, // / 3.25,  //  strictly 2.0, but allow a good margin
+                feed_rate_max       =   60 * max_pulse_freq_Hz / pulses_per_mm;   //  units mm per minute
+//The output frequency F<sub>out</sub> = 'Kernel Speed (Hz)' * n / (2 to the power of 32)
+    
+//#define Fourth_Axis
+//#define SPI_Enable
+#define I2C_Enable
+#define ESTOP   0x100   //  bits used in input reading KX3 limit and EStop switches
 #define XLIM    1
 #define YLIM    2
 #define ZLIM    4
 #define UNKN    8
-const   double  TWO_PI = 8.0 * atan(1.0);
-const   double  epsilon = 1e-5;
+
+const   fl_typ  TWO_PI = 8.0 * atan(1.0);
+const   fl_typ  epsilon = 1e-5;
 struct  pirbufgrain {
-    double  x,
+    fl_typ  x,
             y,
             z,
-            c,
+            distance_code,
             f_rate;
     }   ;
 
 struct  singleGparam    {   //  Place to put all we know about 'x' or 'j' etc parameter from G Code line
-    double          dbl;
+    fl_typ          flt;
     unsigned long   ul;
     int         i,  c;
     bool            changed;      // Flagged true when new value for this axis found in Gcode line, false otherwise 
@@ -24,6 +65,3 @@
     struct  singleGparam   x, y, z, i, j, r, a, b, c, d;   //  After sorting, know where to find any X, Y etc values !
 }   ;
 
-extern  const double  n_for_onemmpermin, feed_rate_max, feed_rate_min, spindle_min, spindle_max, spindle_factor;
-//extern  const   long  pulses_per_mm, max_mm_per_min, interrupt_period_us;
-extern  const   double  pulses_per_mm, max_mm_per_min, interrupt_period_us;
--- a/command_interpreter.cpp	Thu Feb 20 09:27:18 2014 +0000
+++ b/command_interpreter.cpp	Fri Mar 14 14:14:55 2014 +0000
@@ -2,40 +2,93 @@
 #include "rtos.h"
 #include "MODSERIAL.h"
 #include "cnc.h"
-using namespace std;
+#include <cctype>
 
 extern  MODSERIAL pc;
-//extern  Serial pc;
-//extern  bool    liss_active;
 extern  struct  Gparams    last_position;
 extern  void    move_to_XYZ   (struct pirbufgrain & ins)   ;
-//extern  struct  digital_readouts    dro;    //
-//extern  void    scmd (struct singleGparam * a)   ;
 extern  void    flags_report_cmd (struct singleGparam * a)   ;
 extern  void    report_inputs   ()  ;
 
-double  feed_rate = 1.0,  //  global scope, mm per minute. DEFAULTS to 1.0mm per min, very slow.
-        spindle_rpm = 0.0;  //  global scope
+fl_typ  feed_rate = 1.0;  //  global scope, mm per minute. DEFAULTS to 1.0mm per min, very slow.
+signed long    spindle_rpm = 0;  //  global scope
+
+
+
+
+#if defined I2C_Enable
+extern    I2CSlave slave;//(PTE0, PTE1); on KL25
+
+int i2c_checksumchecker (char * buf, int len)    {
+    int k, i = 0x01;
+    for (k = 0; k < len; k++)
+        i += buf[k];
+    i &= 0x0ff;
+    return  i;
+}
 
-bool    isdigit (int a)
-{
-    if(a > ('0' - 1) && a < ('9' + 1))
-        return  true;
-    return  false;
+int i2c_checksumchecker (char * buf)    {
+    return  i2c_checksumchecker (buf, strlen(buf));
+}
+
+char * add_csum (char * buf, int len)    {           //  Adds checksum to end of binary string of known length
+    int j;
+    char    cs = 0;
+    for (j = 0; j < len; j++) {
+        cs += buf[j];
+    }
+    buf[len] = 0xff - cs;
+    buf[len + 1] = 0;
+    return  buf;
+}
+
+char * add_csum (char * buf)    {           //  Adds checksum to end of null terminated string
+    return  add_csum    (buf, strlen(buf));
 }
 
-bool    isupper (int a)
+void    i2c_handler    (void const * name)
 {
-    if  ((a >= 'A') && (a <= 'Z'))  return  true;
-    return  false;
-}
+    const int i2buflen = 16;
+    int err = 0;
+    char    buf[i2buflen];
+    char msg[20] = "Message 2snd\0";
+    add_csum(msg);
+    slave.address(0xc0);
+    err = slave.write(msg, strlen(msg) + 1); // Includes null char    //  returns 0 on success, nz otherwise
+    while   (true)  {
+        int i = slave.receive();
+        switch (i) {
+            case I2CSlave::NoData:  //  Happens most of the time    NoData - the slave has not been addressed
+                osThreadYield();  //  Using RTOS on this project
+                break;
+            case I2CSlave::ReadAddressed:   //   - the master has requested a read from this slave
+                err = slave.write(msg, strlen(msg) + 1); // Includes null char    //  returns 0 on success, nz otherwise
+                pc.printf("RdAddr'd ");
+                break;
+            case I2CSlave::WriteGeneral:    //   - the master is writing to all slave
+                err = slave.read(buf, i2buflen);    //  returns 0 on success, nz otherwise
+                pc.printf("i=%d, - the master is writing to all slave %s\r\n", i, buf);
+                break;
+            case I2CSlave::WriteAddressed:  //   - the master is writing to this slave
+                err = slave.read(buf, i2buflen);    //  returns 0 on success, nz otherwise
+                pc.printf("M wr-> [%s]", buf);
+                for (int z = 0; z < strlen(buf); z++)
+                    pc.printf("%2x, ", buf[z]);
+                pc.printf("cs %2x\r\n", i2c_checksumchecker(buf));
+                break;
+            default:
+                pc.printf("Unknown I2C code %d\r\n");
+                break;
+        }   //  end of switch (i) upon result of slave.receive()
+        if  (err)   {
+            pc.printf("Err %d with i = %d\r\n", err, i);
+            err = 0;
+        }
+        memset  (buf, 0, i2buflen);    // Clear buffer
+    }   //  end of while (true)
+}       //  end of void    i2c_handler    (void const * name)
 
-int tolower (int a)
-{
-    if  (isupper(a))
-        a += 'a' - 'A';
-    return  a;
-}
+#endif
 
 
 const int   goodcodes[] = {0,'a','b','c','i','j','l','r','x','y','z'};    //  possible G Code options
@@ -83,46 +136,47 @@
     }
     dest.x.changed = dest.y.changed = dest.z.changed = dest.a.changed = false;
     dest.i.changed = dest.j.changed = dest.r.changed = false;
-    dest.x.dbl = last_position.x.dbl;   //  copy previous coordinates in case not re-specified
-    dest.y.dbl = last_position.y.dbl;
-    dest.z.dbl = last_position.z.dbl;
-    dest.a.dbl = last_position.a.dbl;
-    dest.i.dbl = last_position.i.dbl;
-    dest.j.dbl = last_position.j.dbl;    dest.r.dbl = last_position.r.dbl;
+    dest.x.flt = last_position.x.flt;   //  copy previous coordinates in case not re-specified
+    dest.y.flt = last_position.y.flt;
+    dest.z.flt = last_position.z.flt;
+    dest.a.flt = last_position.a.flt;
+    dest.i.flt = last_position.i.flt;
+    dest.j.flt = last_position.j.flt;
+    dest.r.flt = last_position.r.flt;
     j = codepos[find_char_in_goodcodes('a')];
     if  (j)  {
         dest.a.changed = true;
-        dest.a.dbl = source_array[j].dbl;
+        dest.a.flt = source_array[j].flt;
     }
     j = codepos[find_char_in_goodcodes('x')];
     if  (j)  {
         dest.x.changed = true;
-        dest.x.dbl = source_array[j].dbl;
+        dest.x.flt = source_array[j].flt;
     }
     j = codepos[find_char_in_goodcodes('y')];
     if  (j)  {
         dest.y.changed = true;
-        dest.y.dbl = source_array[j].dbl;
+        dest.y.flt = source_array[j].flt;
     }
     j = codepos[find_char_in_goodcodes('z')];
     if  (j)  {
         dest.z.changed = true;
-        dest.z.dbl = source_array[j].dbl;
+        dest.z.flt = source_array[j].flt;
     }
     j = codepos[find_char_in_goodcodes('i')];
     if  (j)  {
         dest.i.changed = true;
-        dest.i.dbl = source_array[j].dbl;
+        dest.i.flt = source_array[j].flt;
     }
     j = codepos[find_char_in_goodcodes('j')];
     if  (j)  {
         dest.j.changed = true;
-        dest.j.dbl = source_array[j].dbl;
+        dest.j.flt = source_array[j].flt;
     }
     j = codepos[find_char_in_goodcodes('r')];
     if  (j)  {
         dest.r.changed = true;
-        dest.r.dbl = source_array[j].dbl;
+        dest.r.flt = source_array[j].flt;
     }
 }
 
@@ -131,7 +185,7 @@
     struct  Gparams tmp;
     struct  pirbufgrain start_point, end_point, centre_point, next_point;
     int state = 0, arc_steps;
-    double  rad_start, rad_end, start_angle, end_angle, next_angle, swept_angle, angle_step, arc_len, z_step;
+    fl_typ  rad_start, rad_end, start_angle, end_angle, next_angle, swept_angle, angle_step, arc_len, z_step;
     if  (twoorthree != 2 && twoorthree != 3)    {
         pc.printf("Err got %d when should be 2 or 3", twoorthree);
         return;
@@ -150,20 +204,20 @@
     if  (!tmp.x.changed || !tmp.y.changed)                  state |= 0x10000;    //  Error, X or Y missing
     if  (tmp.r.changed && !tmp.i.changed && !tmp.j.changed) state |= 1;  //  Validated R mode got R not I not J
     if  (!tmp.r.changed && tmp.i.changed && tmp.j.changed)  state |= 2;  //  Validated IJ mode not R got I got J
-    start_point.x   = last_position.x.dbl;
-    start_point.y   = last_position.y.dbl;
-    start_point.z   = last_position.z.dbl;
-    end_point.x     = tmp.x.dbl;
-    end_point.y     = tmp.y.dbl;
-    end_point.z     = tmp.z.dbl;
+    start_point.x   = last_position.x.flt;
+    start_point.y   = last_position.y.flt;
+    start_point.z   = last_position.z.flt;
+    end_point.x     = tmp.x.flt;
+    end_point.y     = tmp.y.flt;
+    end_point.z     = tmp.z.flt;
     switch  (state) {
         case    1:  //  Radius format arc
             pc.printf("Valid Radius format arc TO DO - not yet implemeted\r\n");
         break;
         case    2:  //  Centre format arc ** OFFSETS ARE RELATIVE ** Abs coordinates not catered for
             pc.printf("Valid Centre format arc\r\n");
-            centre_point.x = start_point.x + tmp.i.dbl;
-            centre_point.y = start_point.y + tmp.j.dbl;
+            centre_point.x = start_point.x + tmp.i.flt;
+            centre_point.y = start_point.y + tmp.j.flt;
             rad_start   = hypot(start_point.x - centre_point.x, start_point.y - centre_point.y);
             rad_end     = hypot(end_point.x - centre_point.x, end_point.y - centre_point.y);
             pc.printf("Start  point X %.3f, Y %.3f\r\n", start_point.x, start_point.y);
@@ -213,7 +267,7 @@
     }   //  end of switch(state)
 }
 
-void    g0g1cmdcore (struct singleGparam * source_array, double f_rate)    //  Updates any / all of x, y, z NCOs
+void    g0g1cmdcore (struct singleGparam * source_array, fl_typ f_rate)    //  Updates any / all of x, y, z NCOs
 {   //  Only get here when some G0 or G1 input has been read.  G0 or G1 determined by f_rate
     struct  pirbufgrain ins;//, outs;
     struct  Gparams tmp;
@@ -222,9 +276,9 @@
         pc.printf("No change in X, Y or Z in G0/G1. Ignoring\r\n");
         return;
     }
-    ins.x  = tmp.x.dbl;
-    ins.y  = tmp.y.dbl;
-    ins.z  = tmp.z.dbl;
+    ins.x  = tmp.x.flt;
+    ins.y  = tmp.y.flt;
+    ins.z  = tmp.z.flt;
     ins.f_rate = f_rate;
     move_to_XYZ (ins);
 }
@@ -248,54 +302,42 @@
 }
 
 void    fcmd (struct singleGparam * a)   {  //  Set Feed Rate command
-    if  (a[1].dbl < 0.0)    {
-        pc.printf("feed rate %.1f ? Setting to 0\r\n", a[1].dbl);
-        a[1].dbl = 0.0;
+    if  (a[1].flt < 0.0)    {
+        pc.printf("feed rate %.1f ? Setting to 0\r\n", a[1].flt);
+        a[1].flt = 0.0;
     }
-    if  (a[1].dbl > feed_rate_max)    {
-        pc.printf   ("Error, can't set feed rate to %.1f, max is %.1f, ", a[1].dbl, feed_rate_max);
-        a[1].dbl = feed_rate_max;
+    if  (a[1].flt > feed_rate_max)    {
+        pc.printf   ("Error, can't set feed rate to %.1f, max is %.1f, ", a[1].flt, feed_rate_max);
+        a[1].flt = feed_rate_max;
     }
-    pc.printf   ("Setting feed_rate to %.1f\r\n", a[1].dbl);
-    feed_rate = a[1].dbl;
+    pc.printf   ("Setting feed_rate to %.1f\r\n", a[1].flt);
+    feed_rate = a[1].flt;
 }
-extern  void    spindle_control (double ss)  ;
+extern  void    spindle_control (signed long ss)  ;
 extern  bool    spindle_running ()  ;
 
 void    M3cmd (struct singleGparam * a)   { spindle_control (spindle_rpm);  }
-void    M5cmd (struct singleGparam * a)   { spindle_control (0.0);  }
+void    M5cmd (struct singleGparam * a)   { spindle_control (0);  }
 
 void    scmd (struct singleGparam * a)   {
-    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);
+    if  (fabs(a[1].flt) > spindle_max)    {
+        pc.printf   ("Errror setting spindle RPM, can't set to %.0f, ignoring request\r\n", a[1].flt);
         return;
     }
-    pc.printf   ("Setting spindle RPM to %.0f Can set Pos or Neg for fwd/rev\r\n", a[1].dbl);
-    spindle_rpm = a[1].dbl;
+    pc.printf   ("Setting spindle RPM to %.0f Can set Pos or Neg for fwd/rev\r\n", a[1].flt);
+    spindle_rpm = (signed long) a[1].flt;
     if  (spindle_running())
         spindle_control (spindle_rpm);
-/*
-
-    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("Readback ss %d\r\n", spindle_rpm);
 }
 
 extern  void    target_cmd (struct singleGparam * a)   ;
 
 void    stopcmd (struct singleGparam * a)   {pc.printf("Stop ! er, not working yet\r\n");}
-void    m1cmd (struct singleGparam * a)   {pc.printf("m1 Optional Programme Stop\r\n");}
-void    m3cmd (struct singleGparam * a)   {pc.printf("m3 Rotate Spindle Clockwise\r\n");}
-void    m4cmd (struct singleGparam * a)   {pc.printf("m4 Rotate Spindle Counter Clockwise\r\n");}
-void    m5cmd (struct singleGparam * a)   {pc.printf("m5 Stop Spindle\r\n");}
+//void    m1cmd (struct singleGparam * a)   {pc.printf("m1 Optional Programme Stop\r\n");}
+//void    m3cmd (struct singleGparam * a)   {pc.printf("m3 Rotate Spindle Clockwise\r\n");}
+//void    m4cmd (struct singleGparam * a)   {pc.printf("m4 Rotate Spindle Counter Clockwise\r\n");}
+//void    m5cmd (struct singleGparam * a)   {pc.printf("m5 Stop Spindle\r\n");}
 /*void    m30cmd (struct singleGparam * a)   {pc.printf("m30 Programme End and Rewind\r\n");}
 void    m47cmd (struct singleGparam * a)   {pc.printf("m47 Repeat Prog from First Line\r\n");}
 void    m48cmd (struct singleGparam * a)   {pc.printf("m48 Enable Speed and Feed Override\r\n");}
@@ -319,57 +361,12 @@
 void    report_ins_cmd  (struct singleGparam * a)  {
    report_inputs();
 }
-/*void    tasktstone    (void const * name)
-{
-    static int i = 0;
-    pc.printf("Arrived at tasktstone\r\n");
-    while   (true)  {
-        pc.printf("%s %d\r\n", name, i++);
-        Thread::wait(9500);
-        osThreadYield();
-    }
-}*/
-
-/*void    tasktestcmd (struct singleGparam * a)   {
-    pc.printf("At tasktestcmd\r\n");
-    Thread tasktest (tasktstone, (void *) "Bollocks");
-    pc.printf("Leaving tasktestcmd\r\n");
-}
-*/
-extern  void    lissajous   (double)  ;
+extern  void    lissajous   (fl_typ)  ;
 
 void    lisscmd (struct singleGparam * a)   {
     lissajous   (feed_rate);
-/*    if(liss_active) {
-        pc.printf("Can not add Lissajous, already running.\r\n");
-    }
-    else    {
-        pc.printf("Adding Lissajous task\r\n");
-        liss_active = true;
-    }*/
 }
-/*
-void    drooncmd    (struct singleGparam * a)
-{
-    dro.dro_output = true;     //  Enable continuous dro display update
-}
-void    drooffcmd   (struct singleGparam * a)
-{
-    dro.dro_output = false;    //  Disable continuous dro display update
-}
-*/
 
-/*void    g90p1cmd (struct singleGparam * a)
-{
-    pc.printf   ("Arrived at function fredcmd with %d parameters\r\n", a[0].i);
-    for (int i = 1; i <= a[0].i; i++)   {
-        pc.printf   ("*%c* ", a[i].c);
-        pc.printf   ("%d, ", a[i].i);
-        pc.printf   ("%f\r\n", a[i].dbl);
-    }
-    pc.printf   (" endof param list\r\n");
-}
-*/
 void    menucmd (struct singleGparam * a);
 struct kb_command  {
     const char * cmd_word;         //  points to text e.g. "menu"
@@ -393,26 +390,6 @@
     {"m3", "Start Spindle at last 'S'", M3cmd},
     {"m5", "Stop Spindle", M5cmd},
     {"g0", "Rapid move", g0cmd},
-    /*{"m30", "Not Implemented", m30cmd},
-    {"m47", "Not Implemented", m47cmd},
-    {"m48", "Not Implemented", m48cmd},
-    {"m49", "Not Implemented", m49cmd},
-    {"m98", "Not Implemented", m98cmd},
-    {"m99", "Not Implemented", m99cmd},
-    {"m1", "Not Implemented", m1cmd},
-    {"m3", "Not Implemented", m3cmd},
-    {"m4", "Not Implemented", m4cmd},
-    {"m5", "Not Implemented", m5cmd},
-    {"g10", "Not Implemented", g10cmd},
-    {"g17", "Not Implemented", g17cmd},
-    {"g20", "Not Implemented", g20cmd},
-    {"g21", "Not Implemented", g21cmd},
-    {"g40", "Not Implemented", g40cmd},
-    {"g50", "Not Implemented", g50cmd},
-    {"g90.1", "Not Implemented", g90p1cmd},
-    {"g91.1", "Not Implemented", g91p1cmd},
-    {"g90", "Not Implemented", g90cmd},
-    */
     {"g1", "Linear Interpolation - move straight at current feed rate", g1cmd},
     {"g2", "Helical Interpolation CW (Arc, circle)", g2cmd},
     {"g3", "Helical Interpolation CCW (Arc, circle)", g3cmd},
@@ -420,13 +397,8 @@
     {"flags", "Report System Flags", flags_report_cmd},
     {"inputs", "Report State of Input bits", report_ins_cmd},
     {"target", "Identify computer device", target_cmd},
-//    {"ttest", "Add a task to prove we can, or not", tasktestcmd},
-//    {"dro on", "Turn dro readout on", drooncmd},
-//    {"dro off", "Turn dro readout off", drooffcmd}
 };
-//const    int numof_menu_items = sizeof(kbc2) / sizeof(kb_command);
-//const    int numof_menu_items_sc = sizeof(input_syntax_check) / sizeof(kb_command);
-//const    int numof_menu_items_ce = sizeof(command_execute) / sizeof(kb_command);
+
 int numof_menu_items;
 void    menucmd (struct singleGparam * a)
 {
@@ -436,21 +408,14 @@
     pc.printf("End of List of Commands\r\n");
 }
 
-bool    isalpha (int c)
-{
-    if  ((c >= 'a') && (c <= 'z'))   return  true;
-    if  ((c >= 'A') && (c <= 'Z'))   return  true;
-    return  false;
-}
 
-void    nudger  (int code)  {
+void    nudger  (int code)  {   //  Allows <Ctrl> chars to nudge machine axes
     //  Using <Ctrl>+ 'F', 'B' for Y, 'L', 'R' for X, 'U', 'D' for Z
     //                 6    2          12   18         21   4
-//    struct  Gparams dest;
     struct  pirbufgrain dest;
-    dest.x = last_position.x.dbl;
-    dest.y = last_position.y.dbl;
-    dest.z = last_position.z.dbl;
+    dest.x = last_position.x.flt;
+    dest.y = last_position.y.flt;
+    dest.z = last_position.z.flt;
     dest.f_rate = feed_rate;
     switch  (code)  {
         case    6:  //  'F' move -Y
@@ -472,7 +437,6 @@
         dest.z -= 0.1;
         default:
         break;
-//            pc.printf("Nuffink to do in nudger\r\n");
     }   //  end of switch
     move_to_XYZ (dest);
 }
@@ -484,13 +448,14 @@
 Purpose:
 
 */
+
 void    command_line_interpreter    (void const * name)
 {
 const int MAX_PARAMS = 10, MAX_CMD_LEN = 120;
 static  char    cmd_line[MAX_CMD_LEN + 4];
 static  struct  singleGparam   params[MAX_PARAMS + 1];
 static  int     cl_index = 0, lastalpha = 0;
-//    pc.printf("Got to cli, Starting cli\r\n");
+static  fl_typ  fracmul;
     if  (true)  {
         command_list_ptr = command_execute;
         numof_menu_items = sizeof(command_execute) / sizeof(kb_command);
@@ -525,13 +490,12 @@
                         if(strncmp(command_list_ptr[i].cmd_word, cmd_line, wrdlen) == 0)  {   //  If match found
                             bool negflag = false;
                             int state = 0, paramindex;
-                            double  fracmul;
     //                            pc.printf("Found match for word [%s]\r\n", kbc[i].wrd);
                             for(paramindex = 0; paramindex < MAX_PARAMS; paramindex++) {
                                 // Clear out whole set of old parameters ready for anything new on this line
                                 params[paramindex].i = 0;   //  for integer parameters
                                 params[paramindex].c = 0;   //  for last alpha char, helps tie 'X' to '-23.5' etc
-                                params[paramindex].dbl = 0.0; //  for floating point parameters
+                                params[paramindex].flt = 0.0; //  for floating point parameters
                                 params[paramindex].ul = 0;
                                 params[paramindex].changed = false;
                             }
@@ -565,7 +529,7 @@
                                             if  (ch == '.')  {
                                                 state = 2;
                                                 fracmul = 0.1;
-                                                params[paramindex].dbl = (double)params[paramindex].i;
+                                                params[paramindex].flt = (fl_typ)params[paramindex].i;
                                             } else    {
                                                 params[0].i++;    //  count of validated parameters
                                                 state = 0;  //  Have read past last digit of number string
@@ -573,20 +537,20 @@
                                                     params[paramindex].i = -params[paramindex].i;
                                                     negflag = false;
                                                 }
-                                                params[paramindex].dbl = (double)params[paramindex].i;
+                                                params[paramindex].flt = (fl_typ)params[paramindex].i;
                                             }
                                         }
                                         break;
                                     case    2:  //  looking for fractional part of double
                                         if(isdigit(ch)) {   //  accumulating fractional part from string
-                                            params[paramindex].dbl += (double)((ch - '0') * fracmul);
+                                            params[paramindex].flt += (fl_typ)((ch - '0') * fracmul);
                                             fracmul /= 10.0;
                                         } else    { //  found non-digit terminating double precision number
                                             params[0].i++;    //  count of validated parameters
                                             state = 0;  //  Have read past last digit of number string
                                             if(negflag) {
                                                 params[paramindex].i = -params[paramindex].i;
-                                                params[paramindex].dbl = -params[paramindex].dbl;
+                                                params[paramindex].flt = -params[paramindex].flt;
                                                 negflag = false;
                                             }
                                         }
@@ -607,7 +571,6 @@
                 cl_index = lastalpha = 0;
             }               // End of else key was CR, may or may not be command to lookup
         }                   //  End of while (pc.readable())
-    //    pc.printf("cli yielding\r\n");
         osThreadYield();  //  Using RTOS on this project
     }
 }
--- a/lissajous.cpp	Thu Feb 20 09:27:18 2014 +0000
+++ b/lissajous.cpp	Fri Mar 14 14:14:55 2014 +0000
@@ -16,8 +16,8 @@
 */
 //bool    liss_active = false;    //  global flag used to prevent running more than once at a time
 
-void    lissajous   (double feed_rate)  {
-    const double    PI          = 4.0 * atan(1.0),  //3.142ish but more accurate
+void    lissajous   (fl_typ feed_rate)  {
+    const fl_typ    PI          = 4.0 * atan(1.0),  //3.142ish but more accurate
                     Deg2Rad     = PI / 180.0, //  degrees to radian conversion factor
                     MaxX            = 40.0,     // Plot size X to move +/- MaxX
                     MaxY            = 25.0,     // Plot size Y to move +/- MaxY
@@ -26,12 +26,12 @@
                     FreqRatio       = 0.255;
     const int       StepsPerRevX    = 100,
                     NumofXCycles    = 16;
-    const double    AngleStepX  = (2.0 * PI / StepsPerRevX),
+    const fl_typ    AngleStepX  = (2.0 * PI / StepsPerRevX),
                     AngleStepY  = AngleStepX * FreqRatio;
 
 //void    lissajous   (void const * arg_string)  {
     struct  pirbufgrain Coords;
-    double  AngleX = StartAngDegX * Deg2Rad,
+    fl_typ  AngleX = StartAngDegX * Deg2Rad,
             AngleY = StartAngDegY * Deg2Rad;
             
 //    pc.printf("In lissajous func, Loading Lissajous\r\n");
--- 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()