Jon Freeman
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
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "rtos.h" 00003 #include "MODSERIAL.h" 00004 #include "cnc.h" 00005 extern void i2c_handler (void const *); 00006 extern void command_line_interpreter (void const *) ; 00007 extern fl_typ feed_rate; // float type is 'float' 00008 extern signed long spindle_rpm; 00009 00010 const int BAUD = 38400; 00011 MODSERIAL pc(USBTX, USBRX); // tx, rx to pc via usb lead 00012 Ticker NCO_gen; // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz) 00013 Ticker msec; // Ticker updating global millisecs counter 00014 00015 bool running = false, 00016 new_run_pending = false, 00017 idle = false, 00018 move_ended = false; 00019 00020 volatile unsigned long ticks = 0L; // 32 bit count of "interrupt_period_us" interrupts from time t=0 00021 unsigned long tickrun = 0L, // 32 bit effectively stores time in future when current movement to stop 00022 ticks_next = 0L, // 32 bit effectively stores time in future when current movement to stop 00023 millisecs = 0L; // 32 bit 00024 signed long 00025 #if defined Fourth_Axis 00026 pir_a_next = 0L, // Data for next move assembled here 00027 #endif 00028 pir_x_next = 0L, // during a move. 00029 pir_y_next = 0L, // This way, next move can start immediately 00030 pir_z_next = 0L, // on end of current move - minimised jerking 00031 #if defined Fourth_Axis 00032 inc_a_next = 1L, 00033 #endif 00034 inc_x_next = 1L, 00035 inc_y_next = 1L, 00036 inc_z_next = 1L, 00037 00038 dir_bits_next = 0L, 00039 pir_spin = 0L; // Referenced only in command_interpreter as spindle speed setting 00040 00041 struct Gparams last_position; // 00042 00043 00044 #if defined (TARGET_KL25Z) 00045 const char Target[] = "KL25Z"; // Note need PTE0 (sda) and PTE1 (scl) 00046 DigitalOut intled (PTD7); //(PTE1); //J2p19, was 20 00047 DigitalOut charge_pumpD25pin1 (PTD6); //(PTE0); //J2p17, was 18 00048 // InterruptIn D25pin10_EStop (PTE20); //j10p1 KL25 J10 is KL46 j4 00049 DigitalIn D25pin10_EStop (PTE20); //j10p1 KL25 J10 is KL46 j4 00050 DigitalIn D25pin11_XLim (PTE21); //j10p3 00051 DigitalIn D25pin12_YLim (PTE22); //j10p5 00052 DigitalIn D25pin13_ZLim (PTE23); //j10p7 00053 DigitalIn D25pin15_unkn (PTE30); //j10p11 00054 #if defined I2C_Enable 00055 I2CSlave slave(PTE0, PTE1); // PTE0 sda, (yellow) PTE1 scl (blue) 00056 #endif 00057 #if defined SPI_Enable 00058 SPISlave spidevice(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk (uses p11, p12, p13 on mbed LPC1768) 00059 #endif 00060 // J2p08,J2p10,J2p12, J2p06 00061 #define STEPPER_PORT PortC 00062 const int PortBitXSt = 3, // Port bit num X Step J1P05 D25pin 2 00063 PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3 00064 PortBitYSt = 5, // Port bit num Y Step J1P09 D25pin 4 00065 PortBitYDi = 6, // Port bit num Y Dir J1P11 D25pin 5 00066 PortBitZSt = 10, // Port bit num Z Step J1P13 D25pin 6 00067 PortBitZDi = 11, // Port bit num Z Dir J1P15 D25pin 7 00068 PortBitASt = 12, // Port bit num A Step J2P01 D25pin 8 00069 PortBitADi = 13, // Port bit num A Dir J2P03 D25pin 9 00070 PortBitSSt = 8, // Port bit num Spin Step J1P14 D25pin 14 00071 PortBitSDi = 9; // Port bit num Spin Dir J1P16 D25pin 16 00072 #endif 00073 00074 00075 00076 #if defined (TARGET_KL46Z) 00077 const char Target[] = "KL46Z"; 00078 DigitalOut intled (PTE1); //J2p20 checked 00079 00080 00081 DigitalOut charge_pumpD25pin1 (PTE0); //J2p18 checked 00082 // InterruptIn D25pin10_EStop (PTE20); // j4p1 KL46 J4 is KL25 J10 00083 DigitalIn D25pin10_EStop (PTE20); // j4p1 KL46 J4 is KL25 J10 checked 00084 DigitalIn D25pin11_XLim (PTE21); // j4p3 checked 00085 DigitalIn D25pin12_YLim (PTE22); // j4p5 checked 00086 DigitalIn D25pin13_ZLim (PTE23); // j4p7 checked 00087 DigitalIn D25pin15_unkn (PTE30); // j4p11 checked 00088 #if defined I2C_Enable 00089 I2CSlave slave(p9, p10); 00090 #endif 00091 #if defined SPI_Enable 00092 SPISlave spidevice(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC) 00093 #endif 00094 // J2p13, J2p15, J2p11, J2p09 00095 // Easy way to allocate port bits for 00096 // output of stepper motor Step and DIR sigs 00097 #define STEPPER_PORT PortC 00098 const int PortBitXSt = 0, // Port bit num X Step J1P05 D25pin 2 checked 00099 PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3 checked 00100 PortBitYSt = 6, // Port bit num Y Step J1P09 D25pin 4 checked 00101 PortBitYDi = 7, // Port bit num Y Dir J1P11 D25pin 5 checked 00102 PortBitZSt = 10, // Port bit num Z Step J1P13 D25pin 6 checked 00103 PortBitZDi = 11, // Port bit num Z Dir J1P15 D25pin 7 checked 00104 PortBitASt = 13, // Port bit num A Step J2P01 D25pin 8 checked 00105 PortBitADi = 16, // Port bit num A Dir J2P03 D25pin 9 checked 00106 PortBitSSt = 8, // Port bit num Spin Step J1P14 D25pin 14 checked 00107 PortBitSDi = 9; // Port bit num Spin Dir J1P16 D25pin 16 checked 00108 #endif 00109 #if defined (TARGET_MBED_LPC1768) 00110 const char Target[] = "MBED LPC1768"; 00111 DigitalOut intled(LED2); // Correct 00112 DigitalOut charge_pumpD25pin1 (p25); // 00113 // InterruptIn D25pin10_EStop (p26); //P2.0 00114 DigitalIn D25pin10_EStop (p26); //P2.0 00115 DigitalIn D25pin11_XLim (p24); //P2.2 00116 DigitalIn D25pin12_YLim (p23); //P2.3 00117 DigitalIn D25pin13_ZLim (p19); //P1.30 00118 DigitalIn D25pin15_unkn (p20); //P1.31 00119 #if defined I2C_Enable 00120 I2CSlave slave(p9, p10); 00121 #endif 00122 #if defined SPI_Enable 00123 SPISlave spidevice(p5, p6, p7, p8); 00124 #endif 00125 // Easy way to allocate port bits 00126 // output of stepper motor Step and DIR sigs 00127 #define STEPPER_PORT Port0 00128 /* Port 0 bits routed to DIP pins as follows:- 00129 P0.00 p09 Reserve SDA 00130 P0.01 p10 Reserve SCL 00131 P0.04 p30 CAN rd - USE X Step D25pin 2 00132 P0.05 p29 CAN td - USE X Dir D25pin 3 00133 P0.10 p28 SDA - USE Y Step D25pin 4 00134 P0.11 p27 SCL - USE Y Dir D25pin 5 00135 P0.15 p13 Tx - USE Z Step D25pin 6 00136 P0.16 p14 Rx - USE Z Dir D25pin 7 00137 P0.17 p12 miso - USE A Step D25pin 8 00138 P0.18 p11 mosi - Use A Dir D25pin 9 00139 P0.23 p15 A In - Use S Step D25pin 14 00140 P0.24 p16 A In - Use S Dir D25pin 16 00141 P0.25 p17 Reserve A In 00142 P0.26 p18 Reserve A Out 00143 */ 00144 const int PortBitXSt = 4, // Port bit num X Step 00145 PortBitXDi = 5, // Port bit num X Dir 00146 PortBitYSt = 10, // Port bit num Y Step 00147 PortBitYDi = 11, // Port bit num Y Dir 00148 PortBitZSt = 15, // Port bit num Z Step 00149 PortBitZDi = 16, // Port bit num Z Dir 00150 PortBitASt = 17, // Port bit num A Step 00151 PortBitADi = 18, // Port bit num A Dir 00152 PortBitSSt = 23, // Port bit num Spin Step 00153 PortBitSDi = 24; // Port bit num Spin Dir 00154 #endif 00155 00156 const long // Assemble mask bits from now known port bit positions 00157 XSt = 1 << PortBitXSt, // X axis Step signal 00158 XDi = 1 << PortBitXDi, // X axis Direction signal 00159 YSt = 1 << PortBitYSt, // Y axis Step, etc 00160 YDi = 1 << PortBitYDi, 00161 ZSt = 1 << PortBitZSt, // Z axis 00162 ZDi = 1 << PortBitZDi, 00163 ASt = 1 << PortBitASt, // A axis, not implemented in full, for e.g. rotary axis 00164 ADi = 1 << PortBitADi, 00165 SDi = 1 << PortBitSDi, // Spindle, also driven by Step and Dir signals up to 5kHz 00166 SSt = 1 << PortBitSSt, // for 5000 RPM 00167 00168 SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt), 00169 direction_swappers = XDi | YDi | ZDi | SDi; // include bit to swap direction 00170 00171 PortOut Steppers (STEPPER_PORT, SM_MASK); 00172 00173 void target_cmd (struct singleGparam * a) { 00174 pc.printf("Computer is %s\r\n", Target); 00175 } 00176 00177 void grain_clr (struct singleGparam & g) { 00178 g.flt = 0.0; 00179 g.ul = 0L; 00180 g.i = g.c = 0; 00181 g.changed = false; 00182 } 00183 void Gparams_clr (struct Gparams & p) { 00184 grain_clr (p.x); grain_clr (p.y); grain_clr (p.z); grain_clr (p.i); grain_clr (p.j); 00185 grain_clr (p.r); grain_clr (p.a); grain_clr (p.b); grain_clr (p.c); grain_clr (p.d); 00186 } 00187 00188 class digital_readout_stuff { // class does not need to be named here 00189 private: 00190 char * readout (char * txt, long p) // p has running subtotal of all pulses issued to stepper driver 00191 { 00192 txt[0] = '+'; // constructs string e.g. "+123.456" 00193 txt[8] = 0; // null terminated 00194 if (p < 0) { 00195 txt[0] = '-'; 00196 p = -p; 00197 } 00198 p *= 1000; 00199 p /= pulses_per_mm; 00200 if (p > 999999) { 00201 sprintf(txt + 1, "OVRANGE"); 00202 return txt; 00203 } 00204 for(int k = 7; k > 0; k--) { 00205 if (k == 4) 00206 txt[k] = '.'; 00207 else { 00208 txt[k] = '0' + (p % 10); 00209 p /= 10; 00210 } 00211 } 00212 return txt; // Returns pointer unaltered for subsequent use by e.g. cout 00213 } 00214 public: 00215 signed long x, y, z, a; // Could easily expand up to six or more dros 00216 // bool dro_output; // To enabe / disable output to terminal 00217 void init () { 00218 x = y = z = a = 0; // These dro registers count pulses delivered to stepper motor driver 00219 // dro_output = true; 00220 } 00221 void update () { 00222 static long t = 300; // Prevent display immediately upon startup 00223 if (millisecs < t) 00224 return; 00225 // if(!idle && dro_output) { 00226 if(!idle) { 00227 char txt[12]; 00228 pc.printf("dros X %s,", readout(txt, x)); // dro.n has running subtotal of all pulses issued to stepper driver.n 00229 pc.printf(" Y %s, Z ", readout(txt, y)); 00230 pc.printf("%s, %s\r\n", readout(txt, z), running ? "R":"idle"); 00231 if(!running) idle = true; // Purpose of idle flag is to stop dro updates JUST AFTER run completes. 00232 t = millisecs + 350; // Schedule next update after this non-blocking delay 00233 } 00234 } 00235 } dro_out ; // single instance of class digital_readout_stuff 00236 00237 /** 00238 class circbuff { public functions 00239 void init () { 00240 int On_Q () { 00241 bool readable () {return !buffempty; } 00242 bool writeable () {return !bufffull; } 00243 bool read (pirbufgrain & g) { 00244 bool write (pirbufgrain & g) { 00245 */ 00246 const int PIRBUFFSIZE = 40; // pirbufgrain are 40 bytes each 00247 class circbuff { 00248 private: 00249 pirbufgrain grain [PIRBUFFSIZE + 1]; 00250 int OnPtr, OffPtr; 00251 bool bufffull, buffempty, buffhalf; 00252 void setempty () { 00253 bufffull = false; 00254 buffhalf = false; 00255 buffempty = true; 00256 } 00257 void grain_copy (pirbufgrain & src, pirbufgrain & dest) { 00258 dest.x = src.x; 00259 dest.y = src.y; 00260 dest.z = src.z; 00261 dest.distance_code = src.distance_code; 00262 dest.f_rate = src.f_rate; // int feed rate mm per min * 1000 00263 } 00264 public: 00265 void init () { 00266 OnPtr = OffPtr = 0; 00267 setempty (); 00268 } 00269 int On_Q () { 00270 int k; 00271 if (bufffull) return PIRBUFFSIZE; 00272 k = OnPtr - OffPtr; 00273 if (k < 0) k += PIRBUFFSIZE; 00274 if (k > PIRBUFFSIZE / 2) 00275 buffhalf = true; 00276 else 00277 buffhalf = false; 00278 return k; 00279 } 00280 bool readable () {return !buffempty; } 00281 bool writeable () {return !bufffull; } 00282 bool read (pirbufgrain & g) { 00283 // if (buffempty) return false; // TO DO Fix problem with buffempty flag 00284 if (On_Q() == 0) return false; 00285 bufffull = false; 00286 grain_copy (grain[OffPtr++], g); 00287 if (OffPtr >= PIRBUFFSIZE) 00288 OffPtr = 0; 00289 if (OnPtr == OffPtr) 00290 buffempty = true; 00291 return true; 00292 } 00293 bool write (pirbufgrain & g) { 00294 if (bufffull) return false; 00295 buffempty = false; 00296 grain_copy (g, grain[OnPtr++]); 00297 if (OnPtr >= PIRBUFFSIZE) 00298 OnPtr = 0; 00299 if (OnPtr == OffPtr) 00300 bufffull = true; 00301 return true; 00302 } 00303 } CircBuff; // single instance of class circbuff 00304 00305 00306 /** 00307 void move_to_XYZ (struct pirbufgrain & ins) { 00308 Takes floating point x, y, z and feed_rate as input. 00309 Finds distances from 'last_position' global, 00310 copies structure containing floating point values for x, y, z, distance_multiplier and feed_rate 00311 onto a circular buffer. 00312 If buffer full, executes 'osThreadYield()' until space is created on buffer 00313 */ 00314 void move_to_XYZ (struct pirbufgrain & ins) { 00315 static const fl_typ duration_multiplier = 60000000.0 / interrupt_period_us; 00316 struct pirbufgrain outs; 00317 fl_typ distx = ins.x - last_position.x.flt, 00318 disty = ins.y - last_position.y.flt, 00319 distz = ins.z - last_position.z.flt, 00320 distT = sqrt ((distx * distx) + (disty * disty) + (distz * distz)), // 3D Pythag ! 00321 temp = n_for_onemmpermin / distT; 00322 if (distT < 0.01) { 00323 pc.printf("Very small move %.4f, Ignoring!\r\n", distT); 00324 return; // Return without updating last_position as it was not changed 00325 } 00326 last_position.x.flt = ins.x; // Update global last_position record 00327 last_position.y.flt = ins.y; 00328 last_position.z.flt = ins.z; 00329 outs.f_rate = ins.f_rate; 00330 outs.distance_code = duration_multiplier * distT; // Duration ticks subject to feed rate compo 00331 outs.x = temp * distx; 00332 outs.y = temp * disty; 00333 outs.z = temp * distz; // Have assembled data ready to put onto queue of move instructions 00334 while (!CircBuff.writeable()) 00335 osThreadYield(); 00336 CircBuff.write (outs); // Move details put on circular buffer 00337 } 00338 00339 /** 00340 * Interrupt Service Routines 00341 void millisec_update_ISR () { self explanatory 00342 00343 void Numerically_Controlled_Oscillators_ISR () { 00344 services Ticker 'NCO_gen' generated interrupts ***ISR*** 00345 Does all of the stepper motor driving. 00346 At end of movement, fetches and starts next move to run from circular buffer 00347 If nothing buffered, stops x, y and z, leaves spindle unaltered 00348 */ 00349 void millisec_update_ISR () { 00350 millisecs++; 00351 } 00352 00353 #define STEP_IDLE_HI // Choose IDLE_HI or LO to suit any power save function of stepper motor drive units 00354 //#define STEP_IDLE_LO 00355 void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR*** 00356 static const long step_mask = ASt | XSt | YSt | ZSt, // Added 6th Feb 14 Mask Does NOT include spindle bits 00357 dir_mask = ADi | XDi | YDi | ZDi; // Added 6th Feb 14 Mask Does NOT include spindle bits 00358 static signed long // 27 Feb 14 changed from unsigned 00359 #if defined Fourth_Axis 00360 acc_a = 0L, pir_a = 0L, 00361 #endif 00362 acc_x = 0L, // acc Accumuloators 00363 pir_x = 0L, // pir Phase Increment Registers 00364 acc_y = 0L, pir_y = 0L, 00365 acc_z = 0L, pir_z = 0L, 00366 acc_spin = 0L, // separate acc for spindle rotation NCO 00367 inc_x = 1L, // inc_x, y, z for updating DRO registers 00368 inc_y = 1L, inc_z = 1L, 00369 dir_bits = 0L, // direction flags for up to four axes 00370 oldSteps = 0L; // 00371 long tmp, newSteps = 0L; 00372 00373 intled = 1; // LED on for duration of interrupt service - point for scope probing 00374 ticks++; // count of interrupts serviced, vital to time end of movement 00375 charge_pumpD25pin1 = ticks & 0x01; // Can use 0x01 or 0x02 here to alter charge pump freq 00376 tmp = Steppers ^ direction_swappers; 00377 #if defined STEP_IDLE_LO 00378 tmp &= ~step_mask; // Step bits prepared for idle lo 00379 #endif 00380 #if defined STEP_IDLE_HI 00381 tmp |= step_mask; // Step bits prepared for idle hi 00382 #endif 00383 acc_spin += pir_spin; // Spindle NCO 00384 if (acc_spin < 0) tmp |= SSt; 00385 else tmp &= ~SSt; 00386 if (!running) Steppers = tmp ^ direction_swappers; // Axes not moving, spindle may be turning or not 00387 else { // running == true, Further manipulation of tmp follows, prior to rewriting to 'Steppers' IO Port 00388 // newSteps = 0L; // Added 6th Feb 14 00389 #if defined Fourth_Axis 00390 acc_a += pir_a; 00391 if (acc_a < 0) newSteps |= ASt;// Added 6th Feb 14 00392 #endif 00393 acc_x += pir_x; // Update phase of signals in accumulators 00394 if (acc_x < 0) newSteps |= XSt;// Added 6th Feb 14 00395 acc_y += pir_y; 00396 if (acc_y < 0) newSteps |= YSt;// Added 6th Feb 14 00397 acc_z += pir_z; 00398 if (acc_z < 0) newSteps |= ZSt;// Added 6th Feb 14 00399 // newSteps has copy of all 4 'acc' MSBs shifted into port bit positions 00400 oldSteps ^= newSteps; // Any bit of stbits set to initiate a Step pulse 00401 tmp ^= oldSteps; 00402 Steppers = tmp ^ direction_swappers; // Output signals to stepper motor drivers, next update dros from 'clocked' bits CLOCK IDLES HIGH 00403 if(oldSteps & XSt) dro_out.x += inc_x; // got clk edge for axis X 00404 if(oldSteps & YSt) dro_out.y += inc_y; // got clk edge for axis Y 00405 if(oldSteps & ZSt) dro_out.z += inc_z; // got clk edge for axis Z 00406 oldSteps = newSteps; // Added 6th Feb 14 00407 if (tickrun <= ticks & !new_run_pending) { // End of a machine movement detected, start next move here if possible 00408 running = false; 00409 move_ended = true; 00410 pir_x = 0L; // stop all stepper motors 00411 pir_y = 0L; 00412 pir_z = 0L; 00413 #if defined Fourth_Axis 00414 pir_a = 0L; 00415 #endif 00416 // ticks = 0L; // Simply to avoid having to think about overflow problems 00417 } // end of if (tickrun <= ticks) { 00418 } // end of else is (running) { 00419 if (!running & new_run_pending) { // Start axis movement 00420 dir_bits= dir_bits_next; 00421 #if defined Fourth_Axis 00422 pir_a = pir_a_next; 00423 #endif 00424 pir_x = pir_x_next; 00425 pir_y = pir_y_next; 00426 pir_z = pir_z_next; 00427 inc_x = inc_x_next; 00428 inc_y = inc_y_next; 00429 inc_z = inc_z_next; 00430 tmp = Steppers ^ direction_swappers; // read output lines 00431 tmp &= ~dir_mask; 00432 tmp |= dir_bits; 00433 Steppers = tmp ^ direction_swappers; 00434 tickrun = ticks + ticks_next; 00435 running = true; // Start the new run 00436 new_run_pending = false; // Clear the flag which initiated this update 00437 idle = false; 00438 } // end of else { // Not running. Grab next data here when or if available 00439 intled = 0; // LED off 00440 } // end of interrupt handler 00441 00442 /* 00443 * End of Interrupt Service Routine 00444 */ 00445 bool spindle_running () { 00446 if (pir_spin == 0) return false; 00447 return true; 00448 } 00449 void spindle_control (signed long ss) { 00450 long t, p; 00451 pir_spin = ss * spindle_factor; 00452 t = ticks; 00453 while (t == ticks) {} // wait until just after next interrupt 00454 p = Steppers ^ direction_swappers; 00455 if (pir_spin & 0x80000000) p |= SDi; 00456 else p &= ~SDi; 00457 Steppers = p ^ direction_swappers; 00458 pc.printf("Done setting spindle speed %d, pir_spin %d, delay = %d\r\n", ss, pir_spin, ticks - t); 00459 } 00460 00461 /** 00462 void pir_updater_task (void const * name) { 00463 A task handed to the RTOS Round Robin 00464 ISR controls certain flags. 00465 This task responds after 'move_ended' flag asserted by NCO ISR 00466 If CircBuff not empty, set of floating point data for next point is fetched, 00467 remaining calcs performed and translated into signed longs, results placed in 00468 set of '_next' locations to be picked up by future NCO ISR 00469 */ 00470 void pir_updater_task (void const * name) { 00471 static long x, y, z;//, count = 0; 00472 struct pirbufgrain outs; 00473 while (true) { 00474 // while (!move_ended || !CircBuff.readable()) { ** TO DO ** Solve problem with empty flag 00475 while (!move_ended || CircBuff.On_Q() == 0) { 00476 osThreadYield(); 00477 } // Proceed beyond here only when move_ended AND CircBuff.readable() both TRUE 00478 CircBuff.read(outs); 00479 x = (long)(outs.f_rate * outs.x); // These take much CPU time !! 00480 y = (long)(outs.f_rate * outs.y); 00481 z = (long)(outs.f_rate * outs.z); 00482 ticks_next = (unsigned long)(outs.distance_code / outs.f_rate); 00483 pir_x_next = x; 00484 pir_y_next = y; 00485 pir_z_next = z; 00486 dir_bits_next = 0; 00487 //if (pir_a & 0x80000000) dir_bits_next |= ADi;// Added 6th Feb 14 read sign bits 00488 inc_x_next = inc_y_next = inc_z_next = 1L; 00489 if (x < 0) { 00490 dir_bits_next |= XDi; 00491 inc_x_next = -1L; 00492 } 00493 if (y < 0) { 00494 dir_bits_next |= YDi; 00495 inc_y_next = -1L; 00496 } 00497 if (z < 0) { 00498 dir_bits_next |= ZDi; 00499 inc_z_next = -1L; 00500 } 00501 move_ended = idle = false; 00502 new_run_pending = true; // cleared and 'running' flag set in interrupt handler 00503 } // end of while (true) { 00504 } 00505 00506 00507 void flags_report_cmd (struct singleGparam * a) { // Diagnostic helper function 00508 pc.printf("Flags Report\r\nRunning %s\r\n", running ? "T":"F"); 00509 pc.printf("idle %s\r\n", idle ? "T":"F"); 00510 pc.printf("new_run_pending %s\r\n", new_run_pending ? "T":"F"); 00511 pc.printf("move_ended %s\r\n", move_ended ? "T":"F"); 00512 pc.printf("CircBuff.readable() %s\r\n", CircBuff.readable() ? "T":"F"); 00513 pc.printf("On CircBuff %d\r\n", CircBuff.On_Q()); 00514 } 00515 00516 /* 00517 #define ESTOP 0x100 00518 #define XLIM 1 00519 #define YLIM 2 00520 #define ZLIM 4 00521 #define UNKN 8 00522 */ 00523 class inputsreaderstuff { 00524 private: 00525 long ins_now;//, ins_old, ins_changed; 00526 public: 00527 void init () { ins_now = 0L;}//ins_old = ins_changed = 0L; } 00528 long read () { 00529 ins_now = 0; 00530 if (D25pin10_EStop) ins_now |= ESTOP; 00531 if (D25pin11_XLim) ins_now |= XLIM; 00532 if (D25pin12_YLim) ins_now |= YLIM; 00533 if (D25pin13_ZLim) ins_now |= ZLIM; 00534 if (D25pin15_unkn) ins_now |= UNKN; 00535 // ins_changed = ins_now ^ ins_old; 00536 // ins_old = ins_now; 00537 return ins_now; 00538 } 00539 // long changed () { return ins_changed; } 00540 } Inputs_From_Machine; 00541 00542 void report_inputs () { 00543 long i = Inputs_From_Machine.read(); 00544 pc.printf("Inputs: EStop %d, XLim %d, YLim %d, ", i & ESTOP ? 1:0, i & XLIM ? 1:0, i & YLIM ? 1:0); 00545 pc.printf("ZLim %d, unkn %d\r\n", i & ZLIM ? 1:0, i & UNKN ? 1:0); 00546 } 00547 00548 //void Emergency_Stop_Interrupt () { 00549 // pc.printf("Emergency Stop Activated !!\r\n"); 00550 // spindle_control (0); // Stop spindle rotation 00551 //} 00552 00553 /** 00554 int main() { 00555 00556 */ 00557 int main() { 00558 long ins, ins_old, ins_changed = 0; 00559 pc.baud(BAUD); // comms to 'PuTTY' serial terminal via mbed usb 00560 // D25pin11_XLim.mode (PullDown); External resistors now fitted 00561 // D25pin12_YLim.mode (PullDown); 00562 // D25pin13_ZLim.mode (PullDown); 00563 // D25pin15_unkn.mode (PullDown); 00564 // D25pin10_EStop.mode (PullDown); 00565 // D25pin10_EStop.rise (& Emergency_Stop_Interrupt); 00566 // D25pin10_EStop.fall (& Emergency_Stop_Interrupt); 00567 00568 Gparams_clr (last_position); 00569 dro_out.init (); 00570 CircBuff.init (); 00571 Inputs_From_Machine.init (); 00572 #if defined SPI_Enable 00573 spidevice.format(8, 0); // 8 bits mode 0, // p11 mosi, p12 miso, p13 sclk ** ONLY 8 BIT ** 00574 spidevice.frequency(12000000); // 12MHz bit rate 00575 #endif 00576 pc.printf("\r\n*\n*\nFound Computer %s\r\n", Target); 00577 pc.printf("Welcome to the CNC tester\r\nStep pulses required to move 1.0mm = %9.0f\r\n", pulses_per_mm); 00578 pc.printf("PIR 'n' for 1mm per min = %9.0f\r\ntop speed = %d mm per min\r\n\n", n_for_onemmpermin, feed_rate_max); 00579 00580 00581 // NVIC_SetPriority(TIMER3_IRQn, 255); // set mbed tickers to lower priority than other things ONLY COMPILES FOR LPC1768 00582 00583 00584 NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, interrupt_period_us);// Have setup timed interrupts, let other code deal 00585 msec.attach_us(&millisec_update_ISR, 1001); 00586 00587 Thread tsr2 (pir_updater_task, (void *)"read from CircBuff and move"); 00588 Thread comlin (command_line_interpreter, (void *)"cli"); // Read any instructions arriving via serial port and act upon them 00589 #if defined I2C_Enable 00590 Thread i2cstuff (i2c_handler, (void *)"i2c thing"); 00591 #endif 00592 ins = ins_old = Inputs_From_Machine.read (); 00593 move_ended = true; // Needed to kickstart system 00594 00595 while(1) { // Round Robin loop 00596 dro_out.update (); // Update DRO readings if, and as often as needed 00597 ins = Inputs_From_Machine.read (); 00598 ins_changed = ins ^ ins_old; 00599 ins_old = ins; 00600 if (ins_changed) 00601 pc.printf("Inputs Have Changed 0x%x, read 0x%x\r\n", ins_changed, ins); 00602 osThreadYield(); // 00603 } // end of Round Robin loop 00604 } // end of int main() 00605
Generated on Sat Jul 16 2022 01:39:30 by
1.7.2