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@1:66ee619f206b, 2014-02-06 (annotated)
- Committer:
- JonFreeman
- Date:
- Thu Feb 06 08:45:02 2014 +0000
- Revision:
- 1:66ee619f206b
- Parent:
- 0:5d0f270bfc87
- Child:
- 2:b3c668ec43ac
Currently creates 3 sets of Step and Dir signals for stepper motor drivers. Accepts via putty etc, as yet minimal set of 'G Codes' for CNC - G0, G1, G2 and G3. Still wip.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JonFreeman | 0:5d0f270bfc87 | 1 | #include "mbed.h" |
JonFreeman | 1:66ee619f206b | 2 | #include "rtos.h" |
JonFreeman | 0:5d0f270bfc87 | 3 | #include "cnc.h" |
JonFreeman | 0:5d0f270bfc87 | 4 | using namespace std; |
JonFreeman | 1:66ee619f206b | 5 | extern void command_line_interpreter (void const *) ; |
JonFreeman | 1:66ee619f206b | 6 | extern void lissajous (void const *) ; |
JonFreeman | 1:66ee619f206b | 7 | extern double feed_rate; |
JonFreeman | 1:66ee619f206b | 8 | extern void more_setup () ; |
JonFreeman | 0:5d0f270bfc87 | 9 | |
JonFreeman | 1:66ee619f206b | 10 | const int BAUD = 38400; |
JonFreeman | 0:5d0f270bfc87 | 11 | Serial pc(USBTX, USBRX); // tx, rx to pc |
JonFreeman | 1:66ee619f206b | 12 | Ticker NCO_gen; // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz) |
JonFreeman | 0:5d0f270bfc87 | 13 | struct digital_readouts dro; //some signed int |
JonFreeman | 0:5d0f270bfc87 | 14 | |
JonFreeman | 1:66ee619f206b | 15 | bool running = false, new_run_pending = false, idle = false, move_ended = false; |
JonFreeman | 1:66ee619f206b | 16 | volatile unsigned long ticks = 0L; // 32 bit count of "interrupt_period_us" interrupts from time t=0 |
JonFreeman | 1:66ee619f206b | 17 | unsigned long tickrun = 0L; // 32 bit effectively stores time in future when current movement to stop |
JonFreeman | 1:66ee619f206b | 18 | unsigned long ticks_next = 0L; // 32 bit effectively stores time in future when current movement to stop |
JonFreeman | 1:66ee619f206b | 19 | unsigned long millisecs = 0L; // 32 bit |
JonFreeman | 1:66ee619f206b | 20 | unsigned long pir_a = 0L, // Phase Increment Registers |
JonFreeman | 0:5d0f270bfc87 | 21 | pir_x = 0L, |
JonFreeman | 0:5d0f270bfc87 | 22 | pir_y = 0L, |
JonFreeman | 0:5d0f270bfc87 | 23 | pir_z = 0L, |
JonFreeman | 1:66ee619f206b | 24 | pir_a_next = 0L, // Data for next move assembled here |
JonFreeman | 1:66ee619f206b | 25 | pir_x_next = 0L, // during a move. |
JonFreeman | 1:66ee619f206b | 26 | pir_y_next = 0L, // This way, next move can start immediately |
JonFreeman | 1:66ee619f206b | 27 | pir_z_next = 0L, // on end of current move - minimised jerking |
JonFreeman | 1:66ee619f206b | 28 | pir_s = 0L; // Referenced only in command_interpreter as spindle speed setting |
JonFreeman | 1:66ee619f206b | 29 | |
JonFreeman | 0:5d0f270bfc87 | 30 | int spindlefwdrev = 0; // Takes values of 0 or 4 only |
JonFreeman | 0:5d0f270bfc87 | 31 | |
JonFreeman | 1:66ee619f206b | 32 | |
JonFreeman | 0:5d0f270bfc87 | 33 | #if defined (TARGET_KL25Z) |
JonFreeman | 1:66ee619f206b | 34 | DigitalOut intled (PTE1); //J2p20 |
JonFreeman | 1:66ee619f206b | 35 | DigitalOut charge_pump (PTE0); //J2p18 |
JonFreeman | 1:66ee619f206b | 36 | DigitalIn D25pin10 (PTD6); //jp217 |
JonFreeman | 1:66ee619f206b | 37 | DigitalIn D25pin11 (PTE31); //j2p13 |
JonFreeman | 1:66ee619f206b | 38 | DigitalIn D25pin12 (PTA17); //j2p11 |
JonFreeman | 1:66ee619f206b | 39 | DigitalIn D25pin13 (PTA16); //j2p9 |
JonFreeman | 1:66ee619f206b | 40 | DigitalIn D25pin15 (PTC17); //j2p7 |
JonFreeman | 0:5d0f270bfc87 | 41 | //SPISlave spidevice(PTD3, PTD2, PTD1, PTD0); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) |
JonFreeman | 0:5d0f270bfc87 | 42 | SPISlave spidevice(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) |
JonFreeman | 0:5d0f270bfc87 | 43 | // J2p08,J2p10,J2p12, J2p06 |
JonFreeman | 0:5d0f270bfc87 | 44 | //SPI spidevice(PTD2, PTD3, PTD1); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) |
JonFreeman | 0:5d0f270bfc87 | 45 | //SPI spidevice(PTD3, PTD2, PTD1); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) |
JonFreeman | 0:5d0f270bfc87 | 46 | //NOTE doubt possibly miso mosi in wrong order here, PTD3 and PTD2 |
JonFreeman | 0:5d0f270bfc87 | 47 | #define STEPPER_PORT PortC |
JonFreeman | 0:5d0f270bfc87 | 48 | const int PortBitXSt = 3, // Port bit num X Step J1P05 |
JonFreeman | 0:5d0f270bfc87 | 49 | PortBitXDi = 4, // Port bit num X Dir J1P07 |
JonFreeman | 0:5d0f270bfc87 | 50 | PortBitYSt = 5, // Port bit num Y Step J1P09 |
JonFreeman | 0:5d0f270bfc87 | 51 | PortBitYDi = 6, // Port bit num Y Dir J1P11 |
JonFreeman | 0:5d0f270bfc87 | 52 | PortBitZSt = 10, // Port bit num Z Step J1P13 |
JonFreeman | 0:5d0f270bfc87 | 53 | PortBitZDi = 11, // Port bit num Z Dir J1P15 |
JonFreeman | 0:5d0f270bfc87 | 54 | PortBitASt = 12, // Port bit num A Step J2P01 |
JonFreeman | 0:5d0f270bfc87 | 55 | PortBitADi = 13; // Port bit num A Dir J2P03 |
JonFreeman | 0:5d0f270bfc87 | 56 | #endif |
JonFreeman | 0:5d0f270bfc87 | 57 | #if defined (TARGET_KL46Z) |
JonFreeman | 1:66ee619f206b | 58 | DigitalOut intled (PTE1); //J2p20 |
JonFreeman | 1:66ee619f206b | 59 | DigitalOut charge_pump (PTE0); //J2p18 |
JonFreeman | 1:66ee619f206b | 60 | DigitalIn D25pin10 (PTB9);//d6 on 25 jp217 |
JonFreeman | 1:66ee619f206b | 61 | DigitalIn D25pin11 (PTA16);//e31 on 25 j2p13 |
JonFreeman | 1:66ee619f206b | 62 | DigitalIn D25pin12 (PTA15);//a17 on 20 j2p11 |
JonFreeman | 1:66ee619f206b | 63 | DigitalIn D25pin13 (PTA14);//a16 on 25 j2p9 |
JonFreeman | 1:66ee619f206b | 64 | DigitalIn D25pin15 (PTA6);//c17 on 25 j2p7 |
JonFreeman | 0:5d0f270bfc87 | 65 | SPISlave spidevice(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC) |
JonFreeman | 0:5d0f270bfc87 | 66 | // J2p13, J2p15, J2p11, J2p09 |
JonFreeman | 0:5d0f270bfc87 | 67 | // Easy way to allocate port bits for *** N O T CHECKED for 46Z *** |
JonFreeman | 0:5d0f270bfc87 | 68 | // output of stepper motor Step and DIR sigs |
JonFreeman | 0:5d0f270bfc87 | 69 | #define STEPPER_PORT PortC |
JonFreeman | 0:5d0f270bfc87 | 70 | const int PortBitXSt = 0, // Port bit num X Step J1P05 |
JonFreeman | 0:5d0f270bfc87 | 71 | PortBitXDi = 4, // Port bit num X Dir J1P07 |
JonFreeman | 0:5d0f270bfc87 | 72 | PortBitYSt = 6, // Port bit num Y Step J1P09 |
JonFreeman | 0:5d0f270bfc87 | 73 | PortBitYDi = 7, // Port bit num Y Dir J1P11 |
JonFreeman | 0:5d0f270bfc87 | 74 | PortBitZSt = 10, // Port bit num Z Step J1P13 |
JonFreeman | 0:5d0f270bfc87 | 75 | PortBitZDi = 11, // Port bit num Z Dir J1P15 |
JonFreeman | 0:5d0f270bfc87 | 76 | PortBitASt = 13, // Port bit num A Step J2P01 |
JonFreeman | 0:5d0f270bfc87 | 77 | PortBitADi = 16; // Port bit num A Dir J2P03 |
JonFreeman | 0:5d0f270bfc87 | 78 | #endif |
JonFreeman | 0:5d0f270bfc87 | 79 | #if defined (TARGET_MBED_LPC1768) |
JonFreeman | 1:66ee619f206b | 80 | DigitalOut intled(LED2); // Correct |
JonFreeman | 1:66ee619f206b | 81 | DigitalOut charge_pump (PTE0); //J2p18 Following 5 inputs all wrong - TO DO sort which pins |
JonFreeman | 1:66ee619f206b | 82 | DigitalIn D25pin10 (PTD6); //jp217 |
JonFreeman | 1:66ee619f206b | 83 | DigitalIn D25pin11 (PTE31); //j2p13 |
JonFreeman | 1:66ee619f206b | 84 | DigitalIn D25pin12 (PTA17); //j2p11 |
JonFreeman | 1:66ee619f206b | 85 | DigitalIn D25pin13 (PTA16); //j2p9 |
JonFreeman | 1:66ee619f206b | 86 | DigitalIn D25pin15 (PTC17); //j2p7 |
JonFreeman | 0:5d0f270bfc87 | 87 | SPISlave spidevice(p5, p6, p7, p8); |
JonFreeman | 0:5d0f270bfc87 | 88 | // Easy way to allocate port bits for *** N O T CHECKED for MBED_LPC1768 *** |
JonFreeman | 0:5d0f270bfc87 | 89 | // output of stepper motor Step and DIR sigs |
JonFreeman | 0:5d0f270bfc87 | 90 | #define STEPPER_PORT Port0 |
JonFreeman | 0:5d0f270bfc87 | 91 | /* Port 0 bits routed to DIP pins as follows:- |
JonFreeman | 0:5d0f270bfc87 | 92 | P0.00 p09 Reserve SDA |
JonFreeman | 0:5d0f270bfc87 | 93 | P0.01 p10 Reserve SCL |
JonFreeman | 0:5d0f270bfc87 | 94 | P0.04 p30 CAN rd - USE X Step |
JonFreeman | 0:5d0f270bfc87 | 95 | P0.05 p29 CAN td - USE X Dir |
JonFreeman | 0:5d0f270bfc87 | 96 | P0.10 p28 SDA - USE Y Step |
JonFreeman | 0:5d0f270bfc87 | 97 | P0.11 p27 SCL - USE Y Dir |
JonFreeman | 0:5d0f270bfc87 | 98 | P0.15 p13 Tx - USE Z Step |
JonFreeman | 0:5d0f270bfc87 | 99 | P0.16 p14 Rx - USE Z Dir |
JonFreeman | 0:5d0f270bfc87 | 100 | P0.17 p12 miso - USE A Step |
JonFreeman | 0:5d0f270bfc87 | 101 | P0.18 p11 mosi - Use A Dir |
JonFreeman | 0:5d0f270bfc87 | 102 | P0.23 p15 A In |
JonFreeman | 0:5d0f270bfc87 | 103 | P0.24 p16 A In |
JonFreeman | 0:5d0f270bfc87 | 104 | P0.25 p17 A In |
JonFreeman | 0:5d0f270bfc87 | 105 | P0.26 p18 Reserve A Out |
JonFreeman | 0:5d0f270bfc87 | 106 | */ |
JonFreeman | 0:5d0f270bfc87 | 107 | const int PortBitXSt = 4, // Port bit num X Step |
JonFreeman | 0:5d0f270bfc87 | 108 | PortBitXDi = 5, // Port bit num X Dir |
JonFreeman | 0:5d0f270bfc87 | 109 | PortBitYSt = 10, // Port bit num Y Step |
JonFreeman | 0:5d0f270bfc87 | 110 | PortBitYDi = 11, // Port bit num Y Dir |
JonFreeman | 0:5d0f270bfc87 | 111 | PortBitZSt = 15, // Port bit num Z Step |
JonFreeman | 0:5d0f270bfc87 | 112 | PortBitZDi = 16, // Port bit num Z Dir |
JonFreeman | 0:5d0f270bfc87 | 113 | PortBitASt = 17, // Port bit num A Step |
JonFreeman | 0:5d0f270bfc87 | 114 | PortBitADi = 18; // Port bit num A Dir |
JonFreeman | 0:5d0f270bfc87 | 115 | #endif |
JonFreeman | 0:5d0f270bfc87 | 116 | |
JonFreeman | 1:66ee619f206b | 117 | const long |
JonFreeman | 0:5d0f270bfc87 | 118 | XSt1 = 1 << PortBitXSt, XSt0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 119 | XDi1 = 1 << PortBitXDi, XDi0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 120 | YSt1 = 1 << PortBitYSt, YSt0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 121 | YDi1 = 1 << PortBitYDi, YDi0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 122 | ZSt1 = 1 << PortBitZSt, ZSt0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 123 | ZDi1 = 1 << PortBitZDi, ZDi0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 124 | ASt1 = 1 << PortBitASt, ASt0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 125 | ADi1 = 1 << PortBitADi, ADi0 = 0, |
JonFreeman | 0:5d0f270bfc87 | 126 | |
JonFreeman | 0:5d0f270bfc87 | 127 | SM_MASK = (XSt1 | XDi1 | YSt1 | YDi1 | ZSt1 | ZDi1 | ASt1 | ADi1); |
JonFreeman | 0:5d0f270bfc87 | 128 | |
JonFreeman | 0:5d0f270bfc87 | 129 | PortOut mysteppers(STEPPER_PORT, SM_MASK); |
JonFreeman | 0:5d0f270bfc87 | 130 | |
JonFreeman | 1:66ee619f206b | 131 | const int PIRBUFFSIZE = 10; |
JonFreeman | 1:66ee619f206b | 132 | class circbuff { |
JonFreeman | 1:66ee619f206b | 133 | private: |
JonFreeman | 1:66ee619f206b | 134 | pirbufgrain grain [PIRBUFFSIZE + 1]; |
JonFreeman | 1:66ee619f206b | 135 | int OnPtr, OffPtr; |
JonFreeman | 1:66ee619f206b | 136 | bool bufffull, buffempty, buffhalf; |
JonFreeman | 1:66ee619f206b | 137 | void setempty () { |
JonFreeman | 1:66ee619f206b | 138 | bufffull = false; |
JonFreeman | 1:66ee619f206b | 139 | buffhalf = false; |
JonFreeman | 1:66ee619f206b | 140 | buffempty = true; |
JonFreeman | 1:66ee619f206b | 141 | } |
JonFreeman | 1:66ee619f206b | 142 | void grain_copy (pirbufgrain & src, pirbufgrain & dest) { |
JonFreeman | 1:66ee619f206b | 143 | dest.x = src.x; |
JonFreeman | 1:66ee619f206b | 144 | dest.y = src.y; |
JonFreeman | 1:66ee619f206b | 145 | dest.z = src.z; |
JonFreeman | 1:66ee619f206b | 146 | dest.c = src.c; |
JonFreeman | 1:66ee619f206b | 147 | dest.f_rate = src.f_rate; // int feed rate mm per min * 1000 |
JonFreeman | 1:66ee619f206b | 148 | } |
JonFreeman | 1:66ee619f206b | 149 | public: |
JonFreeman | 1:66ee619f206b | 150 | void init () { |
JonFreeman | 1:66ee619f206b | 151 | OnPtr = OffPtr = 0; |
JonFreeman | 1:66ee619f206b | 152 | setempty (); |
JonFreeman | 1:66ee619f206b | 153 | } |
JonFreeman | 1:66ee619f206b | 154 | int On_Q () { |
JonFreeman | 1:66ee619f206b | 155 | int k; |
JonFreeman | 1:66ee619f206b | 156 | if (bufffull) return PIRBUFFSIZE; |
JonFreeman | 1:66ee619f206b | 157 | k = OnPtr - OffPtr; |
JonFreeman | 1:66ee619f206b | 158 | if (k < 0) k += PIRBUFFSIZE; |
JonFreeman | 1:66ee619f206b | 159 | if (k > PIRBUFFSIZE / 2) |
JonFreeman | 1:66ee619f206b | 160 | buffhalf = true; |
JonFreeman | 1:66ee619f206b | 161 | else |
JonFreeman | 1:66ee619f206b | 162 | buffhalf = false; |
JonFreeman | 1:66ee619f206b | 163 | return k; |
JonFreeman | 1:66ee619f206b | 164 | } |
JonFreeman | 1:66ee619f206b | 165 | bool readable () {return !buffempty; } |
JonFreeman | 1:66ee619f206b | 166 | bool writeable () {return !bufffull; } |
JonFreeman | 1:66ee619f206b | 167 | bool read (pirbufgrain & g) { |
JonFreeman | 1:66ee619f206b | 168 | if (buffempty) return false; |
JonFreeman | 1:66ee619f206b | 169 | bufffull = false; |
JonFreeman | 1:66ee619f206b | 170 | grain_copy (grain[OffPtr++], g); |
JonFreeman | 1:66ee619f206b | 171 | if (OffPtr >= PIRBUFFSIZE) |
JonFreeman | 1:66ee619f206b | 172 | OffPtr = 0; |
JonFreeman | 1:66ee619f206b | 173 | if (OnPtr == OffPtr) |
JonFreeman | 1:66ee619f206b | 174 | buffempty = true; |
JonFreeman | 1:66ee619f206b | 175 | return true; |
JonFreeman | 1:66ee619f206b | 176 | } |
JonFreeman | 1:66ee619f206b | 177 | bool write (pirbufgrain & g) { |
JonFreeman | 1:66ee619f206b | 178 | if (bufffull) return false; |
JonFreeman | 1:66ee619f206b | 179 | buffempty = false; |
JonFreeman | 1:66ee619f206b | 180 | grain_copy (g, grain[OnPtr++]); |
JonFreeman | 1:66ee619f206b | 181 | if (OnPtr >= PIRBUFFSIZE) |
JonFreeman | 1:66ee619f206b | 182 | OnPtr = 0; |
JonFreeman | 1:66ee619f206b | 183 | if (OnPtr == OffPtr) |
JonFreeman | 1:66ee619f206b | 184 | bufffull = true; |
JonFreeman | 1:66ee619f206b | 185 | return true; |
JonFreeman | 1:66ee619f206b | 186 | } |
JonFreeman | 1:66ee619f206b | 187 | } CircBuff; |
JonFreeman | 1:66ee619f206b | 188 | |
JonFreeman | 1:66ee619f206b | 189 | int PutMoveOnList (struct pirbufgrain & s) { |
JonFreeman | 1:66ee619f206b | 190 | while (!CircBuff.writeable()) |
JonFreeman | 1:66ee619f206b | 191 | osThreadYield(); |
JonFreeman | 1:66ee619f206b | 192 | CircBuff.write (s); // pc.printf("CircBuff, contains %d\r\n", CircBuff.On_Q()); |
JonFreeman | 1:66ee619f206b | 193 | return 0; |
JonFreeman | 1:66ee619f206b | 194 | } |
JonFreeman | 1:66ee619f206b | 195 | |
JonFreeman | 1:66ee619f206b | 196 | |
JonFreeman | 0:5d0f270bfc87 | 197 | /* |
JonFreeman | 0:5d0f270bfc87 | 198 | * Interrupt Service Routine |
JonFreeman | 0:5d0f270bfc87 | 199 | */ |
JonFreeman | 0:5d0f270bfc87 | 200 | void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR*** |
JonFreeman | 1:66ee619f206b | 201 | static const int millisec_countdown = 1000 / interrupt_period_us; |
JonFreeman | 0:5d0f270bfc87 | 202 | const long bit_lutx[4] = {XSt0 | XDi0, XSt0 | XDi1, XSt1 | XDi1, XSt1 | XDi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs |
JonFreeman | 0:5d0f270bfc87 | 203 | bit_luty[4] = {YSt0 | YDi0, YSt0 | YDi1, YSt1 | YDi1, YSt1 | YDi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs |
JonFreeman | 0:5d0f270bfc87 | 204 | bit_lutz[4] = {ZSt0 | ZDi0, ZSt0 | ZDi1, ZSt1 | ZDi1, ZSt1 | ZDi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs |
JonFreeman | 0:5d0f270bfc87 | 205 | bit_luta[4] = {ASt0 | ADi0, ASt0 | ADi1, ASt1 | ADi1, ASt1 | ADi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs |
JonFreeman | 0:5d0f270bfc87 | 206 | bits2shift = (sizeof (long) << 3) - 2; |
JonFreeman | 0:5d0f270bfc87 | 207 | static unsigned long |
JonFreeman | 0:5d0f270bfc87 | 208 | // acc_s = 0L, // For Spindle motor, probably not needed as may be pwm |
JonFreeman | 0:5d0f270bfc87 | 209 | acc_a = 0L, |
JonFreeman | 0:5d0f270bfc87 | 210 | acc_x = 0L, |
JonFreeman | 0:5d0f270bfc87 | 211 | acc_y = 0L, |
JonFreeman | 0:5d0f270bfc87 | 212 | acc_z = 0L; |
JonFreeman | 1:66ee619f206b | 213 | static int obitz = 0, mscount = millisec_countdown; |
JonFreeman | 0:5d0f270bfc87 | 214 | int oldbitz, acts; |
JonFreeman | 0:5d0f270bfc87 | 215 | |
JonFreeman | 0:5d0f270bfc87 | 216 | intled = 1; // LED on for duration of interrupt service - point for scope probing |
JonFreeman | 0:5d0f270bfc87 | 217 | ticks++; // count of interrupts serviced |
JonFreeman | 1:66ee619f206b | 218 | if(!--mscount) { // Maintain global counter of elapsed milli seconds |
JonFreeman | 1:66ee619f206b | 219 | mscount = millisec_countdown; |
JonFreeman | 1:66ee619f206b | 220 | millisecs++; |
JonFreeman | 0:5d0f270bfc87 | 221 | } |
JonFreeman | 1:66ee619f206b | 222 | if (running) { |
JonFreeman | 1:66ee619f206b | 223 | acc_x += pir_x; // Update phase of signals in accumulators |
JonFreeman | 1:66ee619f206b | 224 | acc_y += pir_y; |
JonFreeman | 1:66ee619f206b | 225 | acc_z += pir_z; |
JonFreeman | 1:66ee619f206b | 226 | acc_a += pir_a; // not yet implemented |
JonFreeman | 1:66ee619f206b | 227 | // acc_s += pir_s; // pir_s used for spindle speed |
JonFreeman | 1:66ee619f206b | 228 | oldbitz = obitz; // pin output levels as determined during previous interrut |
JonFreeman | 1:66ee619f206b | 229 | obitz = bit_lutx[acc_x >> bits2shift] | bit_luty[acc_y >> bits2shift] | bit_lutz[acc_z >> bits2shift] | bit_luta[acc_a >> bits2shift]; |
JonFreeman | 1:66ee619f206b | 230 | |
JonFreeman | 1:66ee619f206b | 231 | mysteppers = obitz; // Output signals to stepper motor drivers, next look for _- pos clk events on 'Step' outputs |
JonFreeman | 1:66ee619f206b | 232 | |
JonFreeman | 1:66ee619f206b | 233 | acts = (~oldbitz & obitz); // get pos clk edge triggers 'Step' bits |
JonFreeman | 1:66ee619f206b | 234 | acts |= (obitz & (XDi1 | YDi1 | ZDi1)); // get axis X, Y and Z Direction bits |
JonFreeman | 1:66ee619f206b | 235 | if(acts & XSt1) { // got pos clk edge for axis X |
JonFreeman | 1:66ee619f206b | 236 | if (acts & XDi1) |
JonFreeman | 1:66ee619f206b | 237 | dro.x++; |
JonFreeman | 1:66ee619f206b | 238 | else dro.x--; |
JonFreeman | 1:66ee619f206b | 239 | } |
JonFreeman | 1:66ee619f206b | 240 | if(acts & YSt1) { // got pos clk edge for axis Y |
JonFreeman | 1:66ee619f206b | 241 | if (acts & YDi1) |
JonFreeman | 1:66ee619f206b | 242 | dro.y++; |
JonFreeman | 1:66ee619f206b | 243 | else dro.y--; |
JonFreeman | 1:66ee619f206b | 244 | } |
JonFreeman | 1:66ee619f206b | 245 | if(acts & ZSt1) { // got pos clk edge for axis Z |
JonFreeman | 1:66ee619f206b | 246 | if (acts & ZDi1) |
JonFreeman | 1:66ee619f206b | 247 | dro.z++; |
JonFreeman | 1:66ee619f206b | 248 | else dro.z--; |
JonFreeman | 1:66ee619f206b | 249 | } |
JonFreeman | 1:66ee619f206b | 250 | if (tickrun <= ticks) { // End of a machine movement detected, start next move here if possible |
JonFreeman | 1:66ee619f206b | 251 | running = false; |
JonFreeman | 1:66ee619f206b | 252 | move_ended = true; |
JonFreeman | 1:66ee619f206b | 253 | pir_x = 0L; // stop all stepper motors |
JonFreeman | 1:66ee619f206b | 254 | pir_y = 0L; |
JonFreeman | 1:66ee619f206b | 255 | pir_z = 0L; |
JonFreeman | 1:66ee619f206b | 256 | pir_a = 0L; |
JonFreeman | 1:66ee619f206b | 257 | // ticks = 0L; // Simply to avoid having to think about overflow problems |
JonFreeman | 1:66ee619f206b | 258 | } |
JonFreeman | 0:5d0f270bfc87 | 259 | } |
JonFreeman | 1:66ee619f206b | 260 | else { // Not running. Grab next data here when or if available |
JonFreeman | 1:66ee619f206b | 261 | if (new_run_pending) { // Pick up on flag set elsewhere |
JonFreeman | 1:66ee619f206b | 262 | pir_a = pir_a_next; |
JonFreeman | 1:66ee619f206b | 263 | pir_x = pir_x_next; |
JonFreeman | 1:66ee619f206b | 264 | pir_y = pir_y_next; |
JonFreeman | 1:66ee619f206b | 265 | pir_z = pir_z_next; |
JonFreeman | 1:66ee619f206b | 266 | tickrun = ticks + ticks_next; |
JonFreeman | 1:66ee619f206b | 267 | running = true; // Start the new run |
JonFreeman | 1:66ee619f206b | 268 | new_run_pending = false; // Clear the flag which initiated this update |
JonFreeman | 1:66ee619f206b | 269 | idle = false; |
JonFreeman | 1:66ee619f206b | 270 | } |
JonFreeman | 0:5d0f270bfc87 | 271 | } |
JonFreeman | 1:66ee619f206b | 272 | charge_pump = ticks & 0x02; |
JonFreeman | 0:5d0f270bfc87 | 273 | intled = 0; // LED off |
JonFreeman | 0:5d0f270bfc87 | 274 | } // end of interrupt handler |
JonFreeman | 0:5d0f270bfc87 | 275 | |
JonFreeman | 0:5d0f270bfc87 | 276 | /* |
JonFreeman | 0:5d0f270bfc87 | 277 | * End of Interrupt Service Routine |
JonFreeman | 0:5d0f270bfc87 | 278 | */ |
JonFreeman | 0:5d0f270bfc87 | 279 | |
JonFreeman | 0:5d0f270bfc87 | 280 | |
JonFreeman | 1:66ee619f206b | 281 | void newpir_updater (void const * name) { |
JonFreeman | 1:66ee619f206b | 282 | static long x, y, z, count = 0; |
JonFreeman | 1:66ee619f206b | 283 | struct pirbufgrain outs; |
JonFreeman | 1:66ee619f206b | 284 | pc.printf("Arrived at newpir_updater\r\n"); |
JonFreeman | 1:66ee619f206b | 285 | while (true) { |
JonFreeman | 1:66ee619f206b | 286 | while (!move_ended || !CircBuff.readable()) { |
JonFreeman | 1:66ee619f206b | 287 | osThreadYield(); |
JonFreeman | 1:66ee619f206b | 288 | } |
JonFreeman | 1:66ee619f206b | 289 | CircBuff.read(outs); |
JonFreeman | 1:66ee619f206b | 290 | x = (long)(outs.f_rate * outs.x); // These take much CPU time !! |
JonFreeman | 1:66ee619f206b | 291 | y = (long)(outs.f_rate * outs.y); |
JonFreeman | 1:66ee619f206b | 292 | z = (long)(outs.f_rate * outs.z); |
JonFreeman | 1:66ee619f206b | 293 | ticks_next = (unsigned long)(outs.c / outs.f_rate); |
JonFreeman | 1:66ee619f206b | 294 | pir_x_next = x; |
JonFreeman | 1:66ee619f206b | 295 | pir_y_next = y; |
JonFreeman | 1:66ee619f206b | 296 | pir_z_next = z; |
JonFreeman | 1:66ee619f206b | 297 | move_ended = false; |
JonFreeman | 1:66ee619f206b | 298 | new_run_pending = true; // cleared and 'running' flag set in interrupt handler |
JonFreeman | 1:66ee619f206b | 299 | idle = false; |
JonFreeman | 1:66ee619f206b | 300 | count++; |
JonFreeman | 1:66ee619f206b | 301 | // pc.printf("CircB tot %d\r\n", count); |
JonFreeman | 1:66ee619f206b | 302 | } |
JonFreeman | 0:5d0f270bfc87 | 303 | } |
JonFreeman | 0:5d0f270bfc87 | 304 | |
JonFreeman | 1:66ee619f206b | 305 | |
JonFreeman | 1:66ee619f206b | 306 | class digital_readout_stuff { // class does not need to be named here |
JonFreeman | 1:66ee619f206b | 307 | char * readout (char * txt, long p) // p has running subtotal of all pulses issued to stepper driver |
JonFreeman | 1:66ee619f206b | 308 | { |
JonFreeman | 1:66ee619f206b | 309 | txt[0] = '+'; // constructs string e.g. "+123.456" |
JonFreeman | 1:66ee619f206b | 310 | txt[8] = 0; // null terminated |
JonFreeman | 1:66ee619f206b | 311 | if (p < 0) { |
JonFreeman | 1:66ee619f206b | 312 | txt[0] = '-'; |
JonFreeman | 1:66ee619f206b | 313 | p = -p; |
JonFreeman | 1:66ee619f206b | 314 | } |
JonFreeman | 1:66ee619f206b | 315 | p *= 1000; |
JonFreeman | 1:66ee619f206b | 316 | p /= pulses_per_mm; |
JonFreeman | 1:66ee619f206b | 317 | if (p > 999999) { |
JonFreeman | 1:66ee619f206b | 318 | sprintf(txt + 1, "OVRANGE"); |
JonFreeman | 1:66ee619f206b | 319 | return txt; |
JonFreeman | 1:66ee619f206b | 320 | } |
JonFreeman | 1:66ee619f206b | 321 | for(int k = 7; k > 0; k--) { |
JonFreeman | 1:66ee619f206b | 322 | if (k == 4) |
JonFreeman | 1:66ee619f206b | 323 | txt[k] = '.'; |
JonFreeman | 1:66ee619f206b | 324 | else { |
JonFreeman | 1:66ee619f206b | 325 | txt[k] = '0' + (p % 10); |
JonFreeman | 1:66ee619f206b | 326 | p /= 10; |
JonFreeman | 1:66ee619f206b | 327 | } |
JonFreeman | 1:66ee619f206b | 328 | } |
JonFreeman | 1:66ee619f206b | 329 | return txt; // Returns pointer unaltered for subsequent use by e.g. cout |
JonFreeman | 1:66ee619f206b | 330 | } |
JonFreeman | 1:66ee619f206b | 331 | public: |
JonFreeman | 1:66ee619f206b | 332 | void update () { |
JonFreeman | 1:66ee619f206b | 333 | static long t = 0; |
JonFreeman | 1:66ee619f206b | 334 | if (millisecs < t) |
JonFreeman | 1:66ee619f206b | 335 | return; |
JonFreeman | 1:66ee619f206b | 336 | if(!idle && dro.dro_output) { |
JonFreeman | 1:66ee619f206b | 337 | char txt[12]; |
JonFreeman | 1:66ee619f206b | 338 | pc.printf("dros X %s,", readout(txt, dro.x)); // dro.n has running subtotal of all pulses issued to stepper driver.n |
JonFreeman | 1:66ee619f206b | 339 | pc.printf(" Y %s, Z ", readout(txt, dro.y)); |
JonFreeman | 1:66ee619f206b | 340 | pc.printf("%s, %s\r\n", readout(txt, dro.z), running ? "runn":"idle"); |
JonFreeman | 1:66ee619f206b | 341 | if(!running) idle = true; // Purpose of idle flag is to stop dro after run completes. |
JonFreeman | 1:66ee619f206b | 342 | t = millisecs + 350; // Schedule next update after this non-blocking delay |
JonFreeman | 1:66ee619f206b | 343 | } |
JonFreeman | 1:66ee619f206b | 344 | } |
JonFreeman | 1:66ee619f206b | 345 | } dro_out ; |
JonFreeman | 1:66ee619f206b | 346 | |
JonFreeman | 1:66ee619f206b | 347 | /*void taskone (void const * name) |
JonFreeman | 1:66ee619f206b | 348 | { |
JonFreeman | 1:66ee619f206b | 349 | static int i = 0; |
JonFreeman | 1:66ee619f206b | 350 | while (true) { |
JonFreeman | 1:66ee619f206b | 351 | pc.printf("%s %d\r\n", name, i++); |
JonFreeman | 1:66ee619f206b | 352 | Thread::wait(9500); |
JonFreeman | 1:66ee619f206b | 353 | osThreadYield(); |
JonFreeman | 1:66ee619f206b | 354 | } |
JonFreeman | 1:66ee619f206b | 355 | } |
JonFreeman | 1:66ee619f206b | 356 | |
JonFreeman | 1:66ee619f206b | 357 | void tasktwo (void const * name) |
JonFreeman | 1:66ee619f206b | 358 | { |
JonFreeman | 1:66ee619f206b | 359 | pc.printf("Task Two runs once and exits\r\n"); |
JonFreeman | 1:66ee619f206b | 360 | Thread::wait(700); |
JonFreeman | 1:66ee619f206b | 361 | osThreadYield(); |
JonFreeman | 1:66ee619f206b | 362 | } |
JonFreeman | 1:66ee619f206b | 363 | |
JonFreeman | 1:66ee619f206b | 364 | void taskthree (void const * name) |
JonFreeman | 1:66ee619f206b | 365 | { |
JonFreeman | 1:66ee619f206b | 366 | static int i = 0; |
JonFreeman | 1:66ee619f206b | 367 | while (true) { |
JonFreeman | 1:66ee619f206b | 368 | pc.printf("%s %d\r\n", name, i++); |
JonFreeman | 1:66ee619f206b | 369 | Thread::wait(3500); |
JonFreeman | 1:66ee619f206b | 370 | osThreadYield(); |
JonFreeman | 1:66ee619f206b | 371 | } |
JonFreeman | 1:66ee619f206b | 372 | }*/ |
JonFreeman | 1:66ee619f206b | 373 | |
JonFreeman | 0:5d0f270bfc87 | 374 | int main() { |
JonFreeman | 0:5d0f270bfc87 | 375 | pc.baud(BAUD); // comms to 'PuTTY' serial terminal via mbed usb |
JonFreeman | 0:5d0f270bfc87 | 376 | dro.x = dro.y = dro.z = 0; // These dro registers count pulses delivered to stepper motor driver |
JonFreeman | 0:5d0f270bfc87 | 377 | dro.dro_output = true; |
JonFreeman | 1:66ee619f206b | 378 | more_setup () ; // Zeros one 'pirs' structure 'last_position' |
JonFreeman | 1:66ee619f206b | 379 | CircBuff.init (); |
JonFreeman | 0:5d0f270bfc87 | 380 | spidevice.format(8, 0); // 8 bits mode 0, // p11 mosi, p12 miso, p13 sclk ** ONLY 8 BIT ** |
JonFreeman | 0:5d0f270bfc87 | 381 | spidevice.frequency(12000000); // 12MHz bit rate |
JonFreeman | 1:66ee619f206b | 382 | pc.printf("\r\n*\n*\n"); |
JonFreeman | 0:5d0f270bfc87 | 383 | #if defined (TARGET_KL25Z) |
JonFreeman | 0:5d0f270bfc87 | 384 | pc.printf ("Found device Freescale KL25Z\r\n"); |
JonFreeman | 0:5d0f270bfc87 | 385 | #endif |
JonFreeman | 0:5d0f270bfc87 | 386 | #if defined (TARGET_KL46Z) |
JonFreeman | 0:5d0f270bfc87 | 387 | pc.printf ("Found device Freescale KL46Z\r\n"); |
JonFreeman | 0:5d0f270bfc87 | 388 | #endif |
JonFreeman | 0:5d0f270bfc87 | 389 | #if defined (TARGET_MBED_LPC1768) |
JonFreeman | 0:5d0f270bfc87 | 390 | pc.printf ("Found device MBED_LPC1768\r\n"); |
JonFreeman | 0:5d0f270bfc87 | 391 | #endif |
JonFreeman | 1:66ee619f206b | 392 | pc.printf("Welcome to the CNC tester\r\nStep pulses required to move 1.0mm = %9.0f\r\n", pulses_per_mm); |
JonFreeman | 1:66ee619f206b | 393 | 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); |
JonFreeman | 1:66ee619f206b | 394 | NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, (long)interrupt_period_us);// Have setup timed interrupts, let other code deal |
JonFreeman | 1:66ee619f206b | 395 | // Thread threadnametaskone (taskone, (void *)"task one stuff"); |
JonFreeman | 1:66ee619f206b | 396 | // Thread t8 (tasktwo, (void *)"task two"); |
JonFreeman | 1:66ee619f206b | 397 | Thread tsr2 (newpir_updater, (void *)"read from CircBuff and move"); |
JonFreeman | 1:66ee619f206b | 398 | // Thread tthree (taskthree, (void *)"task three"); |
JonFreeman | 1:66ee619f206b | 399 | Thread patterngen (lissajous, (void *)"Loading Lissajous") ; |
JonFreeman | 1:66ee619f206b | 400 | Thread comlin (command_line_interpreter, (void *)"cli"); // Read any instructions arriving via serial port and act upon them |
JonFreeman | 1:66ee619f206b | 401 | pc.printf("Added cli thread\r\n"); |
JonFreeman | 1:66ee619f206b | 402 | move_ended = true; // Needed to kickstart system |
JonFreeman | 1:66ee619f206b | 403 | |
JonFreeman | 1:66ee619f206b | 404 | while(1) { // Round Robin loop |
JonFreeman | 1:66ee619f206b | 405 | dro_out.update (); // Update DRO readings if, and as often as needed |
JonFreeman | 1:66ee619f206b | 406 | osThreadYield(); // |
JonFreeman | 1:66ee619f206b | 407 | } // end of Round Robin loop |
JonFreeman | 1:66ee619f206b | 408 | } // end of int main() |
JonFreeman | 1:66ee619f206b | 409 |