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
Revision 0:5d0f270bfc87, committed 2014-01-31
- Comitter:
- JonFreeman
- Date:
- Fri Jan 31 11:16:21 2014 +0000
- Child:
- 1:66ee619f206b
- Commit message:
- First wip, tested on KL25 and KL46
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/arith.cpp Fri Jan 31 11:16:21 2014 +0000 @@ -0,0 +1,110 @@ +#include "mbed.h" +#include "cnc.h" +using namespace std; + +extern Serial pc; +extern double feed_rate; +const long ball_screw_pitch_mm = 4.0, // KX3 has 4mm ball screws + motor_step_per_rev = 200, // KX3 has 200 step per rev steppers + micro_steps = 32, // Arc Eurotrade choice 2,4,5,8,10,16,20,25,32,40,50,64,100,125,128 + pulses_per_mm = (micro_steps * motor_step_per_rev) / ball_screw_pitch_mm, + + interrupt_period_us = 24, //16, + interrupt_freq_Hz = 1000000 / interrupt_period_us, // Serious limit when doing all in software, no real limit with FPGA + max_pulse_freq_Hz = interrupt_freq_Hz / 6, // strictly 4, but allow a good margin + max_mm_per_min = (60 * max_pulse_freq_Hz) / pulses_per_mm; + +const double n_for_onemmpermin = (double)(pulses_per_mm * interrupt_period_us) * pow(2.0,32) / 60000000.0, // n pir to produce 1mm/min travel + feed_rate_max = 300.0, + feed_rate_min = 0.0, + spindle_max = 5000.0, + spindle_min = 0.0; +//The output frequency F<sub>out</sub> = 'Kernel Speed (Hz)' * n / (2 to the power of 32) + +struct Gparams last_position; +void grain_clr (struct singleGparam & g) { + g.dbl = 0.0; + g.ul = 0L; + g.i = g.c = 0; + g.changed = false; +} +void pirs_clr2 (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); +} +void init_last_position () { + pirs_clr2 (last_position); +} + +double find_distance (struct Gparams & from, struct Gparams & to, struct Gparams & distance) { + distance.x.dbl = to.x.dbl - from.x.dbl; + distance.y.dbl = to.y.dbl - from.y.dbl; + distance.z.dbl = to.z.dbl - from.z.dbl; // Yes, Pythagoras does work in 3D +return sqrt ((distance.x.dbl * distance.x.dbl) + (distance.y.dbl * distance.y.dbl) + (distance.z.dbl * distance.z.dbl)); +} + +double find_traverse_time (double dist, double rate) { // dist mm, rate mm/min + return 60.0 * dist / rate; // time secs +} + +long find_traverse_ticks (double dist, double rate) { // dist mm, rate mm/min + return (long)(find_traverse_time(dist, rate) * 1000000.0) / interrupt_period_us; +} + +/*void craptest () { + Gparams to, distance; + double dist; +// long q = NCO_n_per_Hz; +// feed_rate = 46.5; // global +// from.x.d = 0.0; +// from.y.d = 0.0; +// from.z.d = 0.0; + to.x.d = 45.0; + to.y.d = -12.375; + to.z.d = -3.142; + dist = find_distance (last_position, to, distance); + pc.printf ("From X %f Y %f Z %f to X %f Y %f Z %f\r\n",last_position.x.d, last_position.y.d, last_position.z.d, to.x.d, to.y.d, to.z.d); + pc.printf ("Dist X %f Y %f Z %f, total %f\r\n", distance.x.d, distance.y.d, distance.z.d, dist); + pc.printf ("To move %f mm at feed rate %f mm/min takes %f seconds\r\n", dist, feed_rate, find_traverse_time(dist, feed_rate)); + pc.printf ("This involves %d interrupt ticks\r\n", find_traverse_ticks(dist, feed_rate)); +// pc.printf ("NCO freq = %f when n = %d\r\n", NCO_freq_from_n(q), q); +// pc.printf ("Nfor1mmpersec is %f, pulses_per_mm is %d\r\n",onemmpersec, pulses_per_mm); +}*/ + +void copy_grain (struct singleGparam & d, struct singleGparam & s) { + d.dbl = s.dbl; + d.ul = s.ul; + d.i = s.i; + d.c = s.c; + d.changed = s.changed; +} + +void copy_pirs (struct Gparams & d, struct Gparams & s) { + copy_grain (d.x, s.x); + copy_grain (d.y, s.y); + copy_grain (d.z, s.z); + copy_grain (d.i, s.i); + copy_grain (d.j, s.j); + copy_grain (d.r, s.r); + copy_grain (d.a, s.a); + copy_grain (d.b, s.b); + copy_grain (d.c, s.c); + copy_grain (d.d, s.d); +} + +/*void swap_pirs (struct pirs * d, struct pirs * s) { +//void swap_pirs () { + struct pirs pira, pirb; + struct pirs * ppa, * ppb, * pptmp; + ppa = & pira; + ppb = & pirb; + ppa->x.d = 1.0; + ppa->y.d = 2.0; + ppb->x.d = 2.0; + ppb->y.d = 1.0; + pc.printf ("pira x = %f, y = %f, pirb x = %f, y = %f,\r\n", ppa->x.d, ppa->y.d, ppb->x.d, ppb->y.d); + pptmp = ppa; + ppa = ppb; + ppb = pptmp; + pc.printf ("pira x = %f, y = %f, pirb x = %f, y = %f,\r\n", ppa->x.d, ppa->y.d, ppb->x.d, ppb->y.d); +}*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/cnc.h Fri Jan 31 11:16:21 2014 +0000 @@ -0,0 +1,25 @@ +#define axis_speeds_buffsize 20 + +struct axis_speeds_element { + signed long x, y, z, a, duration_ticks; + bool ready; +} ; + +struct singleGparam { // Place to put all we know about 'x' or 'j' etc parameter from G Code line + double dbl; + unsigned long ul; + int i, c; + bool changed; // Flagged true when new value for this axis found in Gcode line, false otherwise +} ; + +struct Gparams { // Where possibly messy G code line gets ordered and sorted into + struct singleGparam x, y, z, i, j, r, a, b, c, d; // After sorting, know where to find any X, Y etc values ! +} ; + +struct digital_readouts { + signed int x, y, z, a, b, c; // Allow up to six dros + bool dro_output; // To enabe / disable output to terminal +} ; + +extern const double n_for_onemmpermin, feed_rate_max, feed_rate_min, spindle_min, spindle_max; +extern const long pulses_per_mm, max_mm_per_min, interrupt_period_us;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/command_interpreter.cpp Fri Jan 31 11:16:21 2014 +0000 @@ -0,0 +1,428 @@ +#include "mbed.h" +#include "cnc.h" +using namespace std; + +extern Serial pc; +extern void pir_updater (struct axis_speeds_element * p) ; // Uses pointer as we may wish to rapid update from circular buffer + +double feed_rate = 1.0; // global scope, mm per minute + +bool isdigit (int a) +{ + if(a > ('0' - 1) && a < ('9' + 1)) + return true; + return false; +} + +bool isupper (int a) +{ + if ((a >= 'A') && (a <= 'Z')) return true; + return false; +} + +int tolower (int a) +{ + if (isupper(a)) + a += 'a' - 'A'; + return a; +} + +extern double find_distance (struct Gparams & from, struct Gparams & to, struct Gparams & distance); +extern long find_traverse_ticks(double dist, double feed_rate); +extern struct Gparams last_position; + +const int goodcodes[] = {0,'a','b','c','i','j','l','r','x','y','z'}; // possible G Code options +const int const_numofcodes = sizeof(goodcodes) / sizeof(int); + +int find_char_in_goodcodes (int target) // Returns position of char in goodcodes[], 0 if not found. +{ + for (int i = 1; i < const_numofcodes; i++) + if (goodcodes[i] == target) + return i; + return 0; +} + +/* +void get_codepositions (struct singleGparam * a, struct Gparams & p) + Only call from "void g0g1cmdcore (struct singleGparam * a, double f_rate)" +Purpose: + G code line may have any number of valid axes or parameters entered in any order or position. + This function detects any X,Y,Z,A,I,J,R entries in 'p' if present and copies values into their + respective positions within singleGparam 'a', setting the 'changed' flag for each to true if found, + false if not found +struct Gparams { // Where possibly messy G code line gets ordered and sorted into + struct singleGparam x, y, z, i, j, r, a, b, c, d; // After sorting, know where to find any X, Y etc values ! +} ; +*/ +void get_codepositions (struct singleGparam * source_array, struct Gparams & dest) +{ +//const int goodcodes[] = {0,'a','b','c','i','j','l','r','x','y','z'}; // possible G Code options +//const int const_numofcodes = sizeof(goodcodes) / sizeof(int); + int codecnt[const_numofcodes +1]; + int codepos[const_numofcodes +1]; + int j; + for (j = 0; j < const_numofcodes; j++) + codecnt[j] = codepos[j] = 0; // Zero all results + for (int i = 1; i <= source_array[0].i; i++) { // for number of parameters passed to us here + for(j = 0; j < const_numofcodes; j++) { // for a, for b, ... for x, then y, then z + if (source_array[i].c == goodcodes[j]) { + codecnt[j]++; // Count of number of 'a's, 'b's ... 'x's, 'y's, 'z's. All should be 0 or 1 but could be more + codepos[j] = i; // Identifies the a[?] containing last incidence of goodcodes[j] + } + } + } + dest.x.changed = dest.y.changed = dest.z.changed = dest.a.changed = false; + dest.i.changed = dest.j.changed = dest.r.changed = false; + dest.x.dbl = last_position.x.dbl; // copy previous coordinates in case not re-specified + dest.y.dbl = last_position.y.dbl; dest.z.dbl = last_position.z.dbl; + dest.a.dbl = last_position.a.dbl; dest.i.dbl = last_position.i.dbl; + dest.j.dbl = last_position.j.dbl; dest.r.dbl = last_position.r.dbl; + j = codepos[find_char_in_goodcodes('a')]; + if (j) { + dest.a.changed = true; + dest.a.dbl = source_array[j].dbl; + } + j = codepos[find_char_in_goodcodes('x')]; + if (j) { + dest.x.changed = true; + dest.x.dbl = source_array[j].dbl; + } + j = codepos[find_char_in_goodcodes('y')]; + if (j) { + dest.y.changed = true; + dest.y.dbl = source_array[j].dbl; + } + j = codepos[find_char_in_goodcodes('z')]; + if (j) { + dest.z.changed = true; + dest.z.dbl = source_array[j].dbl; + } + j = codepos[find_char_in_goodcodes('i')]; + if (j) { + dest.i.changed = true; + dest.i.dbl = source_array[j].dbl; + } + j = codepos[find_char_in_goodcodes('j')]; + if (j) { + dest.j.changed = true; + dest.j.dbl = source_array[j].dbl; + } + j = codepos[find_char_in_goodcodes('r')]; + if (j) { + dest.r.changed = true; + dest.r.dbl = source_array[j].dbl; + } +} + + +void g0g1cmdcore (struct singleGparam * source_array, double f_rate) // Updates any / all of x, y, z NCOs +{ + struct Gparams pxyz, distance; + struct axis_speeds_element q; + get_codepositions (source_array, pxyz); // will overwrite with new where entered + pc.printf("g0"); + if (pxyz.x.changed) {pc.printf(" X %f", pxyz.x.dbl);} + if (pxyz.y.changed) {pc.printf(" Y %f", pxyz.y.dbl);} + if (pxyz.z.changed) {pc.printf(" Z %f", pxyz.z.dbl);} + pc.printf("\r\n"); +// for (int j = 1; j < const_numofcodes; j++) { +// pc.printf ("Count of %c is %d, last position %d, last value %f\r\n", goodcodes[j], codecnt[j], codepos[j], a[codepos[j]].d); +// } + double distT = find_distance (last_position, pxyz, distance); // also fills in distance x y z + double temp = n_for_onemmpermin * f_rate / distT; + q.duration_ticks = find_traverse_ticks(distT, f_rate); + last_position.x.dbl = pxyz.x.dbl; // Update global last_position record + last_position.y.dbl = pxyz.y.dbl; + last_position.z.dbl = pxyz.z.dbl; + q.x = (signed long)(temp * distance.x.dbl); + q.y = (signed long)(temp * distance.y.dbl); + q.z = (signed long)(temp * distance.z.dbl); + q.a = 0; + pir_updater (&q); // pir_updater (struct Gparams & p); // To arrive here with wanted 'mm per min' values in x, y and z +} + +void g0cmd (struct singleGparam * a) // Updates any / all of x, y, z NCOs +{ + g0g1cmdcore (a, feed_rate_max); // Defined parameter in code +} + +void g1cmd (struct singleGparam * a) // Updates any / all of x, y, z NCOs +{ + g0g1cmdcore (a, feed_rate); // Settable feed_rate +} + +void fcmd (struct singleGparam * a) { + if (a[1].dbl < feed_rate_min || a[1].dbl > feed_rate_max) { + pc.printf ("Errror setting feed rate, can't set to %f, ignoring request\r\n", a[1].dbl); + return; + } + pc.printf ("Setting feed_rate to %f\r\n", a[1].dbl); + feed_rate = a[1].dbl; +} + +extern unsigned long pir_s; +extern int spindlefwdrev; + +void sfcmd (struct singleGparam * a) {pc.printf("Spindle Fwd\r\n"); spindlefwdrev = 0;} +void srcmd (struct singleGparam * a) {pc.printf("Spindle Rev\r\n"); spindlefwdrev = 4;} +void stopcmd (struct singleGparam * a) {pc.printf("Stop ! er, not working yet\r\n");} + +void scmd (struct singleGparam * a) { + pc.printf("pir_s=0x%x\r\n", pir_s); + if (a[1].dbl < spindle_min || a[1].dbl > spindle_max) { + pc.printf ("Errror setting spindle RPM, can't set to %f, ignoring request\r\n", a[1].dbl); +// return; + } + pc.printf ("Setting spindle RPM to %f\r\n", a[1].dbl); +// feed_rate = a[1].d; // ****TO DO**** + pir_s = (unsigned long) (a[1].dbl * 4096); + pc.printf("pir_s=0x%x\r\n", pir_s); +} + +//void stopcmd (struct grain * a) {pc.printf("Stop !\r\n");} +void m1cmd (struct singleGparam * a) {pc.printf("m1 Optional Programme Stop\r\n");} +void m3cmd (struct singleGparam * a) {pc.printf("m3 Rotate Spindle Clockwise\r\n");} +void m4cmd (struct singleGparam * a) {pc.printf("m4 Rotate Spindle Counter Clockwise\r\n");} +void m5cmd (struct singleGparam * a) {pc.printf("m5 Stop Spindle\r\n");} +/*void m30cmd (struct singleGparam * a) {pc.printf("m30 Programme End and Rewind\r\n");} +void m47cmd (struct singleGparam * a) {pc.printf("m47 Repeat Prog from First Line\r\n");} +void m48cmd (struct singleGparam * a) {pc.printf("m48 Enable Speed and Feed Override\r\n");} +void m49cmd (struct singleGparam * a) {pc.printf("m49 Disable Speed and Feed Override\r\n");} +void m98cmd (struct singleGparam * a) {pc.printf("m98 Call Subroutine\r\n");} +void m99cmd (struct singleGparam * a) {pc.printf("m99 Return from Subroutine\r\n");} +void g10cmd (struct singleGparam * a) {pc.printf("g10 Coord System Origin Set\r\n");} +void g17cmd (struct singleGparam * a) {pc.printf("g17 XY Plane Select\r\n");} +void g20cmd (struct singleGparam * a) {pc.printf("g20 Inch\r\n");} +void g21cmd (struct singleGparam * a) {pc.printf("g21 mm\r\n");} + +void g40cmd (struct singleGparam * a) {pc.printf("g40 Cutter Compensation Off\r\n");} +void g50cmd (struct singleGparam * a) {pc.printf("g50 Reset Scale Factors\r\n");} +void g53cmd (struct singleGparam * a) {pc.printf("g53 Move in Absolute Coordinates\r\n");} +void g90cmd (struct singleGparam * a) {pc.printf("g90 Absolute Distance Mode\r\n");} +*/ +void g2cmd (struct singleGparam * a) {pc.printf("g2 Clockwise Arc\r\n");} +void g3cmd (struct singleGparam * a) {pc.printf("g3 CounterClockwise Arc\r\n");} +void g4cmd (struct singleGparam * a) {pc.printf("g4 Dwell\r\n");} +void g91p1cmd (struct singleGparam * a) {pc.printf("g91.1 \r\n");} + +extern struct digital_readouts dro; // +void drooncmd (struct singleGparam * a) +{ + dro.dro_output = true; // Enable continuous dro display update +} +void drooffcmd (struct singleGparam * a) +{ + dro.dro_output = false; // Disable continuous dro display update +} + +//extern void craptest () ; +//void g1cmd (struct singleGparam * a) { +// craptest (); +//} + +void g90p1cmd (struct singleGparam * a) +{ + pc.printf ("Arrived at function fredcmd with %d parameters\r\n", a[0].i); + for (int i = 1; i <= a[0].i; i++) { + pc.printf ("*%c* ", a[i].c); + pc.printf ("%d, ", a[i].i); + pc.printf ("%f\r\n", a[i].dbl); + } + pc.printf (" endof param list\r\n"); +} + +void menucmd (struct singleGparam * a); +struct kb_command { + const char * cmd_word; // points to text e.g. "menu" + const char * explan; + void (*f)(struct singleGparam *); // points to function +} kbc[] = { + {(char const *)"menu", "Lists available commands, same as ls", menucmd}, + {(char const *)"ls", "Lists available commands, same as menu", menucmd}, + {"stop", "To Stop the Machine !", stopcmd}, + {"sf", "Spindle Clockwise", sfcmd}, + {"sr", "Spindle Anticlockwise", srcmd}, + {"f ", "To set Feed Rate mm/min, e.g. f 25", fcmd}, + {"s ", "To set Spindle RPM, e.g. S 1250", scmd}, + {"g0", "Not Implemented", g0cmd}, + /*{"m30", "Not Implemented", m30cmd}, + {"m47", "Not Implemented", m47cmd}, + {"m48", "Not Implemented", m48cmd}, + {"m49", "Not Implemented", m49cmd}, + {"m98", "Not Implemented", m98cmd}, + {"m99", "Not Implemented", m99cmd}, + {"m1", "Not Implemented", m1cmd}, + {"m3", "Not Implemented", m3cmd}, + {"m4", "Not Implemented", m4cmd}, + {"m5", "Not Implemented", m5cmd}, + {"g10", "Not Implemented", g10cmd}, + {"g17", "Not Implemented", g17cmd}, + {"g20", "Not Implemented", g20cmd}, + {"g21", "Not Implemented", g21cmd}, + {"g40", "Not Implemented", g40cmd}, + {"g50", "Not Implemented", g50cmd}, + {"g90.1", "Not Implemented", g90p1cmd}, + {"g91.1", "Not Implemented", g91p1cmd}, + {"g90", "Not Implemented", g90cmd}, + */ + {"g1", "", g1cmd}, + {"g2", "", g2cmd}, + {"g3", "", g3cmd}, + {"g4", "", g4cmd}, + {"dro on", "Turn dro readout on", drooncmd}, + {"dro off", "Turn dro readout off", drooffcmd} +}; +const int numof_menu_items = sizeof(kbc) / sizeof(kb_command); + +void menucmd (struct singleGparam * a) +{ + pc.printf("At menucmd function - listing commands:-\r\n"); + for(int i = 0; i < numof_menu_items; i++) + pc.printf("[%s]\t\t%s\r\n", kbc[i].cmd_word, kbc[i].explan); + pc.printf("End of List of Commands\r\n"); +} + +bool isalpha (int c) +{ + if ((c >= 'a') && (c <= 'z')) return true; + if ((c >= 'A') && (c <= 'Z')) return true; + return false; +} + +char * readout (char * txt, int 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; + 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 +} + +////class CLI { + +const int MAX_PARAMS = 20, MAX_CMD_LEN = 120; +char cmd_line[MAX_CMD_LEN + 4]; +struct singleGparam params[MAX_PARAMS + 1]; +int cl_index = 0, ch, lastalpha = 0; +double fracmul; +/* +void command_line_interpreter () +Purpose: + +*/ +void command_line_interpreter () +{ + while (pc.readable()) { + if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines + pc.printf ("Keyboard Error!! Killing stupidly long command line"); + cl_index = 0; + } + ch = tolower(pc.getc()); + if(ch != '\r') // was this the 'Enter' key? + cmd_line[cl_index++] = ch; // added char to command being assembled + else { // key was CR, may or may not be command to lookup + cmd_line[cl_index] = 0; // null terminate command string + if(cl_index) { // If have got some chars to lookup + int i, wrdlen; + for (i = 0; i < numof_menu_items; i++) { // Look for input match in command list + wrdlen = strlen(kbc[i].cmd_word); + if(strncmp(kbc[i].cmd_word, cmd_line, wrdlen) == 0) { // If match found + bool negflag = false; + int state = 0, paramindex; +// pc.printf("Found match for word [%s]\r\n", kbc[i].wrd); + for(paramindex = 0; paramindex < MAX_PARAMS; paramindex++) { + // Clear out whole set of old parameters ready for anything new on this line + params[paramindex].i = 0; // for integer parameters + params[paramindex].c = 0; // for last alpha char, helps tie 'X' to '-23.5' etc + params[paramindex].dbl = 0.0; // for floating point parameters + params[paramindex].ul = 0; + params[paramindex].changed = false; + } + paramindex = 0; + // read any parameters from command line here + // Using parameters[0] as count of parameters to follow + while (wrdlen <= cl_index) { + ch = cmd_line[wrdlen++]; + if(isalpha(ch)) lastalpha = ch; + if(ch == '-') negflag = true; + if(ch == '+') negflag = false; + switch (state) { + case 0: // looking for start of a number string + if(isdigit(ch)) { // found first digit of a number string + paramindex++; + if(paramindex > MAX_PARAMS) { + wrdlen = cl_index; // exit condition + pc.printf("WARNING - too many parameters, ignoring extra\r\n"); + } else { + params[paramindex].i = ch - '0'; + params[paramindex].c = lastalpha; + state = 1; // Found first digit char of number string + } + } + break; + case 1: // looking for end of a number string + if(isdigit(ch)) { // accumulating integer from string + params[paramindex].i *= 10; + params[paramindex].i += ch - '0'; + } else { // found non-digit terminating number + if (ch == '.') { + state = 2; + fracmul = 0.1; + params[paramindex].dbl = (double)params[paramindex].i; + } else { + params[0].i++; // count of validated parameters + state = 0; // Have read past last digit of number string + if(negflag) { + params[paramindex].i = -params[paramindex].i; + negflag = false; + } + params[paramindex].dbl = (double)params[paramindex].i; + } + } + break; + case 2: // looking for fractional part of double + if(isdigit(ch)) { // accumulating fractional part from string + params[paramindex].dbl += (double)((ch - '0') * fracmul); + fracmul /= 10.0; + } else { // found non-digit terminating double precision number + params[0].i++; // count of validated parameters + state = 0; // Have read past last digit of number string + if(negflag) { + params[paramindex].i = -params[paramindex].i; + params[paramindex].dbl = -params[paramindex].dbl; + negflag = false; + } + } + break; + default: + break; + } // end of switch state + } // end of while wrdlen < cl_index +// pc.printf("Found match to [%s] with %d parameters\r\n", kbc[i].wrd, paramindex); + kbc[i].f(params); // execute command + i = numof_menu_items + 1; // to exit for loop + } + } // End of for numof_menu_items + if(i == numof_menu_items) + pc.printf("No Match Found for CMD [%s]\r\n", cmd_line); + } // End of If have got some chars to lookup + cl_index = lastalpha = 0; + } // End of else key was CR, may or may not be command to lookup + } // End of while (pc.readable()) +// osThreadYield(); // Not using RTOS on this project +} + +////} cli; +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lissajous.cpp Fri Jan 31 11:16:21 2014 +0000 @@ -0,0 +1,101 @@ +#include "mbed.h" +//extern Serial pc; +/* +This file contains one function: +void lissajous () ; +The purpose is to replicate a list of XY coordinates produced by a G Code programme written to run on Mach3 software +to be used to set NCOs in sequence to prove this simple code running on a 'mbed' or 'Freescale KL25Z' +does correctly drive the Sieg KX3 CNC mill, just the same as using the pc / Mach3 setup. + +Thus far we have proved only that both finish at the same point (give or take a few microns) +*/ + +const double Deg2Rad = atan(1.0) / 45.0, + HALFPI = 2.0 * atan(1.0), + TWOPI = 8.0 * atan(1.0), + PI = 4.0 * atan(1.0), + MaxX = 6.40, + MaxY = 3.20, + StartAngDegX = 0.0, + StartAngDegY = 10.0, + FreqRatio = 0.254; +const int StepsPerRevX = 100, + NumofXCycles = 16; + +void lissajous () { + double AngleX = StartAngDegX * Deg2Rad, + AngleY = StartAngDegY * Deg2Rad, + AngleStepX = (TWOPI / StepsPerRevX), + AngleStepY = AngleStepX * FreqRatio, + X_Coord = MaxX * cos(AngleX), + Y_Coord = MaxY * sin(AngleY); + + for (int i = 0; i < NumofXCycles; i++) { + for (int j = 0; j < StepsPerRevX; j++) { + AngleX += AngleStepX; + AngleY += AngleStepY; + X_Coord = MaxX * cos(AngleX); + Y_Coord = MaxY * sin(AngleY); + } + } +// pc.printf("Lissajous finish point X%f, Y%f\r\n", X_Coord, Y_Coord); +} + +/* +The complete Mach3 G Code programme listing "lissajous.txt" follows :- +*/ + +/* +; This Section to put machine into known, safe state +M5 ; Stop spindle +G17 ; Select XY plane +G21 ; Units are mm +G40 ; Cancel cutter radius compensation +G49 ; Cancel tool length offset +G61 ; Exact stop +G50 ; Reset all scale factors to 1.0 +G90 ; Absolute distance mode +G94 ; Feed mm per minute mode - as mm selected above by G21 +; Title: Lissajous Pattern Generator 2014 +; Programme Name "lissajous.txt" +; Author: Jon Freeman +; Date: Feb 2014 + +; Demo code used to demonstrate Freescale FRDM-KL25Z computer board +; driving a Sieg KX3 CNC mill without PC, and without Mach3 !! + +; _____________________________________________ +; Put user alterable parameter values in this section +; User is invited to alter the 6 parameters in this section. + +#10 = 6.40 ; Max 'X' excursion +#11 = 3.20 ; Max 'Y' excursion +#12 = 0.0 ; Start angle of 'X' +#13 = 10.0 ; Start angle of 'Y' +#14 = 0.254 ; Frequency ratio of X and Y signals +#15 = 100 ; Int Steps per 2PI of X +#16 = 16 ; Int Number of whole cycles of 'X' +; +; Programme starts here + +#50 = [#10 * cos[#12]] ;Start X coord +#51 = [#11 * sin[#13]] ;Start Y coord +#52 = [360.0 / #15] ;Angle step X +#53 = [#52 * #14] ;Angle step Y +G0 X#50 Y#51 +M98 P 1000 L #16 ;Execute subroutine 'Numof X Cycles' times +M5 M30 ; Stop, end and rewind + +O 1000 ; Subroutine executed once per complete turn of 'X' +M98 P 2000 L #15 ;Execute the subroutine and repeat 'Steps per Rev' times +M99 ; Return + +O 2000 ; Subroutine executed 'Numof X Cycles' * 'Steps per Rev' times +#12 = [#12 + #52] ; Update X angle +#13 = [#13 + #53] ; Update X angle +#50 = [#10 * cos[#12]] ;Update X coord +#51 = [#11 * sin[#13]] ;Update Y coord +G1 X#50 Y#51 +M99 ; Return +*/ +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jan 31 11:16:21 2014 +0000 @@ -0,0 +1,278 @@ +#include "mbed.h" +#include "cnc.h" +using namespace std; +extern void lissajous (void) ; +extern void command_line_interpreter (void) ; +extern void init_last_position () ; +extern char * readout (char * txt, int p) ; // p has running subtotal of all pulses issued to stepper driver + +Serial pc(USBTX, USBRX); // tx, rx to pc +const int BAUD = 9600; +Ticker NCO_gen; // Ticker generating interrupts at NCO updating frequency +struct axis_speeds_element axis_speeds[axis_speeds_buffsize + 1]; // buffer space for list of future moves +struct digital_readouts dro; //some signed int + +bool running = false; +volatile unsigned long ticks = 0L; +unsigned long + pir_a = 0L, + pir_x = 0L, + pir_y = 0L, + pir_z = 0L, + pir_s = 0L, // Referenced only in command_interpreter as spindle speed setting + tickrun = 0L; + +int spindlefwdrev = 0; // Takes values of 0 or 4 only + +#if defined (TARGET_KL25Z) + DigitalOut intled(PTE1); //J2p20 + //SPISlave spidevice(PTD3, PTD2, PTD1, PTD0); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) + SPISlave spidevice(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) + // J2p08,J2p10,J2p12, J2p06 + //SPI spidevice(PTD2, PTD3, PTD1); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) + //SPI spidevice(PTD3, PTD2, PTD1); // mosi, miso, sclk THIS TURNS LED ON BLUE ! (uses p11, p12, p13 on mbed LPC) + //NOTE doubt possibly miso mosi in wrong order here, PTD3 and PTD2 + #define STEPPER_PORT PortC + const int PortBitXSt = 3, // Port bit num X Step J1P05 + PortBitXDi = 4, // Port bit num X Dir J1P07 + PortBitYSt = 5, // Port bit num Y Step J1P09 + PortBitYDi = 6, // Port bit num Y Dir J1P11 + PortBitZSt = 10, // Port bit num Z Step J1P13 + PortBitZDi = 11, // Port bit num Z Dir J1P15 + PortBitASt = 12, // Port bit num A Step J2P01 + PortBitADi = 13; // Port bit num A Dir J2P03 +#endif +#if defined (TARGET_KL46Z) + DigitalOut intled(PTE1); //J2p20 + SPISlave spidevice(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC) + // J2p13, J2p15, J2p11, J2p09 + // Easy way to allocate port bits for *** N O T CHECKED for 46Z *** + // output of stepper motor Step and DIR sigs + #define STEPPER_PORT PortC + const int PortBitXSt = 0, // Port bit num X Step J1P05 + PortBitXDi = 4, // Port bit num X Dir J1P07 + PortBitYSt = 6, // Port bit num Y Step J1P09 + PortBitYDi = 7, // Port bit num Y Dir J1P11 + PortBitZSt = 10, // Port bit num Z Step J1P13 + PortBitZDi = 11, // Port bit num Z Dir J1P15 + PortBitASt = 13, // Port bit num A Step J2P01 + PortBitADi = 16; // Port bit num A Dir J2P03 +#endif +#if defined (TARGET_MBED_LPC1768) + DigitalOut intled(LED2); + SPISlave spidevice(p5, p6, p7, p8); + // Easy way to allocate port bits for *** N O T CHECKED for MBED_LPC1768 *** + // 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 + P0.05 p29 CAN td - USE X Dir + P0.10 p28 SDA - USE Y Step + P0.11 p27 SCL - USE Y Dir + P0.15 p13 Tx - USE Z Step + P0.16 p14 Rx - USE Z Dir + P0.17 p12 miso - USE A Step + P0.18 p11 mosi - Use A Dir + P0.23 p15 A In + P0.24 p16 A In + P0.25 p17 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 +#endif + +static const long + XSt1 = 1 << PortBitXSt, XSt0 = 0, + XDi1 = 1 << PortBitXDi, XDi0 = 0, + YSt1 = 1 << PortBitYSt, YSt0 = 0, + YDi1 = 1 << PortBitYDi, YDi0 = 0, + ZSt1 = 1 << PortBitZSt, ZSt0 = 0, + ZDi1 = 1 << PortBitZDi, ZDi0 = 0, + ASt1 = 1 << PortBitASt, ASt0 = 0, + ADi1 = 1 << PortBitADi, ADi0 = 0, + + SM_MASK = (XSt1 | XDi1 | YSt1 | YDi1 | ZSt1 | ZDi1 | ASt1 | ADi1); + +PortOut mysteppers(STEPPER_PORT, SM_MASK); + +/* +* Interrupt Service Routine +*/ +//void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR*** +// intled = 1; +// ticks++; +// intled = 0; +//} +void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR*** + const long bit_lutx[4] = {XSt0 | XDi0, XSt0 | XDi1, XSt1 | XDi1, XSt1 | XDi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs + bit_luty[4] = {YSt0 | YDi0, YSt0 | YDi1, YSt1 | YDi1, YSt1 | YDi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs + bit_lutz[4] = {ZSt0 | ZDi0, ZSt0 | ZDi1, ZSt1 | ZDi1, ZSt1 | ZDi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs + bit_luta[4] = {ASt0 | ADi0, ASt0 | ADi1, ASt1 | ADi1, ASt1 | ADi0}, // Used to look-up 'clk' and 'dir' signals from accum MSBs + bits2shift = (sizeof (long) << 3) - 2; + static unsigned long +// acc_s = 0L, // For Spindle motor, probably not needed as may be pwm + acc_a = 0L, + acc_x = 0L, + acc_y = 0L, + acc_z = 0L; + static int obitz = 0; + int oldbitz, acts; + + intled = 1; // LED on for duration of interrupt service - point for scope probing + ticks++; // count of interrupts serviced +// int response = spidevice.write(0x55); // Only if SPI Master -- TAKES 2.5 us -- +// The rest of the whole int handler takes only about 3.0 us + acc_x += pir_x; // Update phase of signals in accumulators + acc_y += pir_y; + acc_z += pir_z; + acc_a += pir_a; +// acc_s += pir_s; // pir_s used for spindle speed + oldbitz = obitz; // pin output levels as determined during previous interrut + obitz = bit_lutx[acc_x >> bits2shift] | bit_luty[acc_y >> bits2shift] | bit_lutz[acc_z >> bits2shift] | bit_luta[acc_a >> bits2shift]; + + mysteppers = obitz; // Output signals to stepper motor drivers, next look for _- pos clk events on bits 0, 2 and 4 + + acts = (~oldbitz & obitz); // get pos clk edge triggers in bits 0, 2 and 4 (1, 4, 16) + acts |= (obitz & (XDi1 | YDi1 | ZDi1)); // get directions + if(acts & XSt1) { // got pos clk edge for axis X + if (acts & XDi1) + dro.x++; + else dro.x--; + } + if(acts & YSt1) { // got pos clk edge for axis Y + if (acts & YDi1) + dro.y++; + else dro.y--; + } + if(acts & ZSt1) { // got pos clk edge for axis Z + if (acts & ZDi1) + dro.z++; + else dro.z--; + } + if (running && tickrun <= ticks) { // End of a machine movement detected, start next move here if possible + running = false; + pir_x = 0L; // stop all stepper motors + pir_y = 0L; + pir_z = 0L; + pir_a = 0L; + } + intled = 0; // LED off +} // end of interrupt handler + +/* +* End of Interrupt Service Routine +*/ + + +void pir_updater (struct axis_speeds_element * p) { // To arrive here with wanted 'mm per min' values in x, y and z +//void pir_updater (struct pirs * p) { // To arrive here with wanted 'mm per min' values in x, y and z +// pc.printf(p.x ? "true":"false"); // Uses pointer as we may wish to rapid update from circular buffer + tickrun = p->duration_ticks; + unsigned long tc = ticks, after; + while (tc == ticks) {} // wait until just after an interrupt - note requires 'volatile' ticks + tickrun += ticks; + pir_x = p->x; // Update NCO phase inc registers + pir_y = p->y; + pir_z = p->z; + pir_a = p->a; + after = ticks - tc; + running = true; + if (after == 1) + pc.printf("pir_update was good !, ticks %d\r\n", p->duration_ticks); + else + pc.printf("Oops! Looks like pir_update got run-over, code = %d\r\n", after); +} + +int main() { + char txt[10]; // few chars used for dro output + pc.baud(BAUD); // comms to 'PuTTY' serial terminal via mbed usb + dro.x = dro.y = dro.z = 0; // These dro registers count pulses delivered to stepper motor driver + dro.dro_output = true; + init_last_position () ; // Zeros one 'pirs' structure + spidevice.format(8, 0); // 8 bits mode 0, // p11 mosi, p12 miso, p13 sclk ** ONLY 8 BIT ** + spidevice.frequency(12000000); // 12MHz bit rate +// int response = spidevice.write(0xFFFF); // Only if SPI Master +// spidevice.reply(0x00); // Prime SPI with first reply + /* +// Reply to a SPI master as slave + + #include "mbed.h" + + SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel + + int main() { + device.reply(0x00); // Prime SPI with first reply + while(1) { + if(device.receive()) { + int v = device.read(); // Read byte from master + v = (v + 1) % 0x100; // Add one to it, modulo 256 + device.reply(v); // Make this the next reply + } + } + } */ + + struct axis_speeds_element * asepp = axis_speeds; // Address of axis_speeds[0] + for (int i = 0; i < axis_speeds_buffsize; i++) { + axis_speeds[i].x = + axis_speeds[i].y = + axis_speeds[i].z = + axis_speeds[i].a = + axis_speeds[i].duration_ticks = 0L; + axis_speeds[i].ready = false; + } +// pc.printf("SPI Setup returned 0x%x\r\n", response); +/* int ch; + while (true) { + while (pc.readable()) { + ch = pc.getc(); + pc.printf("**%c**", ch); + } + pc.printf("No more\r\n"); + wait(0.5); + } + */ + lissajous (); +#if defined (TARGET_KL25Z) + pc.printf ("Found device Freescale KL25Z\r\n"); +// DigitalOut intled(PTA1); ** THIS KILLS SERIAL Rx ** +#endif +#if defined (TARGET_KL46Z) + pc.printf ("Found device Freescale KL46Z\r\n"); +#endif +#if defined (TARGET_MBED_LPC1768) + pc.printf ("Found device MBED_LPC1768\r\n"); +#endif + pc.printf("Three NCOs have been setup, they will move when given values by the G0 x? y? z? command\r\n"); + pc.printf("sizeof long long is %d bytes, pulsecnt at 1mm per min = %f, top speed = %d mm per min\r\n", sizeof(long long), n_for_onemmpermin, max_mm_per_min); + NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, interrupt_period_us);// Have setup timed interrupts, let other code deal + while(1) { +// if(!(ticks & 0x00000ff)) { +// mybigmotor = arr[step++]; +// step &= 0x03; +// pc.printf("^"); +// } + command_line_interpreter (); +// myled = 1; //wait(0.4);// myled = 0; //wait(0.4); +// if(running && dro_output && !(ticks & 0x00007ffc)) { // including 'running' causes display to freeze at almost there ! + if(dro.dro_output && !(ticks & 0x00007ffc)) { + pc.printf("dros X %s, Y ", readout(txt, dro.x)); // dro.n has running subtotal of all pulses issued to stepper driver.n + pc.printf("%s, Z ", readout(txt, dro.y)); + pc.printf("%s", readout(txt, dro.z)); + pc.printf(", ticks %d\r\n", ticks); + asepp++; + if (asepp >= axis_speeds + axis_speeds_buffsize) { + asepp = axis_speeds; + } +// pc.printf ("axis_speed %d, %lx\r\n", asepp - axis_speeds, (long)asep); + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jan 31 11:16:21 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/824293ae5e43 \ No newline at end of file