Jon Freeman / Mbed 2 deprecated Altera_FPGA_test

Test code for proving multi-NCO implementation on Altera FPGA using DEO Nano development board

Dependencies:   MODSERIAL mbed-rtos mbed

main.cpp@0:7f5b51873953, 2014-04-24 (annotated)

Committer:
JonFreeman
Date:
Thu Apr 24 15:35:38 2014 +0000
Revision:
0:7f5b51873953
For testing numerically controlled oscillators implemented in Altera FPGA fitted to DEO Nan0 development board

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:7f5b51873953 1 #include "mbed.h"
JonFreeman 0:7f5b51873953 2 #include "rtos.h"
JonFreeman 0:7f5b51873953 3 #include "MODSERIAL.h"
JonFreeman 0:7f5b51873953 4 #include "cnc.h"
JonFreeman 0:7f5b51873953 5 extern void i2c_handler (void const *);
JonFreeman 0:7f5b51873953 6 extern void command_line_interpreter (void const *) ;
JonFreeman 0:7f5b51873953 7 extern fl_typ feed_rate; // float type is 'float'
JonFreeman 0:7f5b51873953 8 extern signed long spindle_rpm;
JonFreeman 0:7f5b51873953 9
JonFreeman 0:7f5b51873953 10 const int BAUD = 38400;
JonFreeman 0:7f5b51873953 11 MODSERIAL pc(USBTX, USBRX); // tx, rx to pc via usb lead
JonFreeman 0:7f5b51873953 12 Ticker msec; // Ticker updating global millisecs counter
JonFreeman 0:7f5b51873953 13
JonFreeman 0:7f5b51873953 14 bool running = false,
JonFreeman 0:7f5b51873953 15 new_run_pending = false,
JonFreeman 0:7f5b51873953 16 idle = false,
JonFreeman 0:7f5b51873953 17 move_ended = false;
JonFreeman 0:7f5b51873953 18
JonFreeman 0:7f5b51873953 19 unsigned long millisecs = 0L; // 32 bit
JonFreeman 0:7f5b51873953 20
JonFreeman 0:7f5b51873953 21 #if defined (TARGET_KL25Z)
JonFreeman 0:7f5b51873953 22 const char Target[] = "KL25Z"; // Note need PTE0 (sda) and PTE1 (scl)
JonFreeman 0:7f5b51873953 23 DigitalOut intled (PTD7); //(PTE1); //J2p19, was 20
JonFreeman 0:7f5b51873953 24 DigitalOut charge_pumpD25pin1 (PTD6); //(PTE0); //J2p17, was 18
JonFreeman 0:7f5b51873953 25 DigitalIn D25pin10_EStop (PTE20); //j10p1 KL25 J10 is KL46 j4
JonFreeman 0:7f5b51873953 26 DigitalIn D25pin11_XLim (PTE21); //j10p3
JonFreeman 0:7f5b51873953 27 DigitalIn D25pin12_YLim (PTE22); //j10p5
JonFreeman 0:7f5b51873953 28 DigitalIn D25pin13_ZLim (PTE23); //j10p7
JonFreeman 0:7f5b51873953 29 DigitalIn D25pin15_unkn (PTE30); //j10p11
JonFreeman 0:7f5b51873953 30 #if defined I2C_Enable
JonFreeman 0:7f5b51873953 31 I2CSlave slave(PTE0, PTE1); // PTE0 sda, (yellow) PTE1 scl (blue)
JonFreeman 0:7f5b51873953 32 #endif
JonFreeman 0:7f5b51873953 33 #if defined SPI_Enable
JonFreeman 0:7f5b51873953 34 SPI spi(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk (uses p11, p12, p13 on mbed LPC1768)
JonFreeman 0:7f5b51873953 35 #endif
JonFreeman 0:7f5b51873953 36 // J2p08,J2p10,J2p12, J2p06
JonFreeman 0:7f5b51873953 37 #define STEPPER_PORT PortC
JonFreeman 0:7f5b51873953 38 const int PortBitXSt = 3, // Port bit num X Step J1P05 D25pin 2
JonFreeman 0:7f5b51873953 39 PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3
JonFreeman 0:7f5b51873953 40 PortBitYSt = 5, // Port bit num Y Step J1P09 D25pin 4
JonFreeman 0:7f5b51873953 41 PortBitYDi = 6, // Port bit num Y Dir J1P11 D25pin 5
JonFreeman 0:7f5b51873953 42 PortBitZSt = 10, // Port bit num Z Step J1P13 D25pin 6
JonFreeman 0:7f5b51873953 43 PortBitZDi = 11, // Port bit num Z Dir J1P15 D25pin 7
JonFreeman 0:7f5b51873953 44 PortBitASt = 12, // Port bit num A Step J2P01 D25pin 8
JonFreeman 0:7f5b51873953 45 PortBitADi = 13, // Port bit num A Dir J2P03 D25pin 9
JonFreeman 0:7f5b51873953 46 PortBitSSt = 8, // Port bit num Spin Step J1P14 D25pin 14
JonFreeman 0:7f5b51873953 47 PortBitSDi = 9; // Port bit num Spin Dir J1P16 D25pin 16
JonFreeman 0:7f5b51873953 48 #endif
JonFreeman 0:7f5b51873953 49
JonFreeman 0:7f5b51873953 50
JonFreeman 0:7f5b51873953 51
JonFreeman 0:7f5b51873953 52 #if defined (TARGET_KL46Z)
JonFreeman 0:7f5b51873953 53 const char Target[] = "KL46Z";
JonFreeman 0:7f5b51873953 54 DigitalOut intled (PTE1); //J2p20 checked
JonFreeman 0:7f5b51873953 55
JonFreeman 0:7f5b51873953 56
JonFreeman 0:7f5b51873953 57 DigitalOut charge_pumpD25pin1 (PTE0); //J2p18 checked
JonFreeman 0:7f5b51873953 58 // InterruptIn D25pin10_EStop (PTE20); // j4p1 KL46 J4 is KL25 J10
JonFreeman 0:7f5b51873953 59 DigitalIn D25pin10_EStop (PTE20); // j4p1 KL46 J4 is KL25 J10 checked
JonFreeman 0:7f5b51873953 60 DigitalIn D25pin11_XLim (PTE21); // j4p3 checked
JonFreeman 0:7f5b51873953 61 DigitalIn D25pin12_YLim (PTE22); // j4p5 checked
JonFreeman 0:7f5b51873953 62 DigitalIn D25pin13_ZLim (PTE23); // j4p7 checked
JonFreeman 0:7f5b51873953 63 DigitalIn D25pin15_unkn (PTE30); // j4p11 checked
JonFreeman 0:7f5b51873953 64 #if defined I2C_Enable
JonFreeman 0:7f5b51873953 65 I2CSlave slave(p9, p10);
JonFreeman 0:7f5b51873953 66 #endif
JonFreeman 0:7f5b51873953 67 #if defined SPI_Enable
JonFreeman 0:7f5b51873953 68 SPI spi(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC)
JonFreeman 0:7f5b51873953 69 #endif
JonFreeman 0:7f5b51873953 70 // J2p13, J2p15, J2p11, J2p09
JonFreeman 0:7f5b51873953 71 // Easy way to allocate port bits for
JonFreeman 0:7f5b51873953 72 // output of stepper motor Step and DIR sigs
JonFreeman 0:7f5b51873953 73 #define STEPPER_PORT PortC
JonFreeman 0:7f5b51873953 74 const int PortBitXSt = 0, // Port bit num X Step J1P05 D25pin 2 checked
JonFreeman 0:7f5b51873953 75 PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3 checked
JonFreeman 0:7f5b51873953 76 PortBitYSt = 6, // Port bit num Y Step J1P09 D25pin 4 checked
JonFreeman 0:7f5b51873953 77 PortBitYDi = 7, // Port bit num Y Dir J1P11 D25pin 5 checked
JonFreeman 0:7f5b51873953 78 PortBitZSt = 10, // Port bit num Z Step J1P13 D25pin 6 checked
JonFreeman 0:7f5b51873953 79 PortBitZDi = 11, // Port bit num Z Dir J1P15 D25pin 7 checked
JonFreeman 0:7f5b51873953 80 PortBitASt = 13, // Port bit num A Step J2P01 D25pin 8 checked
JonFreeman 0:7f5b51873953 81 PortBitADi = 16, // Port bit num A Dir J2P03 D25pin 9 checked
JonFreeman 0:7f5b51873953 82 PortBitSSt = 8, // Port bit num Spin Step J1P14 D25pin 14 checked
JonFreeman 0:7f5b51873953 83 PortBitSDi = 9; // Port bit num Spin Dir J1P16 D25pin 16 checked
JonFreeman 0:7f5b51873953 84 #endif
JonFreeman 0:7f5b51873953 85 #if defined (TARGET_MBED_LPC1768)
JonFreeman 0:7f5b51873953 86 const char Target[] = "MBED LPC1768";
JonFreeman 0:7f5b51873953 87 DigitalOut intled(LED2); // Correct
JonFreeman 0:7f5b51873953 88 DigitalOut charge_pumpD25pin1 (p25); //
JonFreeman 0:7f5b51873953 89 // InterruptIn D25pin10_EStop (p26); //P2.0
JonFreeman 0:7f5b51873953 90 DigitalIn D25pin10_EStop (p26); //P2.0
JonFreeman 0:7f5b51873953 91 DigitalIn D25pin11_XLim (p24); //P2.2
JonFreeman 0:7f5b51873953 92 DigitalIn D25pin12_YLim (p23); //P2.3
JonFreeman 0:7f5b51873953 93 DigitalIn D25pin13_ZLim (p19); //P1.30
JonFreeman 0:7f5b51873953 94 DigitalIn D25pin15_unkn (p20); //P1.31
JonFreeman 0:7f5b51873953 95 #if defined I2C_Enable
JonFreeman 0:7f5b51873953 96 I2CSlave slave(p9, p10);
JonFreeman 0:7f5b51873953 97 #endif
JonFreeman 0:7f5b51873953 98 //#if defined SPI_Enable
JonFreeman 0:7f5b51873953 99 SPI spi(p5, p6, p7);
JonFreeman 0:7f5b51873953 100 //#endif
JonFreeman 0:7f5b51873953 101 // Easy way to allocate port bits
JonFreeman 0:7f5b51873953 102 // output of stepper motor Step and DIR sigs
JonFreeman 0:7f5b51873953 103 #define STEPPER_PORT Port0
JonFreeman 0:7f5b51873953 104 /* Port 0 bits routed to DIP pins as follows:-
JonFreeman 0:7f5b51873953 105 P0.00 p09 Reserve SDA
JonFreeman 0:7f5b51873953 106 P0.01 p10 Reserve SCL
JonFreeman 0:7f5b51873953 107 P0.04 p30 CAN rd - USE X Step D25pin 2
JonFreeman 0:7f5b51873953 108 P0.05 p29 CAN td - USE X Dir D25pin 3
JonFreeman 0:7f5b51873953 109 P0.10 p28 SDA - USE Y Step D25pin 4
JonFreeman 0:7f5b51873953 110 P0.11 p27 SCL - USE Y Dir D25pin 5
JonFreeman 0:7f5b51873953 111 P0.15 p13 Tx - USE Z Step D25pin 6
JonFreeman 0:7f5b51873953 112 P0.16 p14 Rx - USE Z Dir D25pin 7
JonFreeman 0:7f5b51873953 113 P0.17 p12 miso - USE A Step D25pin 8
JonFreeman 0:7f5b51873953 114 P0.18 p11 mosi - Use A Dir D25pin 9
JonFreeman 0:7f5b51873953 115 P0.23 p15 A In - Use S Step D25pin 14
JonFreeman 0:7f5b51873953 116 P0.24 p16 A In - Use S Dir D25pin 16
JonFreeman 0:7f5b51873953 117 P0.25 p17 Reserve A In
JonFreeman 0:7f5b51873953 118 P0.26 p18 Reserve A Out
JonFreeman 0:7f5b51873953 119 */
JonFreeman 0:7f5b51873953 120 const int PortBitXSt = 4, // Port bit num X Step
JonFreeman 0:7f5b51873953 121 PortBitXDi = 5, // Port bit num X Dir
JonFreeman 0:7f5b51873953 122 PortBitYSt = 10, // Port bit num Y Step
JonFreeman 0:7f5b51873953 123 PortBitYDi = 11, // Port bit num Y Dir
JonFreeman 0:7f5b51873953 124 PortBitZSt = 15, // Port bit num Z Step
JonFreeman 0:7f5b51873953 125 PortBitZDi = 16, // Port bit num Z Dir
JonFreeman 0:7f5b51873953 126 PortBitASt = 17, // Port bit num A Step
JonFreeman 0:7f5b51873953 127 PortBitADi = 18, // Port bit num A Dir
JonFreeman 0:7f5b51873953 128 PortBitSSt = 23, // Port bit num Spin Step
JonFreeman 0:7f5b51873953 129 PortBitSDi = 24; // Port bit num Spin Dir
JonFreeman 0:7f5b51873953 130 #endif
JonFreeman 0:7f5b51873953 131
JonFreeman 0:7f5b51873953 132 const long // Assemble mask bits from now known port bit positions
JonFreeman 0:7f5b51873953 133 XSt = 1 << PortBitXSt, // X axis Step signal
JonFreeman 0:7f5b51873953 134 XDi = 1 << PortBitXDi, // X axis Direction signal
JonFreeman 0:7f5b51873953 135 YSt = 1 << PortBitYSt, // Y axis Step, etc
JonFreeman 0:7f5b51873953 136 YDi = 1 << PortBitYDi,
JonFreeman 0:7f5b51873953 137 ZSt = 1 << PortBitZSt, // Z axis
JonFreeman 0:7f5b51873953 138 ZDi = 1 << PortBitZDi,
JonFreeman 0:7f5b51873953 139 ASt = 1 << PortBitASt, // A axis, not implemented in full, for e.g. rotary axis
JonFreeman 0:7f5b51873953 140 ADi = 1 << PortBitADi,
JonFreeman 0:7f5b51873953 141 SDi = 1 << PortBitSDi, // Spindle, also driven by Step and Dir signals up to 5kHz
JonFreeman 0:7f5b51873953 142 SSt = 1 << PortBitSSt, // for 5000 RPM
JonFreeman 0:7f5b51873953 143
JonFreeman 0:7f5b51873953 144 SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt);
JonFreeman 0:7f5b51873953 145 // direction_swappers = XDi | YDi | ZDi | SDi; // include bit to swap direction
JonFreeman 0:7f5b51873953 146
JonFreeman 0:7f5b51873953 147 PortOut Steppers (STEPPER_PORT, SM_MASK);
JonFreeman 0:7f5b51873953 148
JonFreeman 0:7f5b51873953 149 int freq_to_n (int freq) {
JonFreeman 0:7f5b51873953 150 const double factor = (1 << 25) / 390625.0;
JonFreeman 0:7f5b51873953 151 // unsigned long long ll = ((1 << 31) * freq) / 50000000;
JonFreeman 0:7f5b51873953 152 double ll = factor * (double)freq;
JonFreeman 0:7f5b51873953 153 return (long) ll;
JonFreeman 0:7f5b51873953 154 }
JonFreeman 0:7f5b51873953 155 // freq = (50E6 * 'n' / 2**BUS_WIDTH)
JonFreeman 0:7f5b51873953 156 // 'n' = freq * 2**BUS_WIDTH / 50000000
JonFreeman 0:7f5b51873953 157 // 'n' = freq * 2**31 / 25000000
JonFreeman 0:7f5b51873953 158 // 'n' = freq * 2**30 / 12500000
JonFreeman 0:7f5b51873953 159 // 'n' = freq * 2**29 / 6250000
JonFreeman 0:7f5b51873953 160 // 'n' = freq * 2**25 / 390625
JonFreeman 0:7f5b51873953 161
JonFreeman 0:7f5b51873953 162 int pirs[8], dros[8];
JonFreeman 0:7f5b51873953 163
JonFreeman 0:7f5b51873953 164 void FPGA_bit (int whichbit, int hiorlo) {
JonFreeman 0:7f5b51873953 165 int port = Steppers;
JonFreeman 0:7f5b51873953 166 if (hiorlo) port |= whichbit;
JonFreeman 0:7f5b51873953 167 else port &= ~whichbit;
JonFreeman 0:7f5b51873953 168 Steppers = port;
JonFreeman 0:7f5b51873953 169 }
JonFreeman 0:7f5b51873953 170
JonFreeman 0:7f5b51873953 171 void FPGA_setup () {
JonFreeman 0:7f5b51873953 172 int port = Steppers;
JonFreeman 0:7f5b51873953 173 port |= sclr | clken;
JonFreeman 0:7f5b51873953 174 Steppers = port;
JonFreeman 0:7f5b51873953 175 port &= ~sclr;
JonFreeman 0:7f5b51873953 176 port &= ~clken;
JonFreeman 0:7f5b51873953 177 port &= ~ld_osr;
JonFreeman 0:7f5b51873953 178 port &= ~ld_pir;
JonFreeman 0:7f5b51873953 179 Steppers = port;
JonFreeman 0:7f5b51873953 180 for (int i = 0; i < 8; i++)
JonFreeman 0:7f5b51873953 181 pirs[i] = dros[i] = 0;
JonFreeman 0:7f5b51873953 182 }
JonFreeman 0:7f5b51873953 183
JonFreeman 0:7f5b51873953 184 /*
JonFreeman 0:7f5b51873953 185 -- About Use of 16 bit Command Word
JonFreeman 0:7f5b51873953 186 -- bit 0 - '1' causes zero reset of phase_inc_reg
JonFreeman 0:7f5b51873953 187 -- bit 1 - '1' causes zero reset of dro_udcounter
JonFreeman 0:7f5b51873953 188 -- bit 2 - '1' causes load of phase_inc_reg from input shift reg
JonFreeman 0:7f5b51873953 189 -- bit 3 - '1' causes load of dro_udcounter from input shift reg
JonFreeman 0:7f5b51873953 190 -- bit 4 - '1' causes dro_udcounter --> shift reg ready to read dro value
JonFreeman 0:7f5b51873953 191 -- bit 5 - '1' causes phase_inc_reg --> shift reg ready to read pir value
JonFreeman 0:7f5b51873953 192 -- bit 6 - '1' causes reset everything to 0
JonFreeman 0:7f5b51873953 193 --
JonFreeman 0:7f5b51873953 194 -- bit 15 - '1' causes reset of command_word to all 0 after one clock
JonFreeman 0:7f5b51873953 195
JonFreeman 0:7f5b51873953 196 */
JonFreeman 0:7f5b51873953 197 #define zero_pir 0x8001
JonFreeman 0:7f5b51873953 198 #define zero_dro 0x8002
JonFreeman 0:7f5b51873953 199 #define load_pir_from_sr 0x8004
JonFreeman 0:7f5b51873953 200 #define load_dro_from_sr 0x8008
JonFreeman 0:7f5b51873953 201 #define load_dro_into_sr 0x8010
JonFreeman 0:7f5b51873953 202 #define load_pir_into_sr 0x8020
JonFreeman 0:7f5b51873953 203
JonFreeman 0:7f5b51873953 204 void FPGA_cmd (int command_word) {
JonFreeman 0:7f5b51873953 205 int port = Steppers;
JonFreeman 0:7f5b51873953 206 int spirx[8], command_copy = command_word;
JonFreeman 0:7f5b51873953 207 // pc.printf("At FPGA_cmd, sending %d\r\n", command_word);
JonFreeman 0:7f5b51873953 208 port |= sclr; // cmdhi_datalo set to 1
JonFreeman 0:7f5b51873953 209 Steppers = port; //
JonFreeman 0:7f5b51873953 210 spirx[0] = spi.write(command_copy >> 8);
JonFreeman 0:7f5b51873953 211 spirx[1] = spi.write(command_copy);
JonFreeman 0:7f5b51873953 212 port &= ~sclr; // cmdhi_datalo set to 0
JonFreeman 0:7f5b51873953 213 Steppers = port; //
JonFreeman 0:7f5b51873953 214 // pc.printf("Read spi %x %x\r\n", spirx[0], spirx[1]);
JonFreeman 0:7f5b51873953 215 }
JonFreeman 0:7f5b51873953 216
JonFreeman 0:7f5b51873953 217 void setcmd_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 218 FPGA_cmd (a[1].i);
JonFreeman 0:7f5b51873953 219 }
JonFreeman 0:7f5b51873953 220
JonFreeman 0:7f5b51873953 221 //#define load_pir_from_sr 0x8004
JonFreeman 0:7f5b51873953 222 //#define load_dro_into_sr 0x8010
JonFreeman 0:7f5b51873953 223
JonFreeman 0:7f5b51873953 224 // DigitalIn D25pin15_unkn (p20); //P1.31 use this to read osr
JonFreeman 0:7f5b51873953 225 int FPGA_rdandwr (int tosend) { // send 32 bits to in_sr, read 32 bits from out_sr
JonFreeman 0:7f5b51873953 226 int torecv = 0, port = Steppers, tmp;
JonFreeman 0:7f5b51873953 227 // tosend = freq_to_n(tosend);
JonFreeman 0:7f5b51873953 228 for (int j = 3; j >= 0; j--) {
JonFreeman 0:7f5b51873953 229 torecv <<= 8;
JonFreeman 0:7f5b51873953 230 tmp = tosend >> (j << 3);
JonFreeman 0:7f5b51873953 231 torecv |= spi.write(tmp);
JonFreeman 0:7f5b51873953 232 }
JonFreeman 0:7f5b51873953 233 return torecv;
JonFreeman 0:7f5b51873953 234 }
JonFreeman 0:7f5b51873953 235
JonFreeman 0:7f5b51873953 236 const int numof_ncos = 4;
JonFreeman 0:7f5b51873953 237
JonFreeman 0:7f5b51873953 238 void setdro_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 239 int recd[numof_ncos + 1];
JonFreeman 0:7f5b51873953 240 pc.printf("At setdro with values ");
JonFreeman 0:7f5b51873953 241 FPGA_cmd(load_dro_into_sr);
JonFreeman 0:7f5b51873953 242 for (int k = 0; k < numof_ncos; k++) {
JonFreeman 0:7f5b51873953 243 recd[k] = FPGA_rdandwr (a[k + 1].i);
JonFreeman 0:7f5b51873953 244 pc.printf("%d, ", a[k + 1].i);
JonFreeman 0:7f5b51873953 245 }
JonFreeman 0:7f5b51873953 246 FPGA_cmd(load_dro_from_sr);
JonFreeman 0:7f5b51873953 247 pc.printf("end\r\n");
JonFreeman 0:7f5b51873953 248 }
JonFreeman 0:7f5b51873953 249
JonFreeman 0:7f5b51873953 250 void setpir_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 251 int recd[numof_ncos + 1];
JonFreeman 0:7f5b51873953 252 pc.printf("At setpir with values ");
JonFreeman 0:7f5b51873953 253 FPGA_cmd(load_dro_into_sr);
JonFreeman 0:7f5b51873953 254 for (int k = 0; k < numof_ncos; k++) {
JonFreeman 0:7f5b51873953 255 recd[k] = FPGA_rdandwr (a[k + 1].i);
JonFreeman 0:7f5b51873953 256 pc.printf("%d, ", a[k + 1].i);
JonFreeman 0:7f5b51873953 257 }
JonFreeman 0:7f5b51873953 258 FPGA_cmd(load_pir_from_sr);
JonFreeman 0:7f5b51873953 259 pc.printf("end\r\n");
JonFreeman 0:7f5b51873953 260 }
JonFreeman 0:7f5b51873953 261
JonFreeman 0:7f5b51873953 262 void getdro_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 263 int read_dro[numof_ncos + 1];
JonFreeman 0:7f5b51873953 264 pc.printf("At rddro, retrieved values ");
JonFreeman 0:7f5b51873953 265 FPGA_cmd(load_dro_into_sr);
JonFreeman 0:7f5b51873953 266 for (int k = 0; k < numof_ncos; k++) {
JonFreeman 0:7f5b51873953 267 read_dro[k] = FPGA_rdandwr (0);
JonFreeman 0:7f5b51873953 268 pc.printf("%d, ", read_dro[k]);
JonFreeman 0:7f5b51873953 269 }
JonFreeman 0:7f5b51873953 270 pc.printf(" end\r\n");
JonFreeman 0:7f5b51873953 271 }
JonFreeman 0:7f5b51873953 272
JonFreeman 0:7f5b51873953 273 void getpir_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 274 int read_pir[numof_ncos + 1];
JonFreeman 0:7f5b51873953 275 pc.printf("At rdpir, retrieved values ");
JonFreeman 0:7f5b51873953 276 FPGA_cmd(load_pir_into_sr);
JonFreeman 0:7f5b51873953 277 for (int k = 0; k < numof_ncos; k++) {
JonFreeman 0:7f5b51873953 278 read_pir[k] = FPGA_rdandwr (0);
JonFreeman 0:7f5b51873953 279 pc.printf("%d, ", read_pir[k]);
JonFreeman 0:7f5b51873953 280 }
JonFreeman 0:7f5b51873953 281 pc.printf(" end\r\n");
JonFreeman 0:7f5b51873953 282 }
JonFreeman 0:7f5b51873953 283
JonFreeman 0:7f5b51873953 284 void clrpir_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 285 FPGA_cmd(zero_pir);
JonFreeman 0:7f5b51873953 286 getpir_cmd(a);
JonFreeman 0:7f5b51873953 287 }
JonFreeman 0:7f5b51873953 288
JonFreeman 0:7f5b51873953 289 void clrdro_cmd (struct singleGparam * a) {
JonFreeman 0:7f5b51873953 290 FPGA_cmd(zero_dro);
JonFreeman 0:7f5b51873953 291 getdro_cmd(a);
JonFreeman 0:7f5b51873953 292 }
JonFreeman 0:7f5b51873953 293
JonFreeman 0:7f5b51873953 294
JonFreeman 0:7f5b51873953 295 char const * target_str_addr () {
JonFreeman 0:7f5b51873953 296 return Target;
JonFreeman 0:7f5b51873953 297 }
JonFreeman 0:7f5b51873953 298
JonFreeman 0:7f5b51873953 299 void grain_clr (struct singleGparam & g) {
JonFreeman 0:7f5b51873953 300 g.flt = 0.0;
JonFreeman 0:7f5b51873953 301 g.ul = 0L;
JonFreeman 0:7f5b51873953 302 g.i = g.c = 0;
JonFreeman 0:7f5b51873953 303 g.changed = false;
JonFreeman 0:7f5b51873953 304 }
JonFreeman 0:7f5b51873953 305 void Gparams_clr (struct Gparams & p) {
JonFreeman 0:7f5b51873953 306 grain_clr (p.x); grain_clr (p.y); grain_clr (p.z); grain_clr (p.i); grain_clr (p.j);
JonFreeman 0:7f5b51873953 307 grain_clr (p.r); grain_clr (p.a); grain_clr (p.b); grain_clr (p.c); grain_clr (p.d);
JonFreeman 0:7f5b51873953 308 }
JonFreeman 0:7f5b51873953 309
JonFreeman 0:7f5b51873953 310 class digital_readout_stuff { // class does not need to be named here
JonFreeman 0:7f5b51873953 311 private:
JonFreeman 0:7f5b51873953 312 char * readout (char * txt, long p) // p has running subtotal of all pulses issued to stepper driver
JonFreeman 0:7f5b51873953 313 {
JonFreeman 0:7f5b51873953 314 txt[0] = '+'; // constructs string e.g. "+123.456"
JonFreeman 0:7f5b51873953 315 txt[8] = 0; // null terminated
JonFreeman 0:7f5b51873953 316 if (p < 0) {
JonFreeman 0:7f5b51873953 317 txt[0] = '-';
JonFreeman 0:7f5b51873953 318 p = -p;
JonFreeman 0:7f5b51873953 319 }
JonFreeman 0:7f5b51873953 320 p *= 1000;
JonFreeman 0:7f5b51873953 321 //?? p /= pulses_per_mm;
JonFreeman 0:7f5b51873953 322 if (p > 999999) {
JonFreeman 0:7f5b51873953 323 sprintf(txt + 1, "OVRANGE");
JonFreeman 0:7f5b51873953 324 return txt;
JonFreeman 0:7f5b51873953 325 }
JonFreeman 0:7f5b51873953 326 for(int k = 7; k > 0; k--) {
JonFreeman 0:7f5b51873953 327 if (k == 4)
JonFreeman 0:7f5b51873953 328 txt[k] = '.';
JonFreeman 0:7f5b51873953 329 else {
JonFreeman 0:7f5b51873953 330 txt[k] = '0' + (p % 10);
JonFreeman 0:7f5b51873953 331 p /= 10;
JonFreeman 0:7f5b51873953 332 }
JonFreeman 0:7f5b51873953 333 }
JonFreeman 0:7f5b51873953 334 return txt; // Returns pointer unaltered for subsequent use by e.g. cout
JonFreeman 0:7f5b51873953 335 }
JonFreeman 0:7f5b51873953 336 public:
JonFreeman 0:7f5b51873953 337 signed long x, y, z, a; // Could easily expand up to six or more dros
JonFreeman 0:7f5b51873953 338 // bool dro_output; // To enabe / disable output to terminal
JonFreeman 0:7f5b51873953 339 void init () {
JonFreeman 0:7f5b51873953 340 x = y = z = a = 0; // These dro registers count pulses delivered to stepper motor driver
JonFreeman 0:7f5b51873953 341 // dro_output = true;
JonFreeman 0:7f5b51873953 342 }
JonFreeman 0:7f5b51873953 343 void update () {
JonFreeman 0:7f5b51873953 344 static long t = 300; // Prevent display immediately upon startup
JonFreeman 0:7f5b51873953 345 if (millisecs < t)
JonFreeman 0:7f5b51873953 346 return;
JonFreeman 0:7f5b51873953 347 // if(!idle && dro_output) {
JonFreeman 0:7f5b51873953 348 if(!idle) {
JonFreeman 0:7f5b51873953 349 char txt[12];
JonFreeman 0:7f5b51873953 350 pc.printf("dros X %s,", readout(txt, x)); // dro.n has running subtotal of all pulses issued to stepper driver.n
JonFreeman 0:7f5b51873953 351 pc.printf(" Y %s, Z ", readout(txt, y));
JonFreeman 0:7f5b51873953 352 pc.printf("%s, %s\r\n", readout(txt, z), running ? "R":"idle");
JonFreeman 0:7f5b51873953 353 if(!running) idle = true; // Purpose of idle flag is to stop dro updates JUST AFTER run completes.
JonFreeman 0:7f5b51873953 354 t = millisecs + 350; // Schedule next update after this non-blocking delay
JonFreeman 0:7f5b51873953 355 }
JonFreeman 0:7f5b51873953 356 }
JonFreeman 0:7f5b51873953 357 } dro_out ; // single instance of class digital_readout_stuff
JonFreeman 0:7f5b51873953 358
JonFreeman 0:7f5b51873953 359 void millisec_update_ISR () {
JonFreeman 0:7f5b51873953 360 millisecs++;
JonFreeman 0:7f5b51873953 361 }
JonFreeman 0:7f5b51873953 362
JonFreeman 0:7f5b51873953 363 /*#define STEP_IDLE_HI // Choose IDLE_HI or LO to suit any power save function of stepper motor drive units
JonFreeman 0:7f5b51873953 364 //#define STEP_IDLE_LO
JonFreeman 0:7f5b51873953 365 void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR***
JonFreeman 0:7f5b51873953 366 static const long step_mask = ASt | XSt | YSt | ZSt, // Added 6th Feb 14 Mask Does NOT include spindle bits
JonFreeman 0:7f5b51873953 367 dir_mask = ADi | XDi | YDi | ZDi; // Added 6th Feb 14 Mask Does NOT include spindle bits
JonFreeman 0:7f5b51873953 368 static signed long // 27 Feb 14 changed from unsigned
JonFreeman 0:7f5b51873953 369 #if defined Fourth_Axis
JonFreeman 0:7f5b51873953 370 acc_a = 0L, pir_a = 0L,
JonFreeman 0:7f5b51873953 371 #endif
JonFreeman 0:7f5b51873953 372 acc_x = 0L, // acc Accumuloators
JonFreeman 0:7f5b51873953 373 pir_x = 0L, // pir Phase Increment Registers
JonFreeman 0:7f5b51873953 374 acc_y = 0L, pir_y = 0L,
JonFreeman 0:7f5b51873953 375 acc_z = 0L, pir_z = 0L,
JonFreeman 0:7f5b51873953 376 acc_spin = 0L, // separate acc for spindle rotation NCO
JonFreeman 0:7f5b51873953 377 inc_x = 1L, // inc_x, y, z for updating DRO registers
JonFreeman 0:7f5b51873953 378 inc_y = 1L, inc_z = 1L,
JonFreeman 0:7f5b51873953 379 dir_bits = 0L, // direction flags for up to four axes
JonFreeman 0:7f5b51873953 380 oldSteps = 0L; //
JonFreeman 0:7f5b51873953 381 long tmp, newSteps = 0L;
JonFreeman 0:7f5b51873953 382
JonFreeman 0:7f5b51873953 383 intled = 1; // LED on for duration of interrupt service - point for scope probing
JonFreeman 0:7f5b51873953 384 ticks++; // count of interrupts serviced, vital to time end of movement
JonFreeman 0:7f5b51873953 385 charge_pumpD25pin1 = ticks & 0x01; // Can use 0x01 or 0x02 here to alter charge pump freq
JonFreeman 0:7f5b51873953 386 tmp = Steppers ^ direction_swappers;
JonFreeman 0:7f5b51873953 387 #if defined STEP_IDLE_LO
JonFreeman 0:7f5b51873953 388 tmp &= ~step_mask; // Step bits prepared for idle lo
JonFreeman 0:7f5b51873953 389 #endif
JonFreeman 0:7f5b51873953 390 #if defined STEP_IDLE_HI
JonFreeman 0:7f5b51873953 391 tmp |= step_mask; // Step bits prepared for idle hi
JonFreeman 0:7f5b51873953 392 #endif
JonFreeman 0:7f5b51873953 393 acc_spin += pir_spin; // Spindle NCO
JonFreeman 0:7f5b51873953 394 if (acc_spin < 0) tmp |= SSt;
JonFreeman 0:7f5b51873953 395 else tmp &= ~SSt;
JonFreeman 0:7f5b51873953 396 if (!running) Steppers = tmp ^ direction_swappers; // Axes not moving, spindle may be turning or not
JonFreeman 0:7f5b51873953 397 else { // running == true, Further manipulation of tmp follows, prior to rewriting to 'Steppers' IO Port
JonFreeman 0:7f5b51873953 398 // newSteps = 0L; // Added 6th Feb 14
JonFreeman 0:7f5b51873953 399 #if defined Fourth_Axis
JonFreeman 0:7f5b51873953 400 acc_a += pir_a;
JonFreeman 0:7f5b51873953 401 if (acc_a < 0) newSteps |= ASt;// Added 6th Feb 14
JonFreeman 0:7f5b51873953 402 #endif
JonFreeman 0:7f5b51873953 403 acc_x += pir_x; // Update phase of signals in accumulators
JonFreeman 0:7f5b51873953 404 if (acc_x < 0) newSteps |= XSt;// Added 6th Feb 14
JonFreeman 0:7f5b51873953 405 acc_y += pir_y;
JonFreeman 0:7f5b51873953 406 if (acc_y < 0) newSteps |= YSt;// Added 6th Feb 14
JonFreeman 0:7f5b51873953 407 acc_z += pir_z;
JonFreeman 0:7f5b51873953 408 if (acc_z < 0) newSteps |= ZSt;// Added 6th Feb 14
JonFreeman 0:7f5b51873953 409 // newSteps has copy of all 4 'acc' MSBs shifted into port bit positions
JonFreeman 0:7f5b51873953 410 oldSteps ^= newSteps; // Any bit of stbits set to initiate a Step pulse
JonFreeman 0:7f5b51873953 411 tmp ^= oldSteps;
JonFreeman 0:7f5b51873953 412 Steppers = tmp ^ direction_swappers; // Output signals to stepper motor drivers, next update dros from 'clocked' bits CLOCK IDLES HIGH
JonFreeman 0:7f5b51873953 413 if(oldSteps & XSt) dro_out.x += inc_x; // got clk edge for axis X
JonFreeman 0:7f5b51873953 414 if(oldSteps & YSt) dro_out.y += inc_y; // got clk edge for axis Y
JonFreeman 0:7f5b51873953 415 if(oldSteps & ZSt) dro_out.z += inc_z; // got clk edge for axis Z
JonFreeman 0:7f5b51873953 416 oldSteps = newSteps; // Added 6th Feb 14
JonFreeman 0:7f5b51873953 417 if (tickrun <= ticks & !new_run_pending) { // End of a machine movement detected, start next move here if possible
JonFreeman 0:7f5b51873953 418 running = false;
JonFreeman 0:7f5b51873953 419 move_ended = true;
JonFreeman 0:7f5b51873953 420 pir_x = 0L; // stop all stepper motors
JonFreeman 0:7f5b51873953 421 pir_y = 0L;
JonFreeman 0:7f5b51873953 422 pir_z = 0L;
JonFreeman 0:7f5b51873953 423 #if defined Fourth_Axis
JonFreeman 0:7f5b51873953 424 pir_a = 0L;
JonFreeman 0:7f5b51873953 425 #endif
JonFreeman 0:7f5b51873953 426 // ticks = 0L; // Simply to avoid having to think about overflow problems
JonFreeman 0:7f5b51873953 427 } // end of if (tickrun <= ticks) {
JonFreeman 0:7f5b51873953 428 } // end of else is (running) {
JonFreeman 0:7f5b51873953 429 if (!running & new_run_pending) { // Start axis movement
JonFreeman 0:7f5b51873953 430 dir_bits= dir_bits_next;
JonFreeman 0:7f5b51873953 431 #if defined Fourth_Axis
JonFreeman 0:7f5b51873953 432 pir_a = pir_a_next;
JonFreeman 0:7f5b51873953 433 #endif
JonFreeman 0:7f5b51873953 434 pir_x = pir_x_next;
JonFreeman 0:7f5b51873953 435 pir_y = pir_y_next;
JonFreeman 0:7f5b51873953 436 pir_z = pir_z_next;
JonFreeman 0:7f5b51873953 437 inc_x = inc_x_next;
JonFreeman 0:7f5b51873953 438 inc_y = inc_y_next;
JonFreeman 0:7f5b51873953 439 inc_z = inc_z_next;
JonFreeman 0:7f5b51873953 440 tmp = Steppers ^ direction_swappers; // read output lines
JonFreeman 0:7f5b51873953 441 tmp &= ~dir_mask;
JonFreeman 0:7f5b51873953 442 tmp |= dir_bits;
JonFreeman 0:7f5b51873953 443 Steppers = tmp ^ direction_swappers;
JonFreeman 0:7f5b51873953 444 tickrun = ticks + ticks_next;
JonFreeman 0:7f5b51873953 445 running = true; // Start the new run
JonFreeman 0:7f5b51873953 446 new_run_pending = false; // Clear the flag which initiated this update
JonFreeman 0:7f5b51873953 447 idle = false;
JonFreeman 0:7f5b51873953 448 } // end of else { // Not running. Grab next data here when or if available
JonFreeman 0:7f5b51873953 449 intled = 0; // LED off
JonFreeman 0:7f5b51873953 450 } // end of interrupt handler
JonFreeman 0:7f5b51873953 451 */
JonFreeman 0:7f5b51873953 452 /*
JonFreeman 0:7f5b51873953 453 * End of Interrupt Service Routine
JonFreeman 0:7f5b51873953 454 */
JonFreeman 0:7f5b51873953 455 /*bool spindle_running () {
JonFreeman 0:7f5b51873953 456 */
JonFreeman 0:7f5b51873953 457 class inputsreaderstuff {
JonFreeman 0:7f5b51873953 458 private:
JonFreeman 0:7f5b51873953 459 long ins_now;//, ins_old, ins_changed;
JonFreeman 0:7f5b51873953 460 public:
JonFreeman 0:7f5b51873953 461 void init () { ins_now = 0L;}//ins_old = ins_changed = 0L; }
JonFreeman 0:7f5b51873953 462 long read () {
JonFreeman 0:7f5b51873953 463 ins_now = 0;
JonFreeman 0:7f5b51873953 464 if (D25pin10_EStop) ins_now |= ESTOP;
JonFreeman 0:7f5b51873953 465 if (D25pin11_XLim) ins_now |= XLIM;
JonFreeman 0:7f5b51873953 466 if (D25pin12_YLim) ins_now |= YLIM;
JonFreeman 0:7f5b51873953 467 if (D25pin13_ZLim) ins_now |= ZLIM;
JonFreeman 0:7f5b51873953 468 if (D25pin15_unkn) ins_now |= UNKN;
JonFreeman 0:7f5b51873953 469 // ins_changed = ins_now ^ ins_old;
JonFreeman 0:7f5b51873953 470 // ins_old = ins_now;
JonFreeman 0:7f5b51873953 471 return ins_now;
JonFreeman 0:7f5b51873953 472 }
JonFreeman 0:7f5b51873953 473 // long changed () { return ins_changed; }
JonFreeman 0:7f5b51873953 474 } Inputs_From_Machine;
JonFreeman 0:7f5b51873953 475
JonFreeman 0:7f5b51873953 476 void report_inputs () {
JonFreeman 0:7f5b51873953 477 long i = Inputs_From_Machine.read();
JonFreeman 0:7f5b51873953 478 pc.printf("Inputs: EStop %d, XLim %d, YLim %d, ", i & ESTOP ? 1:0, i & XLIM ? 1:0, i & YLIM ? 1:0);
JonFreeman 0:7f5b51873953 479 pc.printf("ZLim %d, unkn %d\r\n", i & ZLIM ? 1:0, i & UNKN ? 1:0);
JonFreeman 0:7f5b51873953 480 }
JonFreeman 0:7f5b51873953 481
JonFreeman 0:7f5b51873953 482 int main() {
JonFreeman 0:7f5b51873953 483 long ins, ins_old, ins_changed = 0;
JonFreeman 0:7f5b51873953 484 pc.baud(BAUD); // comms to 'PuTTY' serial terminal via mbed usb
JonFreeman 0:7f5b51873953 485 spi.format (8,0); // use 8 bit format for compatibility with Freescale KLxxZ
JonFreeman 0:7f5b51873953 486 spi.frequency(12000000); // 12MHz, fast enough
JonFreeman 0:7f5b51873953 487 dro_out.init ();
JonFreeman 0:7f5b51873953 488 FPGA_setup();
JonFreeman 0:7f5b51873953 489 pc.printf("\r\n*\n*\nFound Computer %s\r\n", Target);
JonFreeman 0:7f5b51873953 490
JonFreeman 0:7f5b51873953 491 msec.attach_us(&millisec_update_ISR, 1001);
JonFreeman 0:7f5b51873953 492
JonFreeman 0:7f5b51873953 493 Thread comlin (command_line_interpreter, (void *)"cli"); // Read any instructions arriving via serial port and act upon them
JonFreeman 0:7f5b51873953 494 //#if defined I2C_Enable
JonFreeman 0:7f5b51873953 495 // Thread i2cstuff (i2c_handler, (void *)"i2c thing");
JonFreeman 0:7f5b51873953 496 //#endif
JonFreeman 0:7f5b51873953 497 ins = ins_old = Inputs_From_Machine.read ();
JonFreeman 0:7f5b51873953 498 move_ended = true; // Needed to kickstart system
JonFreeman 0:7f5b51873953 499
JonFreeman 0:7f5b51873953 500 while(1) { // Round Robin loop
JonFreeman 0:7f5b51873953 501 dro_out.update (); // Update DRO readings if, and as often as needed
JonFreeman 0:7f5b51873953 502 ins = Inputs_From_Machine.read ();
JonFreeman 0:7f5b51873953 503 ins_changed = ins ^ ins_old;
JonFreeman 0:7f5b51873953 504 ins_old = ins;
JonFreeman 0:7f5b51873953 505 if (ins_changed)
JonFreeman 0:7f5b51873953 506 pc.printf("Inputs Have Changed 0x%x, read 0x%x\r\n", ins_changed, ins);
JonFreeman 0:7f5b51873953 507 osThreadYield(); //
JonFreeman 0:7f5b51873953 508 } // end of Round Robin loop
JonFreeman 0:7f5b51873953 509 } // end of int main()
JonFreeman 0:7f5b51873953 510