![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
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() +