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
main.cpp
- Committer:
- JonFreeman
- Date:
- 2014-02-06
- Revision:
- 1:66ee619f206b
- Parent:
- 0:5d0f270bfc87
- Child:
- 2:b3c668ec43ac
File content as of revision 1:66ee619f206b:
#include "mbed.h" #include "rtos.h" #include "cnc.h" using namespace std; 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 Ticker NCO_gen; // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz) struct digital_readouts dro; //some signed int 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_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 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 //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 PortBitXDi = 4, // Port bit num X Dir J1P07 PortBitYSt = 5, // Port bit num Y Step J1P09 PortBitYDi = 6, // Port bit num Y Dir J1P11 PortBitZSt = 10, // Port bit num Z Step J1P13 PortBitZDi = 11, // Port bit num Z Dir J1P15 PortBitASt = 12, // Port bit num A Step J2P01 PortBitADi = 13; // Port bit num A Dir J2P03 #endif #if defined (TARGET_KL46Z) 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 *** // output of stepper motor Step and DIR sigs #define STEPPER_PORT PortC const int PortBitXSt = 0, // Port bit num X Step J1P05 PortBitXDi = 4, // Port bit num X Dir J1P07 PortBitYSt = 6, // Port bit num Y Step J1P09 PortBitYDi = 7, // Port bit num Y Dir J1P11 PortBitZSt = 10, // Port bit num Z Step J1P13 PortBitZDi = 11, // Port bit num Z Dir J1P15 PortBitASt = 13, // Port bit num A Step J2P01 PortBitADi = 16; // Port bit num A Dir J2P03 #endif #if defined (TARGET_MBED_LPC1768) 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 #define STEPPER_PORT Port0 /* Port 0 bits routed to DIP pins as follows:- P0.00 p09 Reserve SDA P0.01 p10 Reserve SCL P0.04 p30 CAN rd - USE X Step P0.05 p29 CAN td - USE X Dir P0.10 p28 SDA - USE Y Step P0.11 p27 SCL - USE Y Dir P0.15 p13 Tx - USE Z Step P0.16 p14 Rx - USE Z Dir P0.17 p12 miso - USE A Step P0.18 p11 mosi - Use A Dir P0.23 p15 A In P0.24 p16 A In P0.25 p17 A In P0.26 p18 Reserve A Out */ const int PortBitXSt = 4, // Port bit num X Step PortBitXDi = 5, // Port bit num X Dir PortBitYSt = 10, // Port bit num Y Step PortBitYDi = 11, // Port bit num Y Dir PortBitZSt = 15, // Port bit num Z Step PortBitZDi = 16, // Port bit num Z Dir PortBitASt = 17, // Port bit num A Step PortBitADi = 18; // Port bit num A Dir #endif const long XSt1 = 1 << PortBitXSt, XSt0 = 0, XDi1 = 1 << PortBitXDi, XDi0 = 0, YSt1 = 1 << PortBitYSt, YSt0 = 0, YDi1 = 1 << PortBitYDi, YDi0 = 0, ZSt1 = 1 << PortBitZSt, ZSt0 = 0, ZDi1 = 1 << PortBitZDi, ZDi0 = 0, ASt1 = 1 << PortBitASt, ASt0 = 0, ADi1 = 1 << PortBitADi, ADi0 = 0, SM_MASK = (XSt1 | XDi1 | YSt1 | YDi1 | ZSt1 | ZDi1 | ASt1 | ADi1); 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*** 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; 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 */ 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() { 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; 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 pc.printf("\r\n*\n*\n"); #if defined (TARGET_KL25Z) pc.printf ("Found device Freescale KL25Z\r\n"); #endif #if defined (TARGET_KL46Z) pc.printf ("Found device Freescale KL46Z\r\n"); #endif #if defined (TARGET_MBED_LPC1768) pc.printf ("Found device MBED_LPC1768\r\n"); #endif 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()