Jon Freeman / Mbed 2 deprecated cnc_machine_driver_3

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

Dependencies:   MODSERIAL mbed

command_interpreter.cpp@1:66ee619f206b, 2014-02-06 (annotated)

Committer:
JonFreeman
Date:
Thu Feb 06 08:45:02 2014 +0000
Revision:
1:66ee619f206b
Parent:
0:5d0f270bfc87
Child:
2:b3c668ec43ac
Currently creates 3 sets of Step and Dir signals for stepper motor drivers.  Accepts via putty etc, as yet minimal set of 'G Codes' for CNC - G0, G1, G2 and G3.  Still wip.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:5d0f270bfc87 1 #include "mbed.h"
JonFreeman 1:66ee619f206b 2 #include "rtos.h"
JonFreeman 0:5d0f270bfc87 3 #include "cnc.h"
JonFreeman 0:5d0f270bfc87 4 using namespace std;
JonFreeman 0:5d0f270bfc87 5
JonFreeman 0:5d0f270bfc87 6 extern Serial pc;
JonFreeman 1:66ee619f206b 7 extern bool liss_active;
JonFreeman 1:66ee619f206b 8 extern unsigned long pir_s;
JonFreeman 1:66ee619f206b 9 extern int spindlefwdrev;
JonFreeman 1:66ee619f206b 10 extern struct Gparams last_position;
JonFreeman 1:66ee619f206b 11 extern int PutMoveOnList (struct pirbufgrain & s) ;
JonFreeman 1:66ee619f206b 12 extern struct digital_readouts dro; //
JonFreeman 0:5d0f270bfc87 13
JonFreeman 1:66ee619f206b 14 double feed_rate = 1.0; // global scope, mm per minute. DEFAULTS to 1.0mm per min, very slow.
JonFreeman 0:5d0f270bfc87 15
JonFreeman 0:5d0f270bfc87 16 bool isdigit (int a)
JonFreeman 0:5d0f270bfc87 17 {
JonFreeman 0:5d0f270bfc87 18 if(a > ('0' - 1) && a < ('9' + 1))
JonFreeman 0:5d0f270bfc87 19 return true;
JonFreeman 0:5d0f270bfc87 20 return false;
JonFreeman 0:5d0f270bfc87 21 }
JonFreeman 0:5d0f270bfc87 22
JonFreeman 0:5d0f270bfc87 23 bool isupper (int a)
JonFreeman 0:5d0f270bfc87 24 {
JonFreeman 0:5d0f270bfc87 25 if ((a >= 'A') && (a <= 'Z')) return true;
JonFreeman 0:5d0f270bfc87 26 return false;
JonFreeman 0:5d0f270bfc87 27 }
JonFreeman 0:5d0f270bfc87 28
JonFreeman 0:5d0f270bfc87 29 int tolower (int a)
JonFreeman 0:5d0f270bfc87 30 {
JonFreeman 0:5d0f270bfc87 31 if (isupper(a))
JonFreeman 0:5d0f270bfc87 32 a += 'a' - 'A';
JonFreeman 0:5d0f270bfc87 33 return a;
JonFreeman 0:5d0f270bfc87 34 }
JonFreeman 0:5d0f270bfc87 35
JonFreeman 0:5d0f270bfc87 36
JonFreeman 0:5d0f270bfc87 37 const int goodcodes[] = {0,'a','b','c','i','j','l','r','x','y','z'}; // possible G Code options
JonFreeman 0:5d0f270bfc87 38 const int const_numofcodes = sizeof(goodcodes) / sizeof(int);
JonFreeman 0:5d0f270bfc87 39
JonFreeman 0:5d0f270bfc87 40 int find_char_in_goodcodes (int target) // Returns position of char in goodcodes[], 0 if not found.
JonFreeman 0:5d0f270bfc87 41 {
JonFreeman 0:5d0f270bfc87 42 for (int i = 1; i < const_numofcodes; i++)
JonFreeman 0:5d0f270bfc87 43 if (goodcodes[i] == target)
JonFreeman 0:5d0f270bfc87 44 return i;
JonFreeman 0:5d0f270bfc87 45 return 0;
JonFreeman 0:5d0f270bfc87 46 }
JonFreeman 0:5d0f270bfc87 47
JonFreeman 0:5d0f270bfc87 48 /*
JonFreeman 0:5d0f270bfc87 49 void get_codepositions (struct singleGparam * a, struct Gparams & p)
JonFreeman 0:5d0f270bfc87 50 Only call from "void g0g1cmdcore (struct singleGparam * a, double f_rate)"
JonFreeman 0:5d0f270bfc87 51 Purpose:
JonFreeman 0:5d0f270bfc87 52 G code line may have any number of valid axes or parameters entered in any order or position.
JonFreeman 0:5d0f270bfc87 53 This function detects any X,Y,Z,A,I,J,R entries in 'p' if present and copies values into their
JonFreeman 0:5d0f270bfc87 54 respective positions within singleGparam 'a', setting the 'changed' flag for each to true if found,
JonFreeman 0:5d0f270bfc87 55 false if not found
JonFreeman 0:5d0f270bfc87 56 struct Gparams { // Where possibly messy G code line gets ordered and sorted into
JonFreeman 0:5d0f270bfc87 57 struct singleGparam x, y, z, i, j, r, a, b, c, d; // After sorting, know where to find any X, Y etc values !
JonFreeman 0:5d0f270bfc87 58 } ;
JonFreeman 0:5d0f270bfc87 59 */
JonFreeman 0:5d0f270bfc87 60 void get_codepositions (struct singleGparam * source_array, struct Gparams & dest)
JonFreeman 0:5d0f270bfc87 61 {
JonFreeman 0:5d0f270bfc87 62 //const int goodcodes[] = {0,'a','b','c','i','j','l','r','x','y','z'}; // possible G Code options
JonFreeman 0:5d0f270bfc87 63 //const int const_numofcodes = sizeof(goodcodes) / sizeof(int);
JonFreeman 1:66ee619f206b 64 // source_array is the array filled by function 'void command_line_interpreter ()'.
JonFreeman 1:66ee619f206b 65 // It contains any parameters read from the command line :
JonFreeman 1:66ee619f206b 66 // source_array[i].c may contain 'x' or 'y' etc to tie this entry to one of the 'goodcodes' - or not
JonFreeman 0:5d0f270bfc87 67 int codecnt[const_numofcodes +1];
JonFreeman 0:5d0f270bfc87 68 int codepos[const_numofcodes +1];
JonFreeman 0:5d0f270bfc87 69 int j;
JonFreeman 0:5d0f270bfc87 70 for (j = 0; j < const_numofcodes; j++)
JonFreeman 0:5d0f270bfc87 71 codecnt[j] = codepos[j] = 0; // Zero all results
JonFreeman 0:5d0f270bfc87 72 for (int i = 1; i <= source_array[0].i; i++) { // for number of parameters passed to us here
JonFreeman 0:5d0f270bfc87 73 for(j = 0; j < const_numofcodes; j++) { // for a, for b, ... for x, then y, then z
JonFreeman 0:5d0f270bfc87 74 if (source_array[i].c == goodcodes[j]) {
JonFreeman 0:5d0f270bfc87 75 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
JonFreeman 0:5d0f270bfc87 76 codepos[j] = i; // Identifies the a[?] containing last incidence of goodcodes[j]
JonFreeman 0:5d0f270bfc87 77 }
JonFreeman 0:5d0f270bfc87 78 }
JonFreeman 0:5d0f270bfc87 79 }
JonFreeman 0:5d0f270bfc87 80 dest.x.changed = dest.y.changed = dest.z.changed = dest.a.changed = false;
JonFreeman 0:5d0f270bfc87 81 dest.i.changed = dest.j.changed = dest.r.changed = false;
JonFreeman 0:5d0f270bfc87 82 dest.x.dbl = last_position.x.dbl; // copy previous coordinates in case not re-specified
JonFreeman 1:66ee619f206b 83 dest.y.dbl = last_position.y.dbl;
JonFreeman 1:66ee619f206b 84 dest.z.dbl = last_position.z.dbl;
JonFreeman 1:66ee619f206b 85 dest.a.dbl = last_position.a.dbl;
JonFreeman 1:66ee619f206b 86 dest.i.dbl = last_position.i.dbl;
JonFreeman 0:5d0f270bfc87 87 dest.j.dbl = last_position.j.dbl; dest.r.dbl = last_position.r.dbl;
JonFreeman 0:5d0f270bfc87 88 j = codepos[find_char_in_goodcodes('a')];
JonFreeman 0:5d0f270bfc87 89 if (j) {
JonFreeman 0:5d0f270bfc87 90 dest.a.changed = true;
JonFreeman 0:5d0f270bfc87 91 dest.a.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 92 }
JonFreeman 0:5d0f270bfc87 93 j = codepos[find_char_in_goodcodes('x')];
JonFreeman 0:5d0f270bfc87 94 if (j) {
JonFreeman 0:5d0f270bfc87 95 dest.x.changed = true;
JonFreeman 0:5d0f270bfc87 96 dest.x.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 97 }
JonFreeman 0:5d0f270bfc87 98 j = codepos[find_char_in_goodcodes('y')];
JonFreeman 0:5d0f270bfc87 99 if (j) {
JonFreeman 0:5d0f270bfc87 100 dest.y.changed = true;
JonFreeman 0:5d0f270bfc87 101 dest.y.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 102 }
JonFreeman 0:5d0f270bfc87 103 j = codepos[find_char_in_goodcodes('z')];
JonFreeman 0:5d0f270bfc87 104 if (j) {
JonFreeman 0:5d0f270bfc87 105 dest.z.changed = true;
JonFreeman 0:5d0f270bfc87 106 dest.z.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 107 }
JonFreeman 0:5d0f270bfc87 108 j = codepos[find_char_in_goodcodes('i')];
JonFreeman 0:5d0f270bfc87 109 if (j) {
JonFreeman 0:5d0f270bfc87 110 dest.i.changed = true;
JonFreeman 0:5d0f270bfc87 111 dest.i.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 112 }
JonFreeman 0:5d0f270bfc87 113 j = codepos[find_char_in_goodcodes('j')];
JonFreeman 0:5d0f270bfc87 114 if (j) {
JonFreeman 0:5d0f270bfc87 115 dest.j.changed = true;
JonFreeman 0:5d0f270bfc87 116 dest.j.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 117 }
JonFreeman 0:5d0f270bfc87 118 j = codepos[find_char_in_goodcodes('r')];
JonFreeman 0:5d0f270bfc87 119 if (j) {
JonFreeman 0:5d0f270bfc87 120 dest.r.changed = true;
JonFreeman 0:5d0f270bfc87 121 dest.r.dbl = source_array[j].dbl;
JonFreeman 0:5d0f270bfc87 122 }
JonFreeman 0:5d0f270bfc87 123 }
JonFreeman 0:5d0f270bfc87 124
JonFreeman 1:66ee619f206b 125 const double duration_multiplier = 60000000.0 / interrupt_period_us;
JonFreeman 1:66ee619f206b 126
JonFreeman 1:66ee619f206b 127 void mover (struct pirbufgrain & ins) {
JonFreeman 1:66ee619f206b 128 struct pirbufgrain outs;
JonFreeman 1:66ee619f206b 129 double distx = ins.x - last_position.x.dbl,
JonFreeman 1:66ee619f206b 130 disty = ins.y - last_position.y.dbl,
JonFreeman 1:66ee619f206b 131 distz = ins.z - last_position.z.dbl,
JonFreeman 1:66ee619f206b 132 distT = sqrt ((distx * distx) + (disty * disty) + (distz * distz)), // 3D Pythag !
JonFreeman 1:66ee619f206b 133 temp = n_for_onemmpermin / distT;
JonFreeman 1:66ee619f206b 134 if (distT < 0.01) {
JonFreeman 1:66ee619f206b 135 pc.printf("Very small move %.4f, Ignoring!\r\n", distT);
JonFreeman 1:66ee619f206b 136 return;
JonFreeman 1:66ee619f206b 137 }
JonFreeman 1:66ee619f206b 138 last_position.x.dbl = ins.x; // Update global last_position record
JonFreeman 1:66ee619f206b 139 last_position.y.dbl = ins.y;
JonFreeman 1:66ee619f206b 140 last_position.z.dbl = ins.z;
JonFreeman 1:66ee619f206b 141 outs.f_rate = ins.f_rate;
JonFreeman 1:66ee619f206b 142 outs.c = duration_multiplier * distT; // Duration ticks subject to feed rate compo
JonFreeman 1:66ee619f206b 143 outs.x = temp * distx;
JonFreeman 1:66ee619f206b 144 outs.y = temp * disty;
JonFreeman 1:66ee619f206b 145 outs.z = temp * distz; // Have assembled data ready to put onto queue of move instructions
JonFreeman 1:66ee619f206b 146 PutMoveOnList (outs);
JonFreeman 1:66ee619f206b 147 }
JonFreeman 1:66ee619f206b 148
JonFreeman 1:66ee619f206b 149 void g2g3cmdcore (struct singleGparam * source_array, int twoorthree) {
JonFreeman 1:66ee619f206b 150 struct Gparams tmp;
JonFreeman 1:66ee619f206b 151 struct pirbufgrain start_point, end_point, centre_point, next_point;
JonFreeman 1:66ee619f206b 152 int state = 0, arc_steps;
JonFreeman 1:66ee619f206b 153 double rad_start, rad_end, start_angle, end_angle, next_angle, swept_angle, angle_step, arc_len, z_step;
JonFreeman 1:66ee619f206b 154 if (twoorthree != 2 && twoorthree != 3) {
JonFreeman 1:66ee619f206b 155 pc.printf("Err got %d when should be 2 or 3", twoorthree);
JonFreeman 1:66ee619f206b 156 return;
JonFreeman 1:66ee619f206b 157 }
JonFreeman 1:66ee619f206b 158 if (twoorthree == 2)
JonFreeman 1:66ee619f206b 159 pc.printf("g2 Clockwise Arc\r\n");
JonFreeman 1:66ee619f206b 160 else
JonFreeman 1:66ee619f206b 161 pc.printf("g3 CounterClockwise Arc\r\n");
JonFreeman 1:66ee619f206b 162 get_codepositions (source_array, tmp); // will overwrite with new where entered
JonFreeman 1:66ee619f206b 163 pc.printf("X %s\r\n", tmp.x.changed ? "T":"F");
JonFreeman 1:66ee619f206b 164 pc.printf("Y %s\r\n", tmp.y.changed ? "T":"F");
JonFreeman 1:66ee619f206b 165 pc.printf("Z %s\r\n", tmp.z.changed ? "T":"F");
JonFreeman 1:66ee619f206b 166 pc.printf("R %s\r\n", tmp.r.changed ? "T":"F");
JonFreeman 1:66ee619f206b 167 pc.printf("I %s\r\n", tmp.i.changed ? "T":"F");
JonFreeman 1:66ee619f206b 168 pc.printf("J %s\r\n", tmp.j.changed ? "T":"F");
JonFreeman 1:66ee619f206b 169 if (!tmp.x.changed || !tmp.y.changed) state |= 0x10000; // Error, X or Y missing
JonFreeman 1:66ee619f206b 170 if (tmp.r.changed && !tmp.i.changed && !tmp.j.changed) state |= 1; // Validated R mode got R not I not J
JonFreeman 1:66ee619f206b 171 if (!tmp.r.changed && tmp.i.changed && tmp.j.changed) state |= 2; // Validated IJ mode not R got I got J
JonFreeman 1:66ee619f206b 172 start_point.x = last_position.x.dbl;
JonFreeman 1:66ee619f206b 173 start_point.y = last_position.y.dbl;
JonFreeman 1:66ee619f206b 174 start_point.z = last_position.z.dbl;
JonFreeman 1:66ee619f206b 175 end_point.x = tmp.x.dbl;
JonFreeman 1:66ee619f206b 176 end_point.y = tmp.y.dbl;
JonFreeman 1:66ee619f206b 177 end_point.z = tmp.z.dbl;
JonFreeman 1:66ee619f206b 178 switch (state) {
JonFreeman 1:66ee619f206b 179 case 1: // Radius format arc
JonFreeman 1:66ee619f206b 180 pc.printf("Valid Radius format arc TO DO - not yet implemeted\r\n");
JonFreeman 1:66ee619f206b 181 break;
JonFreeman 1:66ee619f206b 182 case 2: // Centre format arc ** OFFSETS ARE RELATIVE ** Abs coordinates not catered for
JonFreeman 1:66ee619f206b 183 pc.printf("Valid Centre format arc\r\n");
JonFreeman 1:66ee619f206b 184 centre_point.x = start_point.x + tmp.i.dbl;
JonFreeman 1:66ee619f206b 185 centre_point.y = start_point.y + tmp.j.dbl;
JonFreeman 1:66ee619f206b 186 rad_start = hypot(start_point.x - centre_point.x, start_point.y - centre_point.y);
JonFreeman 1:66ee619f206b 187 rad_end = hypot(end_point.x - centre_point.x, end_point.y - centre_point.y);
JonFreeman 1:66ee619f206b 188 pc.printf("Start point X %.3f, Y %.3f\r\n", start_point.x, start_point.y);
JonFreeman 1:66ee619f206b 189 pc.printf("Centre point X %.3f, Y %.3f\r\n", centre_point.x, centre_point.y);
JonFreeman 1:66ee619f206b 190 pc.printf("End point X %.3f, Y %.3f\r\n", end_point.x, end_point.y);
JonFreeman 1:66ee619f206b 191 pc.printf("Rad start %.3f, Rad end %.3f\r\n", rad_start, rad_end);
JonFreeman 1:66ee619f206b 192 if (fabs(rad_start - rad_end) > 0.001) {
JonFreeman 1:66ee619f206b 193 // if ((rad_start - rad_end) > 0.001 || (rad_start - rad_end) < -0.001) {
JonFreeman 1:66ee619f206b 194 state |= 0x20000;
JonFreeman 1:66ee619f206b 195 pc.printf("Radii mismatch error in g2g3\r\n");
JonFreeman 1:66ee619f206b 196 }
JonFreeman 1:66ee619f206b 197 start_angle = atan2(start_point.y - centre_point.y, start_point.x - centre_point.x);
JonFreeman 1:66ee619f206b 198 end_angle = atan2(end_point.y - centre_point.y, end_point.x - centre_point.x);
JonFreeman 1:66ee619f206b 199 swept_angle = end_angle - start_angle;
JonFreeman 1:66ee619f206b 200 //swept_angle = 0.0; //=IF((B$8=2);IF((H$24>-0.0001);(H$24-2*PI());(H$24))
JonFreeman 1:66ee619f206b 201 // ;IF((H$24>0.0001);(H$24);(H$24+2*PI())))
JonFreeman 1:66ee619f206b 202 if (twoorthree == 2) {
JonFreeman 1:66ee619f206b 203 if (swept_angle > -epsilon)
JonFreeman 1:66ee619f206b 204 swept_angle -= TWO_PI;
JonFreeman 1:66ee619f206b 205 }
JonFreeman 1:66ee619f206b 206 else { // twoorthree is 3
JonFreeman 1:66ee619f206b 207 if (!(swept_angle > epsilon))
JonFreeman 1:66ee619f206b 208 swept_angle += TWO_PI;
JonFreeman 1:66ee619f206b 209 }
JonFreeman 1:66ee619f206b 210 arc_len = fabs(rad_start * swept_angle);
JonFreeman 1:66ee619f206b 211 pc.printf("start_angle %.3f, end_angle %.3f, swept_angle %.3f, arc_len %.3f\r\n", start_angle, end_angle, swept_angle, arc_len);
JonFreeman 1:66ee619f206b 212 arc_steps = (int)(4.0 + fabs(1.7 * rad_end * swept_angle)); // fiddle factors adjusted empirically !
JonFreeman 1:66ee619f206b 213 angle_step = swept_angle / arc_steps;
JonFreeman 1:66ee619f206b 214 next_angle = start_angle;
JonFreeman 1:66ee619f206b 215 z_step = (end_point.z - start_point.z) / arc_steps;
JonFreeman 1:66ee619f206b 216 next_point.z = start_point.z;
JonFreeman 1:66ee619f206b 217 pc.printf("Number of steps = %d, angle_step %.3f\r\n", arc_steps, angle_step);
JonFreeman 1:66ee619f206b 218 for (int i = 0; i < arc_steps; i++) { // cut 'arc_steps' straight lines
JonFreeman 1:66ee619f206b 219 next_angle += angle_step;
JonFreeman 1:66ee619f206b 220 next_point.x = centre_point.x + (rad_start * cos(next_angle));
JonFreeman 1:66ee619f206b 221 next_point.y = centre_point.y + (rad_start * sin(next_angle));
JonFreeman 1:66ee619f206b 222 next_point.z += z_step;
JonFreeman 1:66ee619f206b 223 pc.printf("X %.3f, Y %.3f\r\n", next_point.x, next_point.y);
JonFreeman 1:66ee619f206b 224 Thread::wait(300);
JonFreeman 1:66ee619f206b 225 }
JonFreeman 1:66ee619f206b 226 break; // end of case 2: // Centre format arc ** OFFSETS ARE RELATIVE ** Abs coordinates not catered for
JonFreeman 1:66ee619f206b 227 default: // Input error detected
JonFreeman 1:66ee619f206b 228 pc.printf("Input error detected in g2g3, code %x\r\n", state);
JonFreeman 1:66ee619f206b 229 break;
JonFreeman 1:66ee619f206b 230 } // end of switch(state)
JonFreeman 1:66ee619f206b 231 }
JonFreeman 0:5d0f270bfc87 232
JonFreeman 0:5d0f270bfc87 233 void g0g1cmdcore (struct singleGparam * source_array, double f_rate) // Updates any / all of x, y, z NCOs
JonFreeman 1:66ee619f206b 234 { // Only get here when some G0 or G1 input has been read. G0 or G1 determined by f_rate
JonFreeman 1:66ee619f206b 235 struct pirbufgrain ins, outs;
JonFreeman 1:66ee619f206b 236 double distx, disty, distz;//, outx, outy, outz, outtimefactor;
JonFreeman 1:66ee619f206b 237 struct Gparams tmp;
JonFreeman 1:66ee619f206b 238 get_codepositions (source_array, tmp); // will overwrite with new where entered
JonFreeman 1:66ee619f206b 239 if (!tmp.x.changed && !tmp.y.changed && !tmp.z.changed) {
JonFreeman 1:66ee619f206b 240 pc.printf("No change in X, Y or Z in G0/G1. Ignoring\r\n");
JonFreeman 1:66ee619f206b 241 return;
JonFreeman 1:66ee619f206b 242 }
JonFreeman 1:66ee619f206b 243 ins.x = tmp.x.dbl;
JonFreeman 1:66ee619f206b 244 ins.y = tmp.y.dbl;
JonFreeman 1:66ee619f206b 245 ins.z = tmp.z.dbl;
JonFreeman 1:66ee619f206b 246 ins.f_rate = f_rate;
JonFreeman 1:66ee619f206b 247 distx = ins.x - last_position.x.dbl; // All doubles
JonFreeman 1:66ee619f206b 248 disty = ins.y - last_position.y.dbl;
JonFreeman 1:66ee619f206b 249 distz = ins.z - last_position.z.dbl;
JonFreeman 1:66ee619f206b 250 double distT = sqrt ((distx * distx) + (disty * disty) + (distz * distz));
JonFreeman 1:66ee619f206b 251 if (distT < 0.01) {
JonFreeman 1:66ee619f206b 252 pc.printf("Very small move %.4f, Ignoring!\r\n", distT);
JonFreeman 1:66ee619f206b 253 return;
JonFreeman 1:66ee619f206b 254 }
JonFreeman 1:66ee619f206b 255 if (f_rate > feed_rate_max) {
JonFreeman 1:66ee619f206b 256 pc.printf("WARNING Stupid feed rate in G0/G1 of %f, setting to %f\r\n", f_rate, feed_rate_max);
JonFreeman 1:66ee619f206b 257 f_rate = feed_rate_max;
JonFreeman 1:66ee619f206b 258 }
JonFreeman 1:66ee619f206b 259 if (f_rate < 0.0) {
JonFreeman 1:66ee619f206b 260 pc.printf("Bonkers neg feed rate %f, setting to 0");
JonFreeman 1:66ee619f206b 261 f_rate = 0.0;
JonFreeman 1:66ee619f206b 262 }
JonFreeman 1:66ee619f206b 263 double temp = n_for_onemmpermin / distT;
JonFreeman 1:66ee619f206b 264 // pc.printf("\nG0--G1 Moving to X %4.4f, Y %4.4f, Z%4.4f, distT %f\r\n", px, py, pz, distT);
JonFreeman 1:66ee619f206b 265 // pc.printf("Moving From pos'n X %4.4f, Y %4.4f, Z%4.4f\r\n", last_position.x.dbl, last_position.y.dbl, last_position.z.dbl);
JonFreeman 1:66ee619f206b 266 // pc.printf("Distances to move X %4.4f, Y %4.4f, Z%4.4f\r\n", distx, disty, distz);
JonFreeman 1:66ee619f206b 267 // pc.printf("mm per sec req'd X %4.4f, Y %4.4f, Z%4.4f\r\n", xmmps, ymmps, zmmps);
JonFreeman 1:66ee619f206b 268 pc.printf("Total move distance %4.4f, at feed rate %.2f, run time SECs = %f\r\n", distT, f_rate, 60.0 * distT / f_rate);
JonFreeman 1:66ee619f206b 269 // pc.printf("This time maps to %f interrupt ticks\r\n", run_secs * 1000000.0 / interrupt_period_us);
JonFreeman 1:66ee619f206b 270 last_position.x.dbl = ins.x; // Update global last_position record
JonFreeman 1:66ee619f206b 271 last_position.y.dbl = ins.y;
JonFreeman 1:66ee619f206b 272 last_position.z.dbl = ins.z;
JonFreeman 1:66ee619f206b 273 outs.f_rate = ins.f_rate;
JonFreeman 1:66ee619f206b 274 outs.c = duration_multiplier * distT; // Duration ticks subject to feed rate compo
JonFreeman 1:66ee619f206b 275 outs.x = temp * distx;
JonFreeman 1:66ee619f206b 276 outs.y = temp * disty;
JonFreeman 1:66ee619f206b 277 outs.z = temp * distz;
JonFreeman 1:66ee619f206b 278 // pc.printf("Dists Scaled for angle, X%f, Y%f, Z%f\r\n", outs.x, outs.y, outs.z);
JonFreeman 1:66ee619f206b 279 // pc.printf("Last position X%f, Y%f, Z%f\r\n", last_position.x.dbl, last_position.y.dbl, last_position.z.dbl);
JonFreeman 1:66ee619f206b 280 // outs.a.dbl = 0.0; // not used axis
JonFreeman 1:66ee619f206b 281 PutMoveOnList (outs);
JonFreeman 0:5d0f270bfc87 282 }
JonFreeman 0:5d0f270bfc87 283
JonFreeman 0:5d0f270bfc87 284 void g0cmd (struct singleGparam * a) // Updates any / all of x, y, z NCOs
JonFreeman 0:5d0f270bfc87 285 {
JonFreeman 0:5d0f270bfc87 286 g0g1cmdcore (a, feed_rate_max); // Defined parameter in code
JonFreeman 0:5d0f270bfc87 287 }
JonFreeman 0:5d0f270bfc87 288
JonFreeman 0:5d0f270bfc87 289 void g1cmd (struct singleGparam * a) // Updates any / all of x, y, z NCOs
JonFreeman 0:5d0f270bfc87 290 {
JonFreeman 0:5d0f270bfc87 291 g0g1cmdcore (a, feed_rate); // Settable feed_rate
JonFreeman 0:5d0f270bfc87 292 }
JonFreeman 0:5d0f270bfc87 293
JonFreeman 1:66ee619f206b 294 void fcmd (struct singleGparam * a) { // Set Feed Rate command
JonFreeman 1:66ee619f206b 295 if (a[1].dbl < 0.0) {
JonFreeman 1:66ee619f206b 296 pc.printf("feed rate %f ? Setting to 0\r\n", a[1].dbl);
JonFreeman 1:66ee619f206b 297 a[1].dbl = 0.0;
JonFreeman 1:66ee619f206b 298 }
JonFreeman 1:66ee619f206b 299 if (a[1].dbl > feed_rate_max) {
JonFreeman 1:66ee619f206b 300 pc.printf ("Error, can't set feed rate to %f, max is %f, ", a[1].dbl, feed_rate_max);
JonFreeman 1:66ee619f206b 301 a[1].dbl = feed_rate_max;
JonFreeman 0:5d0f270bfc87 302 }
JonFreeman 0:5d0f270bfc87 303 pc.printf ("Setting feed_rate to %f\r\n", a[1].dbl);
JonFreeman 0:5d0f270bfc87 304 feed_rate = a[1].dbl;
JonFreeman 0:5d0f270bfc87 305 }
JonFreeman 0:5d0f270bfc87 306
JonFreeman 0:5d0f270bfc87 307 void sfcmd (struct singleGparam * a) {pc.printf("Spindle Fwd\r\n"); spindlefwdrev = 0;}
JonFreeman 0:5d0f270bfc87 308 void srcmd (struct singleGparam * a) {pc.printf("Spindle Rev\r\n"); spindlefwdrev = 4;}
JonFreeman 0:5d0f270bfc87 309 void stopcmd (struct singleGparam * a) {pc.printf("Stop ! er, not working yet\r\n");}
JonFreeman 0:5d0f270bfc87 310
JonFreeman 0:5d0f270bfc87 311 void scmd (struct singleGparam * a) {
JonFreeman 0:5d0f270bfc87 312 pc.printf("pir_s=0x%x\r\n", pir_s);
JonFreeman 1:66ee619f206b 313 if (a[1].dbl < 0.0 || a[1].dbl > spindle_max) {
JonFreeman 0:5d0f270bfc87 314 pc.printf ("Errror setting spindle RPM, can't set to %f, ignoring request\r\n", a[1].dbl);
JonFreeman 0:5d0f270bfc87 315 // return;
JonFreeman 0:5d0f270bfc87 316 }
JonFreeman 0:5d0f270bfc87 317 pc.printf ("Setting spindle RPM to %f\r\n", a[1].dbl);
JonFreeman 0:5d0f270bfc87 318 // feed_rate = a[1].d; // ****TO DO****
JonFreeman 0:5d0f270bfc87 319 pir_s = (unsigned long) (a[1].dbl * 4096);
JonFreeman 0:5d0f270bfc87 320 pc.printf("pir_s=0x%x\r\n", pir_s);
JonFreeman 0:5d0f270bfc87 321 }
JonFreeman 0:5d0f270bfc87 322
JonFreeman 0:5d0f270bfc87 323 //void stopcmd (struct grain * a) {pc.printf("Stop !\r\n");}
JonFreeman 0:5d0f270bfc87 324 void m1cmd (struct singleGparam * a) {pc.printf("m1 Optional Programme Stop\r\n");}
JonFreeman 0:5d0f270bfc87 325 void m3cmd (struct singleGparam * a) {pc.printf("m3 Rotate Spindle Clockwise\r\n");}
JonFreeman 0:5d0f270bfc87 326 void m4cmd (struct singleGparam * a) {pc.printf("m4 Rotate Spindle Counter Clockwise\r\n");}
JonFreeman 0:5d0f270bfc87 327 void m5cmd (struct singleGparam * a) {pc.printf("m5 Stop Spindle\r\n");}
JonFreeman 0:5d0f270bfc87 328 /*void m30cmd (struct singleGparam * a) {pc.printf("m30 Programme End and Rewind\r\n");}
JonFreeman 0:5d0f270bfc87 329 void m47cmd (struct singleGparam * a) {pc.printf("m47 Repeat Prog from First Line\r\n");}
JonFreeman 0:5d0f270bfc87 330 void m48cmd (struct singleGparam * a) {pc.printf("m48 Enable Speed and Feed Override\r\n");}
JonFreeman 0:5d0f270bfc87 331 void m49cmd (struct singleGparam * a) {pc.printf("m49 Disable Speed and Feed Override\r\n");}
JonFreeman 0:5d0f270bfc87 332 void m98cmd (struct singleGparam * a) {pc.printf("m98 Call Subroutine\r\n");}
JonFreeman 0:5d0f270bfc87 333 void m99cmd (struct singleGparam * a) {pc.printf("m99 Return from Subroutine\r\n");}
JonFreeman 0:5d0f270bfc87 334 void g10cmd (struct singleGparam * a) {pc.printf("g10 Coord System Origin Set\r\n");}
JonFreeman 0:5d0f270bfc87 335 void g17cmd (struct singleGparam * a) {pc.printf("g17 XY Plane Select\r\n");}
JonFreeman 0:5d0f270bfc87 336 void g20cmd (struct singleGparam * a) {pc.printf("g20 Inch\r\n");}
JonFreeman 0:5d0f270bfc87 337 void g21cmd (struct singleGparam * a) {pc.printf("g21 mm\r\n");}
JonFreeman 0:5d0f270bfc87 338
JonFreeman 0:5d0f270bfc87 339 void g40cmd (struct singleGparam * a) {pc.printf("g40 Cutter Compensation Off\r\n");}
JonFreeman 0:5d0f270bfc87 340 void g50cmd (struct singleGparam * a) {pc.printf("g50 Reset Scale Factors\r\n");}
JonFreeman 0:5d0f270bfc87 341 void g53cmd (struct singleGparam * a) {pc.printf("g53 Move in Absolute Coordinates\r\n");}
JonFreeman 0:5d0f270bfc87 342 void g90cmd (struct singleGparam * a) {pc.printf("g90 Absolute Distance Mode\r\n");}
JonFreeman 0:5d0f270bfc87 343 */
JonFreeman 1:66ee619f206b 344 void g2cmd (struct singleGparam * a) { // Clockwise arc
JonFreeman 1:66ee619f206b 345 g2g3cmdcore (a, 2);
JonFreeman 1:66ee619f206b 346 }
JonFreeman 1:66ee619f206b 347 void g3cmd (struct singleGparam * a) { // Counter clockwise arc
JonFreeman 1:66ee619f206b 348 g2g3cmdcore (a, 3);
JonFreeman 1:66ee619f206b 349 }
JonFreeman 0:5d0f270bfc87 350 void g4cmd (struct singleGparam * a) {pc.printf("g4 Dwell\r\n");}
JonFreeman 0:5d0f270bfc87 351 void g91p1cmd (struct singleGparam * a) {pc.printf("g91.1 \r\n");}
JonFreeman 0:5d0f270bfc87 352
JonFreeman 1:66ee619f206b 353 void tasktstone (void const * name)
JonFreeman 1:66ee619f206b 354 {
JonFreeman 1:66ee619f206b 355 static int i = 0;
JonFreeman 1:66ee619f206b 356 pc.printf("Arrived at tasktstone\r\n");
JonFreeman 1:66ee619f206b 357 while (true) {
JonFreeman 1:66ee619f206b 358 pc.printf("%s %d\r\n", name, i++);
JonFreeman 1:66ee619f206b 359 Thread::wait(9500);
JonFreeman 1:66ee619f206b 360 osThreadYield();
JonFreeman 1:66ee619f206b 361 }
JonFreeman 1:66ee619f206b 362 }
JonFreeman 1:66ee619f206b 363
JonFreeman 1:66ee619f206b 364 /*void tasktestcmd (struct singleGparam * a) {
JonFreeman 1:66ee619f206b 365 pc.printf("At tasktestcmd\r\n");
JonFreeman 1:66ee619f206b 366 Thread tasktest (tasktstone, (void *) "Bollocks");
JonFreeman 1:66ee619f206b 367 pc.printf("Leaving tasktestcmd\r\n");
JonFreeman 1:66ee619f206b 368 }
JonFreeman 1:66ee619f206b 369 */
JonFreeman 1:66ee619f206b 370 void lisscmd (struct singleGparam * a) {
JonFreeman 1:66ee619f206b 371 if(liss_active) {
JonFreeman 1:66ee619f206b 372 pc.printf("Can not add Lissajous, already running.\r\n");
JonFreeman 1:66ee619f206b 373 }
JonFreeman 1:66ee619f206b 374 else {
JonFreeman 1:66ee619f206b 375 pc.printf("Adding Lissajous task\r\n");
JonFreeman 1:66ee619f206b 376 liss_active = true;
JonFreeman 1:66ee619f206b 377 }
JonFreeman 1:66ee619f206b 378 }
JonFreeman 1:66ee619f206b 379
JonFreeman 0:5d0f270bfc87 380 void drooncmd (struct singleGparam * a)
JonFreeman 0:5d0f270bfc87 381 {
JonFreeman 0:5d0f270bfc87 382 dro.dro_output = true; // Enable continuous dro display update
JonFreeman 0:5d0f270bfc87 383 }
JonFreeman 0:5d0f270bfc87 384 void drooffcmd (struct singleGparam * a)
JonFreeman 0:5d0f270bfc87 385 {
JonFreeman 0:5d0f270bfc87 386 dro.dro_output = false; // Disable continuous dro display update
JonFreeman 0:5d0f270bfc87 387 }
JonFreeman 0:5d0f270bfc87 388
JonFreeman 0:5d0f270bfc87 389
JonFreeman 0:5d0f270bfc87 390 void g90p1cmd (struct singleGparam * a)
JonFreeman 0:5d0f270bfc87 391 {
JonFreeman 0:5d0f270bfc87 392 pc.printf ("Arrived at function fredcmd with %d parameters\r\n", a[0].i);
JonFreeman 0:5d0f270bfc87 393 for (int i = 1; i <= a[0].i; i++) {
JonFreeman 0:5d0f270bfc87 394 pc.printf ("*%c* ", a[i].c);
JonFreeman 0:5d0f270bfc87 395 pc.printf ("%d, ", a[i].i);
JonFreeman 0:5d0f270bfc87 396 pc.printf ("%f\r\n", a[i].dbl);
JonFreeman 0:5d0f270bfc87 397 }
JonFreeman 0:5d0f270bfc87 398 pc.printf (" endof param list\r\n");
JonFreeman 0:5d0f270bfc87 399 }
JonFreeman 0:5d0f270bfc87 400
JonFreeman 0:5d0f270bfc87 401 void menucmd (struct singleGparam * a);
JonFreeman 1:66ee619f206b 402 struct kb_command {
JonFreeman 0:5d0f270bfc87 403 const char * cmd_word; // points to text e.g. "menu"
JonFreeman 0:5d0f270bfc87 404 const char * explan;
JonFreeman 0:5d0f270bfc87 405 void (*f)(struct singleGparam *); // points to function
JonFreeman 1:66ee619f206b 406 } ;
JonFreeman 1:66ee619f206b 407
JonFreeman 1:66ee619f206b 408 struct kb_command const * command_list_ptr = NULL; // Pointer switched between 'input_syntax_check' and 'command_execute'
JonFreeman 1:66ee619f206b 409
JonFreeman 1:66ee619f206b 410 struct kb_command const input_syntax_check [] = {
JonFreeman 1:66ee619f206b 411 {"menu", "Lists available commands, same as ls", menucmd},
JonFreeman 1:66ee619f206b 412 {"ls", "Lists available commands, same as menu", menucmd}
JonFreeman 1:66ee619f206b 413 } ;
JonFreeman 1:66ee619f206b 414
JonFreeman 1:66ee619f206b 415 struct kb_command const command_execute[] = {
JonFreeman 1:66ee619f206b 416 {"menu", "Lists available commands, same as ls", menucmd},
JonFreeman 1:66ee619f206b 417 {"ls", "Lists available commands, same as menu", menucmd},
JonFreeman 0:5d0f270bfc87 418 {"stop", "To Stop the Machine !", stopcmd},
JonFreeman 0:5d0f270bfc87 419 {"sf", "Spindle Clockwise", sfcmd},
JonFreeman 0:5d0f270bfc87 420 {"sr", "Spindle Anticlockwise", srcmd},
JonFreeman 0:5d0f270bfc87 421 {"f ", "To set Feed Rate mm/min, e.g. f 25", fcmd},
JonFreeman 0:5d0f270bfc87 422 {"s ", "To set Spindle RPM, e.g. S 1250", scmd},
JonFreeman 1:66ee619f206b 423 {"g0", "Rapid move", g0cmd},
JonFreeman 0:5d0f270bfc87 424 /*{"m30", "Not Implemented", m30cmd},
JonFreeman 0:5d0f270bfc87 425 {"m47", "Not Implemented", m47cmd},
JonFreeman 0:5d0f270bfc87 426 {"m48", "Not Implemented", m48cmd},
JonFreeman 0:5d0f270bfc87 427 {"m49", "Not Implemented", m49cmd},
JonFreeman 0:5d0f270bfc87 428 {"m98", "Not Implemented", m98cmd},
JonFreeman 0:5d0f270bfc87 429 {"m99", "Not Implemented", m99cmd},
JonFreeman 0:5d0f270bfc87 430 {"m1", "Not Implemented", m1cmd},
JonFreeman 0:5d0f270bfc87 431 {"m3", "Not Implemented", m3cmd},
JonFreeman 0:5d0f270bfc87 432 {"m4", "Not Implemented", m4cmd},
JonFreeman 0:5d0f270bfc87 433 {"m5", "Not Implemented", m5cmd},
JonFreeman 0:5d0f270bfc87 434 {"g10", "Not Implemented", g10cmd},
JonFreeman 0:5d0f270bfc87 435 {"g17", "Not Implemented", g17cmd},
JonFreeman 0:5d0f270bfc87 436 {"g20", "Not Implemented", g20cmd},
JonFreeman 0:5d0f270bfc87 437 {"g21", "Not Implemented", g21cmd},
JonFreeman 0:5d0f270bfc87 438 {"g40", "Not Implemented", g40cmd},
JonFreeman 0:5d0f270bfc87 439 {"g50", "Not Implemented", g50cmd},
JonFreeman 0:5d0f270bfc87 440 {"g90.1", "Not Implemented", g90p1cmd},
JonFreeman 0:5d0f270bfc87 441 {"g91.1", "Not Implemented", g91p1cmd},
JonFreeman 0:5d0f270bfc87 442 {"g90", "Not Implemented", g90cmd},
JonFreeman 0:5d0f270bfc87 443 */
JonFreeman 1:66ee619f206b 444 {"g1", "Linear Interpolation - move straight at current feed rate", g1cmd},
JonFreeman 1:66ee619f206b 445 {"g2", "Helical Interpolation CW (Arc, circle)", g2cmd},
JonFreeman 1:66ee619f206b 446 {"g3", "Helical Interpolation CCW (Arc, circle)", g3cmd},
JonFreeman 1:66ee619f206b 447 {"liss", "Add Lissajous pattern generator", lisscmd},
JonFreeman 1:66ee619f206b 448 // {"ttest", "Add a task to prove we can, or not", tasktestcmd},
JonFreeman 0:5d0f270bfc87 449 {"dro on", "Turn dro readout on", drooncmd},
JonFreeman 0:5d0f270bfc87 450 {"dro off", "Turn dro readout off", drooffcmd}
JonFreeman 0:5d0f270bfc87 451 };
JonFreeman 1:66ee619f206b 452 //const int numof_menu_items = sizeof(kbc2) / sizeof(kb_command);
JonFreeman 1:66ee619f206b 453 //const int numof_menu_items_sc = sizeof(input_syntax_check) / sizeof(kb_command);
JonFreeman 1:66ee619f206b 454 //const int numof_menu_items_ce = sizeof(command_execute) / sizeof(kb_command);
JonFreeman 1:66ee619f206b 455 int numof_menu_items;
JonFreeman 0:5d0f270bfc87 456 void menucmd (struct singleGparam * a)
JonFreeman 0:5d0f270bfc87 457 {
JonFreeman 0:5d0f270bfc87 458 pc.printf("At menucmd function - listing commands:-\r\n");
JonFreeman 0:5d0f270bfc87 459 for(int i = 0; i < numof_menu_items; i++)
JonFreeman 1:66ee619f206b 460 pc.printf("[%s]\t\t%s\r\n", command_list_ptr[i].cmd_word, command_list_ptr[i].explan);
JonFreeman 0:5d0f270bfc87 461 pc.printf("End of List of Commands\r\n");
JonFreeman 0:5d0f270bfc87 462 }
JonFreeman 0:5d0f270bfc87 463
JonFreeman 0:5d0f270bfc87 464 bool isalpha (int c)
JonFreeman 0:5d0f270bfc87 465 {
JonFreeman 0:5d0f270bfc87 466 if ((c >= 'a') && (c <= 'z')) return true;
JonFreeman 0:5d0f270bfc87 467 if ((c >= 'A') && (c <= 'Z')) return true;
JonFreeman 0:5d0f270bfc87 468 return false;
JonFreeman 0:5d0f270bfc87 469 }
JonFreeman 0:5d0f270bfc87 470
JonFreeman 0:5d0f270bfc87 471 ////class CLI {
JonFreeman 0:5d0f270bfc87 472
JonFreeman 0:5d0f270bfc87 473 /*
JonFreeman 0:5d0f270bfc87 474 void command_line_interpreter ()
JonFreeman 0:5d0f270bfc87 475 Purpose:
JonFreeman 0:5d0f270bfc87 476
JonFreeman 0:5d0f270bfc87 477 */
JonFreeman 1:66ee619f206b 478 void command_line_interpreter (void const * name)
JonFreeman 0:5d0f270bfc87 479 {
JonFreeman 1:66ee619f206b 480 const int MAX_PARAMS = 10, MAX_CMD_LEN = 120;
JonFreeman 1:66ee619f206b 481 static char cmd_line[MAX_CMD_LEN + 4];
JonFreeman 1:66ee619f206b 482 static struct singleGparam params[MAX_PARAMS + 1];
JonFreeman 1:66ee619f206b 483 static int cl_index = 0, lastalpha = 0;
JonFreeman 1:66ee619f206b 484 pc.printf("Got to cli, Starting cli\r\n");
JonFreeman 1:66ee619f206b 485 if (true) {
JonFreeman 1:66ee619f206b 486 command_list_ptr = command_execute;
JonFreeman 1:66ee619f206b 487 numof_menu_items = sizeof(command_execute) / sizeof(kb_command);
JonFreeman 1:66ee619f206b 488 }
JonFreeman 1:66ee619f206b 489 else {
JonFreeman 1:66ee619f206b 490 command_list_ptr = input_syntax_check;
JonFreeman 1:66ee619f206b 491 numof_menu_items = sizeof(input_syntax_check) / sizeof(kb_command);
JonFreeman 1:66ee619f206b 492 }
JonFreeman 1:66ee619f206b 493 while (true) {
JonFreeman 1:66ee619f206b 494 while (pc.readable()) {
JonFreeman 1:66ee619f206b 495 int ch;
JonFreeman 1:66ee619f206b 496 if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
JonFreeman 1:66ee619f206b 497 pc.printf ("Keyboard Error!! Killing stupidly long command line");
JonFreeman 1:66ee619f206b 498 cl_index = 0;
JonFreeman 1:66ee619f206b 499 }
JonFreeman 1:66ee619f206b 500 ch = tolower(pc.getc());
JonFreeman 1:66ee619f206b 501 if(ch != '\r') // was this the 'Enter' key?
JonFreeman 1:66ee619f206b 502 cmd_line[cl_index++] = ch; // added char to command being assembled
JonFreeman 1:66ee619f206b 503 else { // key was CR, may or may not be command to lookup
JonFreeman 1:66ee619f206b 504 cmd_line[cl_index] = 0; // null terminate command string
JonFreeman 1:66ee619f206b 505 if(cl_index) { // If have got some chars to lookup
JonFreeman 1:66ee619f206b 506 int i, wrdlen;
JonFreeman 1:66ee619f206b 507 for (i = 0; i < numof_menu_items; i++) { // Look for input match in command list
JonFreeman 1:66ee619f206b 508 wrdlen = strlen(command_list_ptr[i].cmd_word);
JonFreeman 1:66ee619f206b 509 if(strncmp(command_list_ptr[i].cmd_word, cmd_line, wrdlen) == 0) { // If match found
JonFreeman 1:66ee619f206b 510 bool negflag = false;
JonFreeman 1:66ee619f206b 511 int state = 0, paramindex;
JonFreeman 1:66ee619f206b 512 double fracmul;
JonFreeman 1:66ee619f206b 513 // pc.printf("Found match for word [%s]\r\n", kbc[i].wrd);
JonFreeman 1:66ee619f206b 514 for(paramindex = 0; paramindex < MAX_PARAMS; paramindex++) {
JonFreeman 1:66ee619f206b 515 // Clear out whole set of old parameters ready for anything new on this line
JonFreeman 1:66ee619f206b 516 params[paramindex].i = 0; // for integer parameters
JonFreeman 1:66ee619f206b 517 params[paramindex].c = 0; // for last alpha char, helps tie 'X' to '-23.5' etc
JonFreeman 1:66ee619f206b 518 params[paramindex].dbl = 0.0; // for floating point parameters
JonFreeman 1:66ee619f206b 519 params[paramindex].ul = 0;
JonFreeman 1:66ee619f206b 520 params[paramindex].changed = false;
JonFreeman 1:66ee619f206b 521 }
JonFreeman 1:66ee619f206b 522 paramindex = 0;
JonFreeman 1:66ee619f206b 523 // read any parameters from command line here
JonFreeman 1:66ee619f206b 524 // Using parameters[0] as count of parameters to follow
JonFreeman 1:66ee619f206b 525 while (wrdlen <= cl_index) {
JonFreeman 1:66ee619f206b 526 ch = cmd_line[wrdlen++];
JonFreeman 1:66ee619f206b 527 if(isalpha(ch)) lastalpha = ch;
JonFreeman 1:66ee619f206b 528 if(ch == '-') negflag = true;
JonFreeman 1:66ee619f206b 529 if(ch == '+') negflag = false;
JonFreeman 1:66ee619f206b 530 switch (state) {
JonFreeman 1:66ee619f206b 531 case 0: // looking for start of a number string
JonFreeman 1:66ee619f206b 532 if(isdigit(ch)) { // found first digit of a number string
JonFreeman 1:66ee619f206b 533 paramindex++;
JonFreeman 1:66ee619f206b 534 if(paramindex > MAX_PARAMS) {
JonFreeman 1:66ee619f206b 535 wrdlen = cl_index; // exit condition
JonFreeman 1:66ee619f206b 536 pc.printf("WARNING - too many parameters, ignoring extra\r\n");
JonFreeman 1:66ee619f206b 537 } else {
JonFreeman 1:66ee619f206b 538 params[paramindex].i = ch - '0';
JonFreeman 1:66ee619f206b 539 params[paramindex].c = lastalpha;
JonFreeman 1:66ee619f206b 540 state = 1; // Found first digit char of number string
JonFreeman 1:66ee619f206b 541 }
JonFreeman 0:5d0f270bfc87 542 }
JonFreeman 1:66ee619f206b 543 break;
JonFreeman 1:66ee619f206b 544 case 1: // looking for end of a number string
JonFreeman 1:66ee619f206b 545 if(isdigit(ch)) { // accumulating integer from string
JonFreeman 1:66ee619f206b 546 params[paramindex].i *= 10;
JonFreeman 1:66ee619f206b 547 params[paramindex].i += ch - '0';
JonFreeman 1:66ee619f206b 548 } else { // found non-digit terminating number
JonFreeman 1:66ee619f206b 549 if (ch == '.') {
JonFreeman 1:66ee619f206b 550 state = 2;
JonFreeman 1:66ee619f206b 551 fracmul = 0.1;
JonFreeman 1:66ee619f206b 552 params[paramindex].dbl = (double)params[paramindex].i;
JonFreeman 1:66ee619f206b 553 } else {
JonFreeman 1:66ee619f206b 554 params[0].i++; // count of validated parameters
JonFreeman 1:66ee619f206b 555 state = 0; // Have read past last digit of number string
JonFreeman 1:66ee619f206b 556 if(negflag) {
JonFreeman 1:66ee619f206b 557 params[paramindex].i = -params[paramindex].i;
JonFreeman 1:66ee619f206b 558 negflag = false;
JonFreeman 1:66ee619f206b 559 }
JonFreeman 1:66ee619f206b 560 params[paramindex].dbl = (double)params[paramindex].i;
JonFreeman 1:66ee619f206b 561 }
JonFreeman 1:66ee619f206b 562 }
JonFreeman 1:66ee619f206b 563 break;
JonFreeman 1:66ee619f206b 564 case 2: // looking for fractional part of double
JonFreeman 1:66ee619f206b 565 if(isdigit(ch)) { // accumulating fractional part from string
JonFreeman 1:66ee619f206b 566 params[paramindex].dbl += (double)((ch - '0') * fracmul);
JonFreeman 1:66ee619f206b 567 fracmul /= 10.0;
JonFreeman 1:66ee619f206b 568 } else { // found non-digit terminating double precision number
JonFreeman 0:5d0f270bfc87 569 params[0].i++; // count of validated parameters
JonFreeman 0:5d0f270bfc87 570 state = 0; // Have read past last digit of number string
JonFreeman 0:5d0f270bfc87 571 if(negflag) {
JonFreeman 0:5d0f270bfc87 572 params[paramindex].i = -params[paramindex].i;
JonFreeman 1:66ee619f206b 573 params[paramindex].dbl = -params[paramindex].dbl;
JonFreeman 0:5d0f270bfc87 574 negflag = false;
JonFreeman 0:5d0f270bfc87 575 }
JonFreeman 0:5d0f270bfc87 576 }
JonFreeman 1:66ee619f206b 577 break;
JonFreeman 1:66ee619f206b 578 default:
JonFreeman 1:66ee619f206b 579 break;
JonFreeman 1:66ee619f206b 580 } // end of switch state
JonFreeman 1:66ee619f206b 581 } // end of while wrdlen < cl_index
JonFreeman 1:66ee619f206b 582 // pc.printf("Found match to [%s] with %d parameters\r\n", command_list_ptr[i].wrd, paramindex);
JonFreeman 1:66ee619f206b 583 command_list_ptr[i].f(params); // execute command
JonFreeman 1:66ee619f206b 584 i = numof_menu_items + 1; // to exit for loop
JonFreeman 1:66ee619f206b 585 }
JonFreeman 1:66ee619f206b 586 } // End of for numof_menu_items
JonFreeman 1:66ee619f206b 587 if(i == numof_menu_items)
JonFreeman 1:66ee619f206b 588 pc.printf("No Match Found for CMD [%s]\r\n", cmd_line);
JonFreeman 1:66ee619f206b 589 } // End of If have got some chars to lookup
JonFreeman 1:66ee619f206b 590 cl_index = lastalpha = 0;
JonFreeman 1:66ee619f206b 591 } // End of else key was CR, may or may not be command to lookup
JonFreeman 1:66ee619f206b 592 } // End of while (pc.readable())
JonFreeman 1:66ee619f206b 593 // pc.printf("cli yielding\r\n");
JonFreeman 1:66ee619f206b 594 osThreadYield(); // Using RTOS on this project
JonFreeman 1:66ee619f206b 595 }
JonFreeman 0:5d0f270bfc87 596 }
JonFreeman 0:5d0f270bfc87 597
JonFreeman 0:5d0f270bfc87 598 ////} cli;
JonFreeman 0:5d0f270bfc87 599