Jon Freeman
Test code for proving multi-NCO implementation on Altera FPGA using DEO Nano development board
Dependencies: MODSERIAL mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:7f5b51873953
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 24 15:35:38 2014 +0000
@@ -0,0 +1,510 @@
+#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()
+