Diff: main.cpp

Revision:

1:66ee619f206b

Parent:

0:5d0f270bfc87

Child:

2:b3c668ec43ac

--- a/main.cpp	Fri Jan 31 11:16:21 2014 +0000
+++ b/main.cpp	Thu Feb 06 08:45:02 2014 +0000
@@ -1,31 +1,43 @@
 #include "mbed.h"
+#include "rtos.h"
 #include "cnc.h"
 using namespace std;
-extern  void    lissajous   (void)  ;
-extern  void    command_line_interpreter    (void) ;
-extern  void    init_last_position  ()  ;
-extern  char *  readout (char * txt, int p)    ;    //  p has running subtotal of all pulses issued to stepper driver
+extern  void    command_line_interpreter    (void const *) ;
+extern  void    lissajous   (void const *)  ;
+extern  double  feed_rate;
+extern  void    more_setup  ()  ;
 
+const   int BAUD = 38400;
 Serial  pc(USBTX, USBRX);        //  tx, rx to pc
-const   int BAUD = 9600;
-Ticker  NCO_gen;                 // Ticker generating interrupts at NCO updating frequency
-struct  axis_speeds_element     axis_speeds[axis_speeds_buffsize + 1];  //  buffer space for list of future moves
+Ticker  NCO_gen;                 // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz)
 struct  digital_readouts    dro;    //some signed int
 
-bool    running     = false;
-volatile unsigned long   ticks = 0L;
-unsigned long
-                pir_a = 0L,
+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_s = 0L,     //  Referenced only in command_interpreter as spindle speed setting
-                tickrun = 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_s = 0L;     //  Referenced only in command_interpreter as spindle speed setting
+
 int spindlefwdrev = 0;  //  Takes values of 0 or 4 only
 
+
 #if defined (TARGET_KL25Z)
-    DigitalOut intled(PTE1);    //J2p20
+    DigitalOut intled       (PTE1);     //J2p20
+    DigitalOut charge_pump  (PTE0);     //J2p18    
+    DigitalIn   D25pin10    (PTD6);     //jp217
+    DigitalIn   D25pin11    (PTE31);    //j2p13
+    DigitalIn   D25pin12    (PTA17);    //j2p11
+    DigitalIn   D25pin13    (PTA16);    //j2p9
+    DigitalIn   D25pin15    (PTC17);    //j2p7
     //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)
     //                 J2p08,J2p10,J2p12, J2p06
@@ -43,7 +55,13 @@
                 PortBitADi   = 13;    //  Port bit num A Dir    J2P03
 #endif
 #if defined (TARGET_KL46Z)
-    DigitalOut intled(PTE1);    //J2p20
+    DigitalOut intled       (PTE1);    //J2p20
+    DigitalOut charge_pump  (PTE0);    //J2p18    
+    DigitalIn   D25pin10    (PTB9);//d6 on 25 jp217
+    DigitalIn   D25pin11    (PTA16);//e31 on 25 j2p13
+    DigitalIn   D25pin12    (PTA15);//a17 on 20 j2p11
+    DigitalIn   D25pin13    (PTA14);//a16 on 25 j2p9
+    DigitalIn   D25pin15    (PTA6);//c17 on 25 j2p7
     SPISlave spidevice(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC)
     //                 J2p13, J2p15, J2p11, J2p09
                 // Easy way to allocate port bits for           ***  N O T  CHECKED for 46Z ***
@@ -59,7 +77,13 @@
                 PortBitADi   = 16;    //  Port bit num A Dir    J2P03
 #endif
 #if defined (TARGET_MBED_LPC1768)
-    DigitalOut intled(LED2);
+    DigitalOut intled(LED2);                    //  Correct
+    DigitalOut charge_pump  (PTE0);    //J2p18    Following 5 inputs all wrong - TO DO sort which pins
+    DigitalIn   D25pin10    (PTD6);     //jp217
+    DigitalIn   D25pin11    (PTE31);    //j2p13
+    DigitalIn   D25pin12    (PTA17);    //j2p11
+    DigitalIn   D25pin13    (PTA16);    //j2p9
+    DigitalIn   D25pin15    (PTC17);    //j2p7
     SPISlave spidevice(p5, p6, p7, p8);
                 // Easy way to allocate port bits for           ***  N O T  CHECKED for MBED_LPC1768 ***
                 // output of stepper motor Step and DIR sigs
@@ -90,7 +114,7 @@
                 PortBitADi   = 18;    //  Port bit num A Dir
 #endif
 
-static const   long
+const   long
             XSt1 =   1 << PortBitXSt, XSt0 = 0,
             XDi1 =   1 << PortBitXDi, XDi0 = 0,
             YSt1 =   1 << PortBitYSt, YSt0 = 0,
@@ -104,15 +128,77 @@
 
 PortOut mysteppers(STEPPER_PORT, SM_MASK);
 
+const int PIRBUFFSIZE = 10;
+class   circbuff    {
+    private:
+        pirbufgrain grain   [PIRBUFFSIZE + 1];
+    int     OnPtr, OffPtr;
+    bool    bufffull, buffempty, buffhalf;
+    void    setempty    ()  {
+        bufffull = false;
+        buffhalf = false;
+        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.f_rate = src.f_rate;   //  int feed rate mm per min * 1000
+    }
+    public:
+    void    init    ()  {
+        OnPtr = OffPtr = 0;
+        setempty    ();
+    }
+    int     On_Q    ()  {
+        int k;
+        if  (bufffull)    return  PIRBUFFSIZE;
+        k = OnPtr - OffPtr;
+        if  (k < 0) k += PIRBUFFSIZE;
+        if  (k > PIRBUFFSIZE / 2)
+            buffhalf = true;
+        else
+            buffhalf = false;
+        return  k;
+    }
+    bool    readable    ()  {return !buffempty; }
+    bool    writeable   ()  {return !bufffull; }
+    bool    read    (pirbufgrain & g)   {
+        if  (buffempty) return  false;
+        bufffull = false;
+        grain_copy  (grain[OffPtr++], g);
+        if  (OffPtr >= PIRBUFFSIZE)
+            OffPtr = 0;
+        if  (OnPtr == OffPtr)
+            buffempty = true;
+        return  true;
+    }
+    bool    write   (pirbufgrain & g)   {
+        if  (bufffull)  return  false;
+        buffempty = false;
+        grain_copy  (g, grain[OnPtr++]);
+        if  (OnPtr >= PIRBUFFSIZE)
+            OnPtr = 0;
+        if  (OnPtr == OffPtr)
+            bufffull = true;
+        return  true;
+    }
+}   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;
+}
+
+
 /*
 *   Interrupt Service Routine
 */
-//void    Numerically_Controlled_Oscillators_ISR ()  {   // services Ticker 'NCO_gen' generated interrupts ***ISR***
-//    intled = 1;
-//    ticks++;
-//    intled = 0;
-//}
 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
@@ -124,47 +210,66 @@
         acc_x = 0L,
         acc_y = 0L,
         acc_z = 0L;
-    static  int obitz = 0;
+    static  int obitz = 0, mscount = millisec_countdown;
     int oldbitz, acts;
 
     intled = 1;     //  LED on for duration of interrupt service - point for scope probing
     ticks++;        //  count of interrupts serviced
-//    int response = spidevice.write(0x55); // Only if SPI Master -- TAKES 2.5 us --
-//      The rest of the whole int handler takes only about 3.0 us
-    acc_x += pir_x;     //  Update phase of signals in accumulators
-    acc_y += pir_y;
-    acc_z += pir_z;
-    acc_a += pir_a;
-//    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 bits 0, 2 and 4
-
-    acts = (~oldbitz & obitz);  //  get pos clk edge triggers in bits 0, 2 and 4 (1, 4, 16)
-    acts |= (obitz & (XDi1 | YDi1 | ZDi1));      //  get directions
-    if(acts & XSt1)    {           //  got pos clk edge for axis X
-        if  (acts & XDi1)
-            dro.x++;
-        else    dro.x--;
+    if(!--mscount)  {   //  Maintain global counter of elapsed milli seconds
+        mscount = millisec_countdown;
+        millisecs++;
     }
-    if(acts & YSt1)    {           //  got pos clk edge for axis Y
-        if  (acts & YDi1)
-            dro.y++;
-        else    dro.y--;
+    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
+        }
     }
-    if(acts & ZSt1)   {           //  got pos clk edge for axis Z
-        if  (acts & ZDi1)
-            dro.z++;
-        else    dro.z--;
+    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;
+        }
     }
-    if  (running && tickrun <= ticks)   {   //  End of a machine movement detected, start next move here if possible
-        running = false;
-        pir_x = 0L; //  stop all stepper motors
-        pir_y = 0L;
-        pir_z = 0L;
-        pir_a = 0L;
-    }
+    charge_pump = ticks & 0x02;
     intled = 0;         //  LED off
 }           //  end of interrupt handler
 
@@ -173,77 +278,110 @@
 */
 
 
-void    pir_updater (struct axis_speeds_element * p)   {   //  To arrive here with wanted 'mm per min' values in x, y and z
-//void    pir_updater (struct pirs * p)   {   //  To arrive here with wanted 'mm per min' values in x, y and z
-//    pc.printf(p.x ? "true":"false");      //  Uses pointer as we may wish to rapid update from circular buffer
-    tickrun = p->duration_ticks;
-    unsigned long tc = ticks, after;
-    while   (tc == ticks)   {}  //  wait until just after an interrupt - note requires 'volatile' ticks
-    tickrun += ticks;
-    pir_x = p->x;  //  Update NCO phase inc registers
-    pir_y = p->y;
-    pir_z = p->z;
-    pir_a = p->a;
-    after = ticks - tc;
-    running = true;
-    if  (after == 1)
-        pc.printf("pir_update was good !, ticks %d\r\n", p->duration_ticks);
-    else
-        pc.printf("Oops! Looks like pir_update got run-over, code = %d\r\n", after);
+void    newpir_updater (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())   {
+            osThreadYield();
+        }
+        CircBuff.read(outs);
+        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);
+        pir_x_next = x;
+        pir_y_next = y;
+        pir_z_next = z;
+        move_ended = false;
+        new_run_pending = true; //  cleared and 'running' flag set in interrupt handler
+        idle = false;
+        count++;
+//        pc.printf("CircB tot %d\r\n", count);
+    }
 }
 
+
+class digital_readout_stuff  {   //  class does not need to be named here
+    char *  readout (char * txt, long p)         //  p has running subtotal of all pulses issued to stepper driver
+    {
+        txt[0] = '+';               //  constructs string e.g. "+123.456"
+        txt[8] = 0;                 //  null terminated
+        if  (p < 0)    {
+            txt[0] = '-';
+            p = -p;
+        }
+        p *= 1000;
+        p /= pulses_per_mm;
+        if  (p > 999999)    {
+            sprintf(txt + 1, "OVRANGE");
+            return  txt;
+        }
+        for(int k = 7; k > 0; k--)  {
+            if  (k == 4)
+                txt[k] = '.';
+            else    {
+                txt[k] = '0' + (p % 10);
+                p /= 10;
+            }
+        }
+        return  txt;    //  Returns pointer unaltered for subsequent use by e.g. cout
+    }
+        public:
+    void    update  ()  {
+        static  long    t = 0;
+        if  (millisecs < t)
+            return;
+        if(!idle && dro.dro_output)  {
+            char    txt[12];
+            pc.printf("dros X %s,", readout(txt, dro.x));    //  dro.n has running subtotal of all pulses issued to stepper driver.n
+            pc.printf(" Y %s, Z ", readout(txt, dro.y));
+            pc.printf("%s, %s\r\n", readout(txt, dro.z), running ? "runn":"idle");
+            if(!running)    idle = true;    //  Purpose of idle flag is to stop dro after run completes.
+            t = millisecs + 350;    //  Schedule next update after this non-blocking delay
+        }
+    }
+}   dro_out ;
+
+/*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();
+    }
+}*/
+
 int main() {
-    char txt[10];   //  few chars used for dro output
     pc.baud(BAUD); //  comms to 'PuTTY' serial terminal via mbed usb
     dro.x = dro.y = dro.z = 0;   //  These dro registers count pulses delivered to stepper motor driver
     dro.dro_output = true;
-    init_last_position  ()  ;   //  Zeros one 'pirs' structure
+    more_setup  ()  ;   //  Zeros one 'pirs' structure 'last_position'
+    CircBuff.init   ();
     spidevice.format(8, 0);    //  8 bits mode 0,  // p11 mosi, p12 miso, p13 sclk ** ONLY 8 BIT **
     spidevice.frequency(12000000);  //  12MHz bit rate
-//    int response = spidevice.write(0xFFFF); // Only if SPI Master
-//    spidevice.reply(0x00);              // Prime SPI with first reply
-    /*
-// Reply to a SPI master as slave
- 
- #include "mbed.h"
- 
- SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
- 
- int main() {
-     device.reply(0x00);              // Prime SPI with first reply
-     while(1) {
-         if(device.receive()) {
-             int v = device.read();   // Read byte from master
-             v = (v + 1) % 0x100;     // Add one to it, modulo 256
-             device.reply(v);         // Make this the next reply
-         }
-     }
- }    */
-
-    struct  axis_speeds_element * asepp = axis_speeds;   //  Address of axis_speeds[0]
-    for (int i = 0; i < axis_speeds_buffsize; i++)  {
-        axis_speeds[i].x =
-        axis_speeds[i].y =
-        axis_speeds[i].z =
-        axis_speeds[i].a =
-        axis_speeds[i].duration_ticks = 0L;
-        axis_speeds[i].ready = false;
-    }
-//    pc.printf("SPI Setup returned 0x%x\r\n", response);
-/*    int ch;
-    while   (true) {
-        while   (pc.readable()) {
-            ch = pc.getc();
-            pc.printf("**%c**", ch);
-        }
-        pc.printf("No more\r\n");
-        wait(0.5);
-    }
-  */      
-    lissajous   ();
+    pc.printf("\r\n*\n*\n");
 #if defined (TARGET_KL25Z)
     pc.printf   ("Found device Freescale KL25Z\r\n");
-//    DigitalOut intled(PTA1);  ** THIS KILLS SERIAL Rx **
 #endif
 #if defined (TARGET_KL46Z)
     pc.printf   ("Found device Freescale KL46Z\r\n");
@@ -251,28 +389,21 @@
 #if defined (TARGET_MBED_LPC1768)
     pc.printf   ("Found device MBED_LPC1768\r\n");
 #endif
-    pc.printf("Three NCOs have been setup, they will move when given values by the G0 x? y? z? command\r\n");
-    pc.printf("sizeof long long is %d bytes, pulsecnt at 1mm per min = %f, top speed = %d mm per min\r\n", sizeof(long long), n_for_onemmpermin, max_mm_per_min);
-    NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, interrupt_period_us);// Have setup timed interrupts, let other code deal
-    while(1) {
-//        if(!(ticks & 0x00000ff))    {
-//            mybigmotor = arr[step++];
-//            step &= 0x03;
-//            pc.printf("^");
-//        }
-        command_line_interpreter    ();
-//        myled = 1;        //wait(0.4);//        myled = 0;        //wait(0.4);
-//        if(running && dro_output && !(ticks & 0x00007ffc))  { // including 'running' causes display to freeze at almost there !
-        if(dro.dro_output && !(ticks & 0x00007ffc))  {
-            pc.printf("dros X %s, Y ", readout(txt, dro.x));    //  dro.n has running subtotal of all pulses issued to stepper driver.n
-            pc.printf("%s, Z ", readout(txt, dro.y));
-            pc.printf("%s", readout(txt, dro.z));
-            pc.printf(", ticks %d\r\n", ticks);
-            asepp++;
-            if  (asepp >= axis_speeds + axis_speeds_buffsize) {
-                asepp = axis_speeds;
-            }
-//            pc.printf   ("axis_speed %d, %lx\r\n", asepp - axis_speeds, (long)asep);
-        }
-    }
-}
+    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.5f 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")  ;
+    Thread comlin (command_line_interpreter,    (void *)"cli"); //  Read any instructions arriving via serial port and act upon them
+    pc.printf("Added cli thread\r\n");
+    move_ended = true;  //  Needed to kickstart system
+    
+    while(1) {  //  Round Robin loop
+        dro_out.update  ();             //  Update DRO readings if, and as often as needed
+        osThreadYield();                //
+    }   //  end of Round Robin loop
+}       //  end of int main()
+