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-01-31
- Revision:
- 0:5d0f270bfc87
- Child:
- 1:66ee619f206b
File content as of revision 0:5d0f270bfc87:
#include "mbed.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 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 struct digital_readouts dro; //some signed int bool running = false; volatile unsigned long ticks = 0L; unsigned long pir_a = 0L, pir_x = 0L, pir_y = 0L, pir_z = 0L, pir_s = 0L, // Referenced only in command_interpreter as spindle speed setting tickrun = 0L; int spindlefwdrev = 0; // Takes values of 0 or 4 only #if defined (TARGET_KL25Z) DigitalOut intled(PTE1); //J2p20 //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 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); 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 static 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); /* * 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*** 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; 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(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 (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; } intled = 0; // LED off } // end of interrupt handler /* * End of Interrupt Service Routine */ 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); } 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 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 (); #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"); #endif #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); } } }