Test code for proving multi-NCO implementation on Altera FPGA using DEO Nano development board
Dependencies: MODSERIAL mbed-rtos mbed
main.cpp
- Committer:
- JonFreeman
- Date:
- 2014-04-24
- Revision:
- 0:7f5b51873953
File content as of revision 0:7f5b51873953:
#include "mbed.h" #include "rtos.h" #include "MODSERIAL.h" #include "cnc.h" extern void i2c_handler (void const *); extern void command_line_interpreter (void const *) ; extern fl_typ feed_rate; // float type is 'float' extern signed long spindle_rpm; const int BAUD = 38400; MODSERIAL pc(USBTX, USBRX); // tx, rx to pc via usb lead Ticker msec; // Ticker updating global millisecs counter bool running = false, new_run_pending = false, idle = false, move_ended = false; unsigned long millisecs = 0L; // 32 bit #if defined (TARGET_KL25Z) const char Target[] = "KL25Z"; // Note need PTE0 (sda) and PTE1 (scl) DigitalOut intled (PTD7); //(PTE1); //J2p19, was 20 DigitalOut charge_pumpD25pin1 (PTD6); //(PTE0); //J2p17, was 18 DigitalIn D25pin10_EStop (PTE20); //j10p1 KL25 J10 is KL46 j4 DigitalIn D25pin11_XLim (PTE21); //j10p3 DigitalIn D25pin12_YLim (PTE22); //j10p5 DigitalIn D25pin13_ZLim (PTE23); //j10p7 DigitalIn D25pin15_unkn (PTE30); //j10p11 #if defined I2C_Enable I2CSlave slave(PTE0, PTE1); // PTE0 sda, (yellow) PTE1 scl (blue) #endif #if defined SPI_Enable SPI spi(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk (uses p11, p12, p13 on mbed LPC1768) #endif // J2p08,J2p10,J2p12, J2p06 #define STEPPER_PORT PortC const int PortBitXSt = 3, // Port bit num X Step J1P05 D25pin 2 PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3 PortBitYSt = 5, // Port bit num Y Step J1P09 D25pin 4 PortBitYDi = 6, // Port bit num Y Dir J1P11 D25pin 5 PortBitZSt = 10, // Port bit num Z Step J1P13 D25pin 6 PortBitZDi = 11, // Port bit num Z Dir J1P15 D25pin 7 PortBitASt = 12, // Port bit num A Step J2P01 D25pin 8 PortBitADi = 13, // Port bit num A Dir J2P03 D25pin 9 PortBitSSt = 8, // Port bit num Spin Step J1P14 D25pin 14 PortBitSDi = 9; // Port bit num Spin Dir J1P16 D25pin 16 #endif #if defined (TARGET_KL46Z) const char Target[] = "KL46Z"; DigitalOut intled (PTE1); //J2p20 checked DigitalOut charge_pumpD25pin1 (PTE0); //J2p18 checked // InterruptIn D25pin10_EStop (PTE20); // j4p1 KL46 J4 is KL25 J10 DigitalIn D25pin10_EStop (PTE20); // j4p1 KL46 J4 is KL25 J10 checked DigitalIn D25pin11_XLim (PTE21); // j4p3 checked DigitalIn D25pin12_YLim (PTE22); // j4p5 checked DigitalIn D25pin13_ZLim (PTE23); // j4p7 checked DigitalIn D25pin15_unkn (PTE30); // j4p11 checked #if defined I2C_Enable I2CSlave slave(p9, p10); #endif #if defined SPI_Enable SPI spi(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC) #endif // J2p13, J2p15, J2p11, J2p09 // Easy way to allocate port bits for // output of stepper motor Step and DIR sigs #define STEPPER_PORT PortC const int PortBitXSt = 0, // Port bit num X Step J1P05 D25pin 2 checked PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3 checked PortBitYSt = 6, // Port bit num Y Step J1P09 D25pin 4 checked PortBitYDi = 7, // Port bit num Y Dir J1P11 D25pin 5 checked PortBitZSt = 10, // Port bit num Z Step J1P13 D25pin 6 checked PortBitZDi = 11, // Port bit num Z Dir J1P15 D25pin 7 checked PortBitASt = 13, // Port bit num A Step J2P01 D25pin 8 checked PortBitADi = 16, // Port bit num A Dir J2P03 D25pin 9 checked PortBitSSt = 8, // Port bit num Spin Step J1P14 D25pin 14 checked PortBitSDi = 9; // Port bit num Spin Dir J1P16 D25pin 16 checked #endif #if defined (TARGET_MBED_LPC1768) const char Target[] = "MBED LPC1768"; DigitalOut intled(LED2); // Correct DigitalOut charge_pumpD25pin1 (p25); // // InterruptIn D25pin10_EStop (p26); //P2.0 DigitalIn D25pin10_EStop (p26); //P2.0 DigitalIn D25pin11_XLim (p24); //P2.2 DigitalIn D25pin12_YLim (p23); //P2.3 DigitalIn D25pin13_ZLim (p19); //P1.30 DigitalIn D25pin15_unkn (p20); //P1.31 #if defined I2C_Enable I2CSlave slave(p9, p10); #endif //#if defined SPI_Enable SPI spi(p5, p6, p7); //#endif // Easy way to allocate port bits // 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 D25pin 2 P0.05 p29 CAN td - USE X Dir D25pin 3 P0.10 p28 SDA - USE Y Step D25pin 4 P0.11 p27 SCL - USE Y Dir D25pin 5 P0.15 p13 Tx - USE Z Step D25pin 6 P0.16 p14 Rx - USE Z Dir D25pin 7 P0.17 p12 miso - USE A Step D25pin 8 P0.18 p11 mosi - Use A Dir D25pin 9 P0.23 p15 A In - Use S Step D25pin 14 P0.24 p16 A In - Use S Dir D25pin 16 P0.25 p17 Reserve 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 PortBitSSt = 23, // Port bit num Spin Step PortBitSDi = 24; // Port bit num Spin Dir #endif const long // Assemble mask bits from now known port bit positions XSt = 1 << PortBitXSt, // X axis Step signal XDi = 1 << PortBitXDi, // X axis Direction signal YSt = 1 << PortBitYSt, // Y axis Step, etc YDi = 1 << PortBitYDi, ZSt = 1 << PortBitZSt, // Z axis ZDi = 1 << PortBitZDi, ASt = 1 << PortBitASt, // A axis, not implemented in full, for e.g. rotary axis ADi = 1 << PortBitADi, SDi = 1 << PortBitSDi, // Spindle, also driven by Step and Dir signals up to 5kHz SSt = 1 << PortBitSSt, // for 5000 RPM SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt); // direction_swappers = XDi | YDi | ZDi | SDi; // include bit to swap direction PortOut Steppers (STEPPER_PORT, SM_MASK); int freq_to_n (int freq) { const double factor = (1 << 25) / 390625.0; // unsigned long long ll = ((1 << 31) * freq) / 50000000; double ll = factor * (double)freq; return (long) ll; } // freq = (50E6 * 'n' / 2**BUS_WIDTH) // 'n' = freq * 2**BUS_WIDTH / 50000000 // 'n' = freq * 2**31 / 25000000 // 'n' = freq * 2**30 / 12500000 // 'n' = freq * 2**29 / 6250000 // 'n' = freq * 2**25 / 390625 int pirs[8], dros[8]; void FPGA_bit (int whichbit, int hiorlo) { int port = Steppers; if (hiorlo) port |= whichbit; else port &= ~whichbit; Steppers = port; } void FPGA_setup () { int port = Steppers; port |= sclr | clken; Steppers = port; port &= ~sclr; port &= ~clken; port &= ~ld_osr; port &= ~ld_pir; Steppers = port; for (int i = 0; i < 8; i++) pirs[i] = dros[i] = 0; } /* -- About Use of 16 bit Command Word -- bit 0 - '1' causes zero reset of phase_inc_reg -- bit 1 - '1' causes zero reset of dro_udcounter -- bit 2 - '1' causes load of phase_inc_reg from input shift reg -- bit 3 - '1' causes load of dro_udcounter from input shift reg -- bit 4 - '1' causes dro_udcounter --> shift reg ready to read dro value -- bit 5 - '1' causes phase_inc_reg --> shift reg ready to read pir value -- bit 6 - '1' causes reset everything to 0 -- -- bit 15 - '1' causes reset of command_word to all 0 after one clock */ #define zero_pir 0x8001 #define zero_dro 0x8002 #define load_pir_from_sr 0x8004 #define load_dro_from_sr 0x8008 #define load_dro_into_sr 0x8010 #define load_pir_into_sr 0x8020 void FPGA_cmd (int command_word) { int port = Steppers; int spirx[8], command_copy = command_word; // pc.printf("At FPGA_cmd, sending %d\r\n", command_word); port |= sclr; // cmdhi_datalo set to 1 Steppers = port; // spirx[0] = spi.write(command_copy >> 8); spirx[1] = spi.write(command_copy); port &= ~sclr; // cmdhi_datalo set to 0 Steppers = port; // // pc.printf("Read spi %x %x\r\n", spirx[0], spirx[1]); } void setcmd_cmd (struct singleGparam * a) { FPGA_cmd (a[1].i); } //#define load_pir_from_sr 0x8004 //#define load_dro_into_sr 0x8010 // DigitalIn D25pin15_unkn (p20); //P1.31 use this to read osr int FPGA_rdandwr (int tosend) { // send 32 bits to in_sr, read 32 bits from out_sr int torecv = 0, port = Steppers, tmp; // tosend = freq_to_n(tosend); for (int j = 3; j >= 0; j--) { torecv <<= 8; tmp = tosend >> (j << 3); torecv |= spi.write(tmp); } return torecv; } const int numof_ncos = 4; void setdro_cmd (struct singleGparam * a) { int recd[numof_ncos + 1]; pc.printf("At setdro with values "); FPGA_cmd(load_dro_into_sr); for (int k = 0; k < numof_ncos; k++) { recd[k] = FPGA_rdandwr (a[k + 1].i); pc.printf("%d, ", a[k + 1].i); } FPGA_cmd(load_dro_from_sr); pc.printf("end\r\n"); } void setpir_cmd (struct singleGparam * a) { int recd[numof_ncos + 1]; pc.printf("At setpir with values "); FPGA_cmd(load_dro_into_sr); for (int k = 0; k < numof_ncos; k++) { recd[k] = FPGA_rdandwr (a[k + 1].i); pc.printf("%d, ", a[k + 1].i); } FPGA_cmd(load_pir_from_sr); pc.printf("end\r\n"); } void getdro_cmd (struct singleGparam * a) { int read_dro[numof_ncos + 1]; pc.printf("At rddro, retrieved values "); FPGA_cmd(load_dro_into_sr); for (int k = 0; k < numof_ncos; k++) { read_dro[k] = FPGA_rdandwr (0); pc.printf("%d, ", read_dro[k]); } pc.printf(" end\r\n"); } void getpir_cmd (struct singleGparam * a) { int read_pir[numof_ncos + 1]; pc.printf("At rdpir, retrieved values "); FPGA_cmd(load_pir_into_sr); for (int k = 0; k < numof_ncos; k++) { read_pir[k] = FPGA_rdandwr (0); pc.printf("%d, ", read_pir[k]); } pc.printf(" end\r\n"); } void clrpir_cmd (struct singleGparam * a) { FPGA_cmd(zero_pir); getpir_cmd(a); } void clrdro_cmd (struct singleGparam * a) { FPGA_cmd(zero_dro); getdro_cmd(a); } char const * target_str_addr () { return Target; } void grain_clr (struct singleGparam & g) { g.flt = 0.0; g.ul = 0L; g.i = g.c = 0; g.changed = false; } void Gparams_clr (struct Gparams & p) { grain_clr (p.x); grain_clr (p.y); grain_clr (p.z); grain_clr (p.i); grain_clr (p.j); grain_clr (p.r); grain_clr (p.a); grain_clr (p.b); grain_clr (p.c); grain_clr (p.d); } class digital_readout_stuff { // class does not need to be named here private: 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: signed long x, y, z, a; // Could easily expand up to six or more dros // bool dro_output; // To enabe / disable output to terminal void init () { x = y = z = a = 0; // These dro registers count pulses delivered to stepper motor driver // dro_output = true; } void update () { static long t = 300; // Prevent display immediately upon startup if (millisecs < t) return; // if(!idle && dro_output) { if(!idle) { char txt[12]; pc.printf("dros X %s,", readout(txt, x)); // dro.n has running subtotal of all pulses issued to stepper driver.n pc.printf(" Y %s, Z ", readout(txt, y)); pc.printf("%s, %s\r\n", readout(txt, z), running ? "R":"idle"); if(!running) idle = true; // Purpose of idle flag is to stop dro updates JUST AFTER run completes. t = millisecs + 350; // Schedule next update after this non-blocking delay } } } dro_out ; // single instance of class digital_readout_stuff void millisec_update_ISR () { millisecs++; } /*#define STEP_IDLE_HI // Choose IDLE_HI or LO to suit any power save function of stepper motor drive units //#define STEP_IDLE_LO void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR*** static const long step_mask = ASt | XSt | YSt | ZSt, // Added 6th Feb 14 Mask Does NOT include spindle bits dir_mask = ADi | XDi | YDi | ZDi; // Added 6th Feb 14 Mask Does NOT include spindle bits static signed long // 27 Feb 14 changed from unsigned #if defined Fourth_Axis acc_a = 0L, pir_a = 0L, #endif acc_x = 0L, // acc Accumuloators pir_x = 0L, // pir Phase Increment Registers acc_y = 0L, pir_y = 0L, acc_z = 0L, pir_z = 0L, acc_spin = 0L, // separate acc for spindle rotation NCO inc_x = 1L, // inc_x, y, z for updating DRO registers inc_y = 1L, inc_z = 1L, dir_bits = 0L, // direction flags for up to four axes oldSteps = 0L; // long tmp, newSteps = 0L; intled = 1; // LED on for duration of interrupt service - point for scope probing ticks++; // count of interrupts serviced, vital to time end of movement charge_pumpD25pin1 = ticks & 0x01; // Can use 0x01 or 0x02 here to alter charge pump freq tmp = Steppers ^ direction_swappers; #if defined STEP_IDLE_LO tmp &= ~step_mask; // Step bits prepared for idle lo #endif #if defined STEP_IDLE_HI tmp |= step_mask; // Step bits prepared for idle hi #endif acc_spin += pir_spin; // Spindle NCO if (acc_spin < 0) tmp |= SSt; else tmp &= ~SSt; if (!running) Steppers = tmp ^ direction_swappers; // Axes not moving, spindle may be turning or not else { // running == true, Further manipulation of tmp follows, prior to rewriting to 'Steppers' IO Port // newSteps = 0L; // Added 6th Feb 14 #if defined Fourth_Axis acc_a += pir_a; if (acc_a < 0) newSteps |= ASt;// Added 6th Feb 14 #endif acc_x += pir_x; // Update phase of signals in accumulators if (acc_x < 0) newSteps |= XSt;// Added 6th Feb 14 acc_y += pir_y; if (acc_y < 0) newSteps |= YSt;// Added 6th Feb 14 acc_z += pir_z; if (acc_z < 0) newSteps |= ZSt;// Added 6th Feb 14 // newSteps has copy of all 4 'acc' MSBs shifted into port bit positions oldSteps ^= newSteps; // Any bit of stbits set to initiate a Step pulse tmp ^= oldSteps; Steppers = tmp ^ direction_swappers; // Output signals to stepper motor drivers, next update dros from 'clocked' bits CLOCK IDLES HIGH if(oldSteps & XSt) dro_out.x += inc_x; // got clk edge for axis X if(oldSteps & YSt) dro_out.y += inc_y; // got clk edge for axis Y if(oldSteps & ZSt) dro_out.z += inc_z; // got clk edge for axis Z oldSteps = newSteps; // Added 6th Feb 14 if (tickrun <= ticks & !new_run_pending) { // 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; #if defined Fourth_Axis pir_a = 0L; #endif // ticks = 0L; // Simply to avoid having to think about overflow problems } // end of if (tickrun <= ticks) { } // end of else is (running) { if (!running & new_run_pending) { // Start axis movement dir_bits= dir_bits_next; #if defined Fourth_Axis pir_a = pir_a_next; #endif pir_x = pir_x_next; pir_y = pir_y_next; pir_z = pir_z_next; inc_x = inc_x_next; inc_y = inc_y_next; inc_z = inc_z_next; tmp = Steppers ^ direction_swappers; // read output lines tmp &= ~dir_mask; tmp |= dir_bits; Steppers = tmp ^ direction_swappers; tickrun = ticks + ticks_next; running = true; // Start the new run new_run_pending = false; // Clear the flag which initiated this update idle = false; } // end of else { // Not running. Grab next data here when or if available intled = 0; // LED off } // end of interrupt handler */ /* * End of Interrupt Service Routine */ /*bool spindle_running () { */ class inputsreaderstuff { private: long ins_now;//, ins_old, ins_changed; public: void init () { ins_now = 0L;}//ins_old = ins_changed = 0L; } long read () { ins_now = 0; if (D25pin10_EStop) ins_now |= ESTOP; if (D25pin11_XLim) ins_now |= XLIM; if (D25pin12_YLim) ins_now |= YLIM; if (D25pin13_ZLim) ins_now |= ZLIM; if (D25pin15_unkn) ins_now |= UNKN; // ins_changed = ins_now ^ ins_old; // ins_old = ins_now; return ins_now; } // long changed () { return ins_changed; } } Inputs_From_Machine; void report_inputs () { long i = Inputs_From_Machine.read(); pc.printf("Inputs: EStop %d, XLim %d, YLim %d, ", i & ESTOP ? 1:0, i & XLIM ? 1:0, i & YLIM ? 1:0); pc.printf("ZLim %d, unkn %d\r\n", i & ZLIM ? 1:0, i & UNKN ? 1:0); } int main() { long ins, ins_old, ins_changed = 0; pc.baud(BAUD); // comms to 'PuTTY' serial terminal via mbed usb spi.format (8,0); // use 8 bit format for compatibility with Freescale KLxxZ spi.frequency(12000000); // 12MHz, fast enough dro_out.init (); FPGA_setup(); pc.printf("\r\n*\n*\nFound Computer %s\r\n", Target); msec.attach_us(&millisec_update_ISR, 1001); Thread comlin (command_line_interpreter, (void *)"cli"); // Read any instructions arriving via serial port and act upon them //#if defined I2C_Enable // Thread i2cstuff (i2c_handler, (void *)"i2c thing"); //#endif ins = ins_old = Inputs_From_Machine.read (); move_ended = true; // Needed to kickstart system while(1) { // Round Robin loop dro_out.update (); // Update DRO readings if, and as often as needed ins = Inputs_From_Machine.read (); ins_changed = ins ^ ins_old; ins_old = ins; if (ins_changed) pc.printf("Inputs Have Changed 0x%x, read 0x%x\r\n", ins_changed, ins); osThreadYield(); // } // end of Round Robin loop } // end of int main()