Jon Freeman
/
cnc_machine_driver_3
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 3:7aaf0072cc22, committed 2014-03-14
- Comitter:
- JonFreeman
- Date:
- Fri Mar 14 14:14:55 2014 +0000
- Parent:
- 2:b3c668ec43ac
- Commit message:
- CNC Machine driver, emulates PC LPT port, docs are wip
Changed in this revision
--- a/arith.cpp Thu Feb 20 09:27:18 2014 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,37 +0,0 @@
-#include "mbed.h"
-#include "cnc.h"
-using namespace std;
-
-const double ball_screw_pitch_mm = 4.0, // KX3 has 4mm ball screws
- motor_step_per_rev = 200.0, // KX3 has 200 step per rev steppers
- micro_steps = 10.0, // 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 = 32.0,//24.0, //16 is possible with Mbed LPC1768
- interrupt_freq_Hz = 1000000.0 / interrupt_period_us, // Serious limit when doing all in software, no real limit with FPGA
- max_pulse_freq_Hz = interrupt_freq_Hz / 3.25, // strictly 2.0, but allow a good margin
- max_mm_per_min = 60.0 * max_pulse_freq_Hz / pulses_per_mm,
-// n_for_onemmpermin = pulses_per_mm * interrupt_period_us * pow(2.0,32) / 60000000.0, // n pir to produce 1mm/min travel
- n_for_onemmpermin = pulses_per_mm * interrupt_period_us * pow(2.0,31) / 60000000.0, // n pir to produce 1mm/min travel
- // note power reduced from 32to 31 as interrupt handler now issued step pulse on both edges
- feed_rate_max = max_mm_per_min,
- spindle_factor = interrupt_period_us * 4280,
- spindle_max = 5000.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 more_setup () {
- pirs_clr2 (last_position);
-}
-
-
--- a/cnc.h Thu Feb 20 09:27:18 2014 +0000
+++ b/cnc.h Fri Mar 14 14:14:55 2014 +0000
@@ -1,20 +1,61 @@
-#define ESTOP 0x100 // bits used in input reading
+/**
+Programme "cnc__machine_driver_3"
+Author Jon Freeman
+
+Accepts subset of 'G Code' CNC machine instructions as input from USB serial
+Generates 'Step' and 'Direction' signals for X, Y and Z axes, signals suit
+standard stepper / servo drive systems.
+Also generates similar Step and Dir for spindle
+Intended to connect to 25 pin 'D' connector to emulate old PC 'LPT' port
+for easy swap with PC/Mach driven CNC systems.
+Tested driving Sieg KX3 CNC mill.
+
+Designed to compile and run on:
+ Mbed LPC1768
+ Freescale KL25Z
+ Freescale KL46Z
+*/
+using namespace std;
+
+typedef float fl_typ; //
+const signed long
+ motor_step_per_rev = 200, // KX3 has 200 step per rev steppers
+ micro_steps = 10, // Arc Eurotrade choice 2,4,5,8,10,16,20,25,32,40,50,64,100,125,128
+ interrupt_period_us = 25, //32, //24.0, //16 is possible with Mbed LPC1768
+ interrupt_freq_Hz = 1000000 / interrupt_period_us, // Serious limit when doing all in software, no real limit with FPGA
+ spindle_factor = interrupt_period_us * 4280, // Derived empirically
+ spindle_max = 5000; // Stated in KX3 spec
+
+const fl_typ ball_screw_pitch_mm = 4.0, // KX3 has 4mm pitch ball screws
+ pulses_per_mm = micro_steps * motor_step_per_rev / ball_screw_pitch_mm,
+ n_for_onemmpermin = pulses_per_mm * interrupt_period_us * pow(2.0,31) / 60000000.0; // n pir to produce 1mm/min travel
+
+const signed long
+ max_pulse_freq_Hz = interrupt_freq_Hz * 4 / 13, // / 3.25, // strictly 2.0, but allow a good margin
+ feed_rate_max = 60 * max_pulse_freq_Hz / pulses_per_mm; // units mm per minute
+//The output frequency F<sub>out</sub> = 'Kernel Speed (Hz)' * n / (2 to the power of 32)
+
+//#define Fourth_Axis
+//#define SPI_Enable
+#define I2C_Enable
+#define ESTOP 0x100 // bits used in input reading KX3 limit and EStop switches
#define XLIM 1
#define YLIM 2
#define ZLIM 4
#define UNKN 8
-const double TWO_PI = 8.0 * atan(1.0);
-const double epsilon = 1e-5;
+
+const fl_typ TWO_PI = 8.0 * atan(1.0);
+const fl_typ epsilon = 1e-5;
struct pirbufgrain {
- double x,
+ fl_typ x,
y,
z,
- c,
+ distance_code,
f_rate;
} ;
struct singleGparam { // Place to put all we know about 'x' or 'j' etc parameter from G Code line
- double dbl;
+ fl_typ flt;
unsigned long ul;
int i, c;
bool changed; // Flagged true when new value for this axis found in Gcode line, false otherwise
@@ -24,6 +65,3 @@
struct singleGparam x, y, z, i, j, r, a, b, c, d; // After sorting, know where to find any X, Y etc values !
} ;
-extern const double n_for_onemmpermin, feed_rate_max, feed_rate_min, spindle_min, spindle_max, spindle_factor;
-//extern const long pulses_per_mm, max_mm_per_min, interrupt_period_us;
-extern const double pulses_per_mm, max_mm_per_min, interrupt_period_us;
--- a/command_interpreter.cpp Thu Feb 20 09:27:18 2014 +0000
+++ b/command_interpreter.cpp Fri Mar 14 14:14:55 2014 +0000
@@ -2,40 +2,93 @@
#include "rtos.h"
#include "MODSERIAL.h"
#include "cnc.h"
-using namespace std;
+#include <cctype>
extern MODSERIAL pc;
-//extern Serial pc;
-//extern bool liss_active;
extern struct Gparams last_position;
extern void move_to_XYZ (struct pirbufgrain & ins) ;
-//extern struct digital_readouts dro; //
-//extern void scmd (struct singleGparam * a) ;
extern void flags_report_cmd (struct singleGparam * a) ;
extern void report_inputs () ;
-double feed_rate = 1.0, // global scope, mm per minute. DEFAULTS to 1.0mm per min, very slow.
- spindle_rpm = 0.0; // global scope
+fl_typ feed_rate = 1.0; // global scope, mm per minute. DEFAULTS to 1.0mm per min, very slow.
+signed long spindle_rpm = 0; // global scope
+
+
+
+
+#if defined I2C_Enable
+extern I2CSlave slave;//(PTE0, PTE1); on KL25
+
+int i2c_checksumchecker (char * buf, int len) {
+ int k, i = 0x01;
+ for (k = 0; k < len; k++)
+ i += buf[k];
+ i &= 0x0ff;
+ return i;
+}
-bool isdigit (int a)
-{
- if(a > ('0' - 1) && a < ('9' + 1))
- return true;
- return false;
+int i2c_checksumchecker (char * buf) {
+ return i2c_checksumchecker (buf, strlen(buf));
+}
+
+char * add_csum (char * buf, int len) { // Adds checksum to end of binary string of known length
+ int j;
+ char cs = 0;
+ for (j = 0; j < len; j++) {
+ cs += buf[j];
+ }
+ buf[len] = 0xff - cs;
+ buf[len + 1] = 0;
+ return buf;
+}
+
+char * add_csum (char * buf) { // Adds checksum to end of null terminated string
+ return add_csum (buf, strlen(buf));
}
-bool isupper (int a)
+void i2c_handler (void const * name)
{
- if ((a >= 'A') && (a <= 'Z')) return true;
- return false;
-}
+ const int i2buflen = 16;
+ int err = 0;
+ char buf[i2buflen];
+ char msg[20] = "Message 2snd\0";
+ add_csum(msg);
+ slave.address(0xc0);
+ err = slave.write(msg, strlen(msg) + 1); // Includes null char // returns 0 on success, nz otherwise
+ while (true) {
+ int i = slave.receive();
+ switch (i) {
+ case I2CSlave::NoData: // Happens most of the time NoData - the slave has not been addressed
+ osThreadYield(); // Using RTOS on this project
+ break;
+ case I2CSlave::ReadAddressed: // - the master has requested a read from this slave
+ err = slave.write(msg, strlen(msg) + 1); // Includes null char // returns 0 on success, nz otherwise
+ pc.printf("RdAddr'd ");
+ break;
+ case I2CSlave::WriteGeneral: // - the master is writing to all slave
+ err = slave.read(buf, i2buflen); // returns 0 on success, nz otherwise
+ pc.printf("i=%d, - the master is writing to all slave %s\r\n", i, buf);
+ break;
+ case I2CSlave::WriteAddressed: // - the master is writing to this slave
+ err = slave.read(buf, i2buflen); // returns 0 on success, nz otherwise
+ pc.printf("M wr-> [%s]", buf);
+ for (int z = 0; z < strlen(buf); z++)
+ pc.printf("%2x, ", buf[z]);
+ pc.printf("cs %2x\r\n", i2c_checksumchecker(buf));
+ break;
+ default:
+ pc.printf("Unknown I2C code %d\r\n");
+ break;
+ } // end of switch (i) upon result of slave.receive()
+ if (err) {
+ pc.printf("Err %d with i = %d\r\n", err, i);
+ err = 0;
+ }
+ memset (buf, 0, i2buflen); // Clear buffer
+ } // end of while (true)
+} // end of void i2c_handler (void const * name)
-int tolower (int a)
-{
- if (isupper(a))
- a += 'a' - 'A';
- return a;
-}
+#endif
const int goodcodes[] = {0,'a','b','c','i','j','l','r','x','y','z'}; // possible G Code options
@@ -83,46 +136,47 @@
}
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;
+ dest.x.flt = last_position.x.flt; // copy previous coordinates in case not re-specified
+ dest.y.flt = last_position.y.flt;
+ dest.z.flt = last_position.z.flt;
+ dest.a.flt = last_position.a.flt;
+ dest.i.flt = last_position.i.flt;
+ dest.j.flt = last_position.j.flt;
+ dest.r.flt = last_position.r.flt;
j = codepos[find_char_in_goodcodes('a')];
if (j) {
dest.a.changed = true;
- dest.a.dbl = source_array[j].dbl;
+ dest.a.flt = source_array[j].flt;
}
j = codepos[find_char_in_goodcodes('x')];
if (j) {
dest.x.changed = true;
- dest.x.dbl = source_array[j].dbl;
+ dest.x.flt = source_array[j].flt;
}
j = codepos[find_char_in_goodcodes('y')];
if (j) {
dest.y.changed = true;
- dest.y.dbl = source_array[j].dbl;
+ dest.y.flt = source_array[j].flt;
}
j = codepos[find_char_in_goodcodes('z')];
if (j) {
dest.z.changed = true;
- dest.z.dbl = source_array[j].dbl;
+ dest.z.flt = source_array[j].flt;
}
j = codepos[find_char_in_goodcodes('i')];
if (j) {
dest.i.changed = true;
- dest.i.dbl = source_array[j].dbl;
+ dest.i.flt = source_array[j].flt;
}
j = codepos[find_char_in_goodcodes('j')];
if (j) {
dest.j.changed = true;
- dest.j.dbl = source_array[j].dbl;
+ dest.j.flt = source_array[j].flt;
}
j = codepos[find_char_in_goodcodes('r')];
if (j) {
dest.r.changed = true;
- dest.r.dbl = source_array[j].dbl;
+ dest.r.flt = source_array[j].flt;
}
}
@@ -131,7 +185,7 @@
struct Gparams tmp;
struct pirbufgrain start_point, end_point, centre_point, next_point;
int state = 0, arc_steps;
- double rad_start, rad_end, start_angle, end_angle, next_angle, swept_angle, angle_step, arc_len, z_step;
+ fl_typ rad_start, rad_end, start_angle, end_angle, next_angle, swept_angle, angle_step, arc_len, z_step;
if (twoorthree != 2 && twoorthree != 3) {
pc.printf("Err got %d when should be 2 or 3", twoorthree);
return;
@@ -150,20 +204,20 @@
if (!tmp.x.changed || !tmp.y.changed) state |= 0x10000; // Error, X or Y missing
if (tmp.r.changed && !tmp.i.changed && !tmp.j.changed) state |= 1; // Validated R mode got R not I not J
if (!tmp.r.changed && tmp.i.changed && tmp.j.changed) state |= 2; // Validated IJ mode not R got I got J
- start_point.x = last_position.x.dbl;
- start_point.y = last_position.y.dbl;
- start_point.z = last_position.z.dbl;
- end_point.x = tmp.x.dbl;
- end_point.y = tmp.y.dbl;
- end_point.z = tmp.z.dbl;
+ start_point.x = last_position.x.flt;
+ start_point.y = last_position.y.flt;
+ start_point.z = last_position.z.flt;
+ end_point.x = tmp.x.flt;
+ end_point.y = tmp.y.flt;
+ end_point.z = tmp.z.flt;
switch (state) {
case 1: // Radius format arc
pc.printf("Valid Radius format arc TO DO - not yet implemeted\r\n");
break;
case 2: // Centre format arc ** OFFSETS ARE RELATIVE ** Abs coordinates not catered for
pc.printf("Valid Centre format arc\r\n");
- centre_point.x = start_point.x + tmp.i.dbl;
- centre_point.y = start_point.y + tmp.j.dbl;
+ centre_point.x = start_point.x + tmp.i.flt;
+ centre_point.y = start_point.y + tmp.j.flt;
rad_start = hypot(start_point.x - centre_point.x, start_point.y - centre_point.y);
rad_end = hypot(end_point.x - centre_point.x, end_point.y - centre_point.y);
pc.printf("Start point X %.3f, Y %.3f\r\n", start_point.x, start_point.y);
@@ -213,7 +267,7 @@
} // end of switch(state)
}
-void g0g1cmdcore (struct singleGparam * source_array, double f_rate) // Updates any / all of x, y, z NCOs
+void g0g1cmdcore (struct singleGparam * source_array, fl_typ f_rate) // Updates any / all of x, y, z NCOs
{ // Only get here when some G0 or G1 input has been read. G0 or G1 determined by f_rate
struct pirbufgrain ins;//, outs;
struct Gparams tmp;
@@ -222,9 +276,9 @@
pc.printf("No change in X, Y or Z in G0/G1. Ignoring\r\n");
return;
}
- ins.x = tmp.x.dbl;
- ins.y = tmp.y.dbl;
- ins.z = tmp.z.dbl;
+ ins.x = tmp.x.flt;
+ ins.y = tmp.y.flt;
+ ins.z = tmp.z.flt;
ins.f_rate = f_rate;
move_to_XYZ (ins);
}
@@ -248,54 +302,42 @@
}
void fcmd (struct singleGparam * a) { // Set Feed Rate command
- if (a[1].dbl < 0.0) {
- pc.printf("feed rate %.1f ? Setting to 0\r\n", a[1].dbl);
- a[1].dbl = 0.0;
+ if (a[1].flt < 0.0) {
+ pc.printf("feed rate %.1f ? Setting to 0\r\n", a[1].flt);
+ a[1].flt = 0.0;
}
- if (a[1].dbl > feed_rate_max) {
- pc.printf ("Error, can't set feed rate to %.1f, max is %.1f, ", a[1].dbl, feed_rate_max);
- a[1].dbl = feed_rate_max;
+ if (a[1].flt > feed_rate_max) {
+ pc.printf ("Error, can't set feed rate to %.1f, max is %.1f, ", a[1].flt, feed_rate_max);
+ a[1].flt = feed_rate_max;
}
- pc.printf ("Setting feed_rate to %.1f\r\n", a[1].dbl);
- feed_rate = a[1].dbl;
+ pc.printf ("Setting feed_rate to %.1f\r\n", a[1].flt);
+ feed_rate = a[1].flt;
}
-extern void spindle_control (double ss) ;
+extern void spindle_control (signed long ss) ;
extern bool spindle_running () ;
void M3cmd (struct singleGparam * a) { spindle_control (spindle_rpm); }
-void M5cmd (struct singleGparam * a) { spindle_control (0.0); }
+void M5cmd (struct singleGparam * a) { spindle_control (0); }
void scmd (struct singleGparam * a) {
- if (fabs(a[1].dbl) > spindle_max) {
- pc.printf ("Errror setting spindle RPM, can't set to %.0f, ignoring request\r\n", a[1].dbl);
+ if (fabs(a[1].flt) > spindle_max) {
+ pc.printf ("Errror setting spindle RPM, can't set to %.0f, ignoring request\r\n", a[1].flt);
return;
}
- pc.printf ("Setting spindle RPM to %.0f Can set Pos or Neg for fwd/rev\r\n", a[1].dbl);
- spindle_rpm = a[1].dbl;
+ pc.printf ("Setting spindle RPM to %.0f Can set Pos or Neg for fwd/rev\r\n", a[1].flt);
+ spindle_rpm = (signed long) a[1].flt;
if (spindle_running())
spindle_control (spindle_rpm);
-/*
-
- pir_spin = (signed long) (a[1].dbl * spindle_factor);
- t = ticks;
- while (t == ticks) {} // wait until just after next interrupt
- p = mysteppers;
- if (pir_spin & 0x80000000)
- p |= SDi;
- else
- p &= ~SDi;
- mysteppers = p;
-*/
-
+ pc.printf("Readback ss %d\r\n", spindle_rpm);
}
extern void target_cmd (struct singleGparam * a) ;
void stopcmd (struct singleGparam * a) {pc.printf("Stop ! er, not working yet\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 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");}
@@ -319,57 +361,12 @@
void report_ins_cmd (struct singleGparam * a) {
report_inputs();
}
-/*void tasktstone (void const * name)
-{
- static int i = 0;
- pc.printf("Arrived at tasktstone\r\n");
- while (true) {
- pc.printf("%s %d\r\n", name, i++);
- Thread::wait(9500);
- osThreadYield();
- }
-}*/
-
-/*void tasktestcmd (struct singleGparam * a) {
- pc.printf("At tasktestcmd\r\n");
- Thread tasktest (tasktstone, (void *) "Bollocks");
- pc.printf("Leaving tasktestcmd\r\n");
-}
-*/
-extern void lissajous (double) ;
+extern void lissajous (fl_typ) ;
void lisscmd (struct singleGparam * a) {
lissajous (feed_rate);
-/* if(liss_active) {
- pc.printf("Can not add Lissajous, already running.\r\n");
- }
- else {
- pc.printf("Adding Lissajous task\r\n");
- liss_active = true;
- }*/
}
-/*
-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
-}
-*/
-/*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"
@@ -393,26 +390,6 @@
{"m3", "Start Spindle at last 'S'", M3cmd},
{"m5", "Stop Spindle", M5cmd},
{"g0", "Rapid move", 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", "Linear Interpolation - move straight at current feed rate", g1cmd},
{"g2", "Helical Interpolation CW (Arc, circle)", g2cmd},
{"g3", "Helical Interpolation CCW (Arc, circle)", g3cmd},
@@ -420,13 +397,8 @@
{"flags", "Report System Flags", flags_report_cmd},
{"inputs", "Report State of Input bits", report_ins_cmd},
{"target", "Identify computer device", target_cmd},
-// {"ttest", "Add a task to prove we can, or not", tasktestcmd},
-// {"dro on", "Turn dro readout on", drooncmd},
-// {"dro off", "Turn dro readout off", drooffcmd}
};
-//const int numof_menu_items = sizeof(kbc2) / sizeof(kb_command);
-//const int numof_menu_items_sc = sizeof(input_syntax_check) / sizeof(kb_command);
-//const int numof_menu_items_ce = sizeof(command_execute) / sizeof(kb_command);
+
int numof_menu_items;
void menucmd (struct singleGparam * a)
{
@@ -436,21 +408,14 @@
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;
-}
-void nudger (int code) {
+void nudger (int code) { // Allows <Ctrl> chars to nudge machine axes
// Using <Ctrl>+ 'F', 'B' for Y, 'L', 'R' for X, 'U', 'D' for Z
// 6 2 12 18 21 4
-// struct Gparams dest;
struct pirbufgrain dest;
- dest.x = last_position.x.dbl;
- dest.y = last_position.y.dbl;
- dest.z = last_position.z.dbl;
+ dest.x = last_position.x.flt;
+ dest.y = last_position.y.flt;
+ dest.z = last_position.z.flt;
dest.f_rate = feed_rate;
switch (code) {
case 6: // 'F' move -Y
@@ -472,7 +437,6 @@
dest.z -= 0.1;
default:
break;
-// pc.printf("Nuffink to do in nudger\r\n");
} // end of switch
move_to_XYZ (dest);
}
@@ -484,13 +448,14 @@
Purpose:
*/
+
void command_line_interpreter (void const * name)
{
const int MAX_PARAMS = 10, MAX_CMD_LEN = 120;
static char cmd_line[MAX_CMD_LEN + 4];
static struct singleGparam params[MAX_PARAMS + 1];
static int cl_index = 0, lastalpha = 0;
-// pc.printf("Got to cli, Starting cli\r\n");
+static fl_typ fracmul;
if (true) {
command_list_ptr = command_execute;
numof_menu_items = sizeof(command_execute) / sizeof(kb_command);
@@ -525,13 +490,12 @@
if(strncmp(command_list_ptr[i].cmd_word, cmd_line, wrdlen) == 0) { // If match found
bool negflag = false;
int state = 0, paramindex;
- double fracmul;
// 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].flt = 0.0; // for floating point parameters
params[paramindex].ul = 0;
params[paramindex].changed = false;
}
@@ -565,7 +529,7 @@
if (ch == '.') {
state = 2;
fracmul = 0.1;
- params[paramindex].dbl = (double)params[paramindex].i;
+ params[paramindex].flt = (fl_typ)params[paramindex].i;
} else {
params[0].i++; // count of validated parameters
state = 0; // Have read past last digit of number string
@@ -573,20 +537,20 @@
params[paramindex].i = -params[paramindex].i;
negflag = false;
}
- params[paramindex].dbl = (double)params[paramindex].i;
+ params[paramindex].flt = (fl_typ)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);
+ params[paramindex].flt += (fl_typ)((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;
+ params[paramindex].flt = -params[paramindex].flt;
negflag = false;
}
}
@@ -607,7 +571,6 @@
cl_index = lastalpha = 0;
} // End of else key was CR, may or may not be command to lookup
} // End of while (pc.readable())
- // pc.printf("cli yielding\r\n");
osThreadYield(); // Using RTOS on this project
}
}
--- a/lissajous.cpp Thu Feb 20 09:27:18 2014 +0000
+++ b/lissajous.cpp Fri Mar 14 14:14:55 2014 +0000
@@ -16,8 +16,8 @@
*/
//bool liss_active = false; // global flag used to prevent running more than once at a time
-void lissajous (double feed_rate) {
- const double PI = 4.0 * atan(1.0), //3.142ish but more accurate
+void lissajous (fl_typ feed_rate) {
+ const fl_typ PI = 4.0 * atan(1.0), //3.142ish but more accurate
Deg2Rad = PI / 180.0, // degrees to radian conversion factor
MaxX = 40.0, // Plot size X to move +/- MaxX
MaxY = 25.0, // Plot size Y to move +/- MaxY
@@ -26,12 +26,12 @@
FreqRatio = 0.255;
const int StepsPerRevX = 100,
NumofXCycles = 16;
- const double AngleStepX = (2.0 * PI / StepsPerRevX),
+ const fl_typ AngleStepX = (2.0 * PI / StepsPerRevX),
AngleStepY = AngleStepX * FreqRatio;
//void lissajous (void const * arg_string) {
struct pirbufgrain Coords;
- double AngleX = StartAngDegX * Deg2Rad,
+ fl_typ AngleX = StartAngDegX * Deg2Rad,
AngleY = StartAngDegY * Deg2Rad;
// pc.printf("In lissajous func, Loading Lissajous\r\n");
--- a/main.cpp Thu Feb 20 09:27:18 2014 +0000
+++ b/main.cpp Fri Mar 14 14:14:55 2014 +0000
@@ -2,51 +2,62 @@
#include "rtos.h"
#include "MODSERIAL.h"
#include "cnc.h"
-using namespace std;
+extern void i2c_handler (void const *);
extern void command_line_interpreter (void const *) ;
-//extern void lissajous (void const *) ;
-extern double feed_rate, spindle_rpm;
-extern void more_setup () ;
-extern struct Gparams last_position;
+extern fl_typ feed_rate; // float type is 'float'
+extern signed long spindle_rpm;
const int BAUD = 38400;
-MODSERIAL pc(USBTX, USBRX); // tx, rx to pc
-//Serial pc(USBTX, USBRX); // tx, rx to pc
-Ticker NCO_gen; // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz)
+MODSERIAL pc(USBTX, USBRX); // tx, rx to pc via usb lead
+Ticker NCO_gen; // Ticker generating interrupts at "Kernel Speed", NCO updating frequency (about 40kHz)
+Ticker msec; // Ticker updating global millisecs counter
+
+bool running = false,
+ new_run_pending = false,
+ idle = false,
+ move_ended = false;
-bool running = false, new_run_pending = false, idle = false, move_ended = false;
-volatile unsigned long ticks = 0L; // 32 bit count of "interrupt_period_us" interrupts from time t=0
-unsigned long tickrun = 0L; // 32 bit effectively stores time in future when current movement to stop
-unsigned long ticks_next = 0L; // 32 bit effectively stores time in future when current movement to stop
-unsigned long millisecs = 0L; // 32 bit
-unsigned long pir_a = 0L, // Phase Increment Registers
- pir_x = 0L,
- pir_y = 0L,
- pir_z = 0L,
- pir_a_next = 0L, // Data for next move assembled here
- pir_x_next = 0L, // during a move.
- pir_y_next = 0L, // This way, next move can start immediately
- pir_z_next = 0L, // on end of current move - minimised jerking
- pir_spin = 0L; // Referenced only in command_interpreter as spindle speed setting
+volatile unsigned long ticks = 0L; // 32 bit count of "interrupt_period_us" interrupts from time t=0
+unsigned long tickrun = 0L, // 32 bit effectively stores time in future when current movement to stop
+ ticks_next = 0L, // 32 bit effectively stores time in future when current movement to stop
+ millisecs = 0L; // 32 bit
+signed long
+#if defined Fourth_Axis
+ pir_a_next = 0L, // Data for next move assembled here
+#endif
+ pir_x_next = 0L, // during a move.
+ pir_y_next = 0L, // This way, next move can start immediately
+ pir_z_next = 0L, // on end of current move - minimised jerking
+#if defined Fourth_Axis
+ inc_a_next = 1L,
+#endif
+ inc_x_next = 1L,
+ inc_y_next = 1L,
+ inc_z_next = 1L,
+ dir_bits_next = 0L,
+ pir_spin = 0L; // Referenced only in command_interpreter as spindle speed setting
+
+struct Gparams last_position; //
#if defined (TARGET_KL25Z)
- const char Target[] = "KL25Z";
- DigitalOut intled (PTE1); //J2p20
- DigitalOut charge_pumpD25pin1 (PTE0); //J2p18
+ const char Target[] = "KL25Z"; // Note need PTE0 (sda) and PTE1 (scl)
+ DigitalOut intled (PTD7); //(PTE1); //J2p19, was 20
+ DigitalOut charge_pumpD25pin1 (PTD6); //(PTE0); //J2p17, was 18
// InterruptIn D25pin10_EStop (PTE20); //j10p1 KL25 J10 is KL46 j4
DigitalIn D25pin10_EStop (PTE20); //j10p1 KL25 J10 is KL46 j4
DigitalIn D25pin11_XLim (PTE21); //j10p3
DigitalIn D25pin12_YLim (PTE22); //j10p5
DigitalIn D25pin13_ZLim (PTE23); //j10p7
DigitalIn D25pin15_unkn (PTE30); //j10p11
- //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)
+#if defined I2C_Enable
+ I2CSlave slave(PTE0, PTE1); // PTE0 sda, (yellow) PTE1 scl (blue)
+#endif
+#if defined SPI_Enable
+ SPISlave spidevice(PTD2, PTD3, PTD1, PTD0); // mosi, miso, sclk (uses p11, p12, p13 on mbed LPC1768)
+#endif
// 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 D25pin 2
PortBitXDi = 4, // Port bit num X Dir J1P07 D25pin 3
@@ -74,7 +85,12 @@
DigitalIn D25pin12_YLim (PTE22); // j4p5 checked
DigitalIn D25pin13_ZLim (PTE23); // j4p7 checked
DigitalIn D25pin15_unkn (PTE30); // j4p11 checked
+#if defined I2C_Enable
+ I2CSlave slave(p9, p10);
+#endif
+#if defined SPI_Enable
SPISlave spidevice(PTA16, PTA17, PTA15, PTA14); // mosi, miso, sclk, ssel (uses p11, p12, p13, p? on mbed LPC)
+#endif
// J2p13, J2p15, J2p11, J2p09
// Easy way to allocate port bits for
// output of stepper motor Step and DIR sigs
@@ -93,15 +109,20 @@
#if defined (TARGET_MBED_LPC1768)
const char Target[] = "MBED LPC1768";
DigitalOut intled(LED2); // Correct
- DigitalOut charge_pumpD25pin1 (p25); //J2p18 Following 5 inputs all wrong - TO DO sort which pins
+ DigitalOut charge_pumpD25pin1 (p25); //
// InterruptIn D25pin10_EStop (p26); //P2.0
DigitalIn D25pin10_EStop (p26); //P2.0
DigitalIn D25pin11_XLim (p24); //P2.2
DigitalIn D25pin12_YLim (p23); //P2.3
DigitalIn D25pin13_ZLim (p19); //P1.30
DigitalIn D25pin15_unkn (p20); //P1.31
+#if defined I2C_Enable
+ I2CSlave slave(p9, p10);
+#endif
+#if defined SPI_Enable
SPISlave spidevice(p5, p6, p7, p8);
- // Easy way to allocate port bits for *** N O T CHECKED for MBED_LPC1768 ***
+#endif
+ // Easy way to allocate port bits
// output of stepper motor Step and DIR sigs
#define STEPPER_PORT Port0
/* Port 0 bits routed to DIP pins as follows:-
@@ -133,29 +154,37 @@
#endif
const long // Assemble mask bits from now known port bit positions
- XSt = 1 << PortBitXSt, // X axis Step signal
- XDi = 1 << PortBitXDi, // X axis Direction signal
- YSt = 1 << PortBitYSt, // Y axis Step, etc
- YDi = 1 << PortBitYDi,
- ZSt = 1 << PortBitZSt, // Z axis
- ZDi = 1 << PortBitZDi,
- ASt = 1 << PortBitASt, // A axis, not implemented in full, for e.g. rotary axis
- ADi = 1 << PortBitADi,
- SDi = 1 << PortBitSDi, // Spindle, also driven by Step and Dir signals up to 5kHz
- SSt = 1 << PortBitSSt, // for 5000 RPM
+ XSt = 1 << PortBitXSt, // X axis Step signal
+ XDi = 1 << PortBitXDi, // X axis Direction signal
+ YSt = 1 << PortBitYSt, // Y axis Step, etc
+ YDi = 1 << PortBitYDi,
+ ZSt = 1 << PortBitZSt, // Z axis
+ ZDi = 1 << PortBitZDi,
+ ASt = 1 << PortBitASt, // A axis, not implemented in full, for e.g. rotary axis
+ ADi = 1 << PortBitADi,
+ SDi = 1 << PortBitSDi, // Spindle, also driven by Step and Dir signals up to 5kHz
+ SSt = 1 << PortBitSSt, // for 5000 RPM
- SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt);
+ SM_MASK = (XSt | XDi | YSt | YDi | ZSt | ZDi | ASt | ADi | SDi | SSt),
+ direction_swappers = XDi | YDi | ZDi | SDi; // include bit to swap direction
PortOut Steppers (STEPPER_PORT, SM_MASK);
- const long direction_swappers = XDi | YDi | ZDi | SDi; // include bit to swap direction
-/*
- long read () {
- return mysteppers ^ direction_swappers;
- }
- void write (long val) {
- mysteppers = val ^ direction_swappers;
- }
- */
+
+void target_cmd (struct singleGparam * a) {
+ pc.printf("Computer is %s\r\n", Target);
+}
+
+void grain_clr (struct singleGparam & g) {
+ g.flt = 0.0;
+ g.ul = 0L;
+ g.i = g.c = 0;
+ g.changed = false;
+}
+void Gparams_clr (struct Gparams & p) {
+ grain_clr (p.x); grain_clr (p.y); grain_clr (p.z); grain_clr (p.i); grain_clr (p.j);
+ grain_clr (p.r); grain_clr (p.a); grain_clr (p.b); grain_clr (p.c); grain_clr (p.d);
+}
+
class digital_readout_stuff { // class does not need to be named here
private:
char * readout (char * txt, long p) // p has running subtotal of all pulses issued to stepper driver
@@ -184,16 +213,17 @@
}
public:
signed long x, y, z, a; // Could easily expand up to six or more dros
- bool dro_output; // To enabe / disable output to terminal
+// bool dro_output; // To enabe / disable output to terminal
void init () {
x = y = z = a = 0; // These dro registers count pulses delivered to stepper motor driver
- dro_output = true;
+// dro_output = true;
}
void update () {
static long t = 300; // Prevent display immediately upon startup
if (millisecs < t)
return;
- if(!idle && dro_output) {
+// if(!idle && dro_output) {
+ if(!idle) {
char txt[12];
pc.printf("dros X %s,", readout(txt, x)); // dro.n has running subtotal of all pulses issued to stepper driver.n
pc.printf(" Y %s, Z ", readout(txt, y));
@@ -202,8 +232,17 @@
t = millisecs + 350; // Schedule next update after this non-blocking delay
}
}
-} dro_out ;
+} dro_out ; // single instance of class digital_readout_stuff
+/**
+class circbuff { public functions
+ void init () {
+ int On_Q () {
+ bool readable () {return !buffempty; }
+ bool writeable () {return !bufffull; }
+ bool read (pirbufgrain & g) {
+ bool write (pirbufgrain & g) {
+*/
const int PIRBUFFSIZE = 40; // pirbufgrain are 40 bytes each
class circbuff {
private:
@@ -216,10 +255,10 @@
buffempty = true;
}
void grain_copy (pirbufgrain & src, pirbufgrain & dest) {
- dest.x = src.x;
- dest.y = src.y;
- dest.z = src.z;
- dest.c = src.c;
+ dest.x = src.x;
+ dest.y = src.y;
+ dest.z = src.z;
+ dest.distance_code = src.distance_code;
dest.f_rate = src.f_rate; // int feed rate mm per min * 1000
}
public:
@@ -261,167 +300,79 @@
bufffull = true;
return true;
}
-} CircBuff;
+} CircBuff; // single instance of class circbuff
-int PutMoveOnList (struct pirbufgrain & s) {
- while (!CircBuff.writeable())
- osThreadYield();
- CircBuff.write (s); // pc.printf("CircBuff, contains %d\r\n", CircBuff.On_Q());
- return 0;
-}
-const double duration_multiplier = 60000000.0 / interrupt_period_us;
-
+/**
void move_to_XYZ (struct pirbufgrain & ins) {
+ Takes floating point x, y, z and feed_rate as input.
+ Finds distances from 'last_position' global,
+ copies structure containing floating point values for x, y, z, distance_multiplier and feed_rate
+ onto a circular buffer.
+ If buffer full, executes 'osThreadYield()' until space is created on buffer
+*/
+void move_to_XYZ (struct pirbufgrain & ins) {
+static const fl_typ duration_multiplier = 60000000.0 / interrupt_period_us;
struct pirbufgrain outs;
- double distx = ins.x - last_position.x.dbl,
- disty = ins.y - last_position.y.dbl,
- distz = ins.z - last_position.z.dbl,
+ fl_typ distx = ins.x - last_position.x.flt,
+ disty = ins.y - last_position.y.flt,
+ distz = ins.z - last_position.z.flt,
distT = sqrt ((distx * distx) + (disty * disty) + (distz * distz)), // 3D Pythag !
temp = n_for_onemmpermin / distT;
if (distT < 0.01) {
pc.printf("Very small move %.4f, Ignoring!\r\n", distT);
- return;
+ return; // Return without updating last_position as it was not changed
}
- last_position.x.dbl = ins.x; // Update global last_position record
- last_position.y.dbl = ins.y;
- last_position.z.dbl = ins.z;
+ last_position.x.flt = ins.x; // Update global last_position record
+ last_position.y.flt = ins.y;
+ last_position.z.flt = ins.z;
outs.f_rate = ins.f_rate;
- outs.c = duration_multiplier * distT; // Duration ticks subject to feed rate compo
+ outs.distance_code = duration_multiplier * distT; // Duration ticks subject to feed rate compo
outs.x = temp * distx;
outs.y = temp * disty;
outs.z = temp * distz; // Have assembled data ready to put onto queue of move instructions
- PutMoveOnList (outs);
+ while (!CircBuff.writeable())
+ osThreadYield();
+ CircBuff.write (outs); // Move details put on circular buffer
+}
+
+/**
+* Interrupt Service Routines
+void millisec_update_ISR () { self explanatory
+
+void Numerically_Controlled_Oscillators_ISR () {
+ services Ticker 'NCO_gen' generated interrupts ***ISR***
+ Does all of the stepper motor driving.
+ At end of movement, fetches and starts next move to run from circular buffer
+ If nothing buffered, stops x, y and z, leaves spindle unaltered
+*/
+void millisec_update_ISR () {
+ millisecs++;
}
-void target_cmd (struct singleGparam * a) {
- pc.printf("Computer is %s\r\n", Target);
-}
-/*
-* Interrupt Service Routine
-*/
-/* **** UNBROKEN VERSION 6th Feb 2014 ******************
+#define STEP_IDLE_HI // Choose IDLE_HI or LO to suit any power save function of stepper motor drive units
+//#define STEP_IDLE_LO
void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR***
- static const int millisec_countdown = 1000 / interrupt_period_us;
- 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, mscount = millisec_countdown;
- int oldbitz, acts;
+ static const long step_mask = ASt | XSt | YSt | ZSt, // Added 6th Feb 14 Mask Does NOT include spindle bits
+ dir_mask = ADi | XDi | YDi | ZDi; // Added 6th Feb 14 Mask Does NOT include spindle bits
+ static signed long // 27 Feb 14 changed from unsigned
+#if defined Fourth_Axis
+ acc_a = 0L, pir_a = 0L,
+#endif
+ acc_x = 0L, // acc Accumuloators
+ pir_x = 0L, // pir Phase Increment Registers
+ acc_y = 0L, pir_y = 0L,
+ acc_z = 0L, pir_z = 0L,
+ acc_spin = 0L, // separate acc for spindle rotation NCO
+ inc_x = 1L, // inc_x, y, z for updating DRO registers
+ inc_y = 1L, inc_z = 1L,
+ dir_bits = 0L, // direction flags for up to four axes
+ oldSteps = 0L; //
+ long tmp, newSteps = 0L;
intled = 1; // LED on for duration of interrupt service - point for scope probing
- ticks++; // count of interrupts serviced
- if(!--mscount) { // Maintain global counter of elapsed milli seconds
- mscount = millisec_countdown;
- millisecs++;
- }
- if (running) {
- acc_x += pir_x; // Update phase of signals in accumulators
- acc_y += pir_y;
- acc_z += pir_z;
- acc_a += pir_a; // not yet implemented
- // 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 'Step' outputs
-
- acts = (~oldbitz & obitz); // get pos clk edge triggers 'Step' bits
- acts |= (obitz & (XDi1 | YDi1 | ZDi1)); // get axis X, Y and Z Direction bits
- 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 (tickrun <= ticks) { // End of a machine movement detected, start next move here if possible
- running = false;
- move_ended = true;
- pir_x = 0L; // stop all stepper motors
- pir_y = 0L;
- pir_z = 0L;
- pir_a = 0L;
-// ticks = 0L; // Simply to avoid having to think about overflow problems
- }
- }
- else { // Not running. Grab next data here when or if available
- if (new_run_pending) { // Pick up on flag set elsewhere
- pir_a = pir_a_next;
- pir_x = pir_x_next;
- pir_y = pir_y_next;
- pir_z = pir_z_next;
- tickrun = ticks + ticks_next;
- running = true; // Start the new run
- new_run_pending = false; // Clear the flag which initiated this update
- idle = false;
- }
- }
- charge_pump = ticks & 0x02;
- intled = 0; // LED off
-} // end of interrupt handler
-*/
-/*
-* End of Interrupt Service Routine
-*/
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-/*
-* Interrupt Service Routine
-*/
-#define STEP_IDLE_HI
-//#define STEP_IDLE_LO
-void Numerically_Controlled_Oscillators_ISR () { // services Ticker 'NCO_gen' generated interrupts ***ISR***
- static const int millisec_countdown = 1000 / interrupt_period_us;
- const long step_mask = ASt | XSt | YSt | ZSt, // Added 6th Feb 14 Mask Does NOT include spindle bits
- dir_mask = ADi | XDi | YDi | ZDi; // Added 6th Feb 14 Mask Does NOT include spindle bits
- static unsigned long
- acc_spin = 0L,
- acc_a = 0L,
- acc_x = 0L,
- acc_y = 0L,
- acc_z = 0L;
- static long mscount = millisec_countdown;
- static long dir_bits = 0L, oldSteps = 0L; // Added 6th Feb 14
- long acts, tmp, newSteps, stbits;
-
- intled = 1; // LED on for duration of interrupt service - point for scope probing
- ticks++; // count of interrupts serviced
- if(!--mscount) { // Maintain global counter of elapsed milli seconds
- mscount = millisec_countdown;
- millisecs++;
- }
- acc_spin += pir_spin;
+ ticks++; // count of interrupts serviced, vital to time end of movement
+ charge_pumpD25pin1 = ticks & 0x01; // Can use 0x01 or 0x02 here to alter charge pump freq
tmp = Steppers ^ direction_swappers;
#if defined STEP_IDLE_LO
tmp &= ~step_mask; // Step bits prepared for idle lo
@@ -429,96 +380,62 @@
#if defined STEP_IDLE_HI
tmp |= step_mask; // Step bits prepared for idle hi
#endif
- if (acc_spin & 0x80000000) { tmp |= SSt; }
- else { tmp &= ~SSt; }
-// mysteppers = tmp;
- if (running) {
- acc_x += pir_x; // Update phase of signals in accumulators
- acc_y += pir_y;
- acc_z += pir_z;
- acc_a += pir_a; // not yet implemented
-
- newSteps = 0L; // Added 6th Feb 14
- if (acc_a & 0x80000000) newSteps |= ASt;// Added 6th Feb 14
- if (acc_x & 0x80000000) newSteps |= XSt;// Added 6th Feb 14
- if (acc_y & 0x80000000) newSteps |= YSt;// Added 6th Feb 14
- if (acc_z & 0x80000000) newSteps |= ZSt;// Added 6th Feb 14
- stbits = newSteps ^ oldSteps; // Any bit of stbits set to initiate a Step pulse
- acts = dir_bits | stbits; //
- oldSteps = newSteps; // Added 6th Feb 14
-// tmp = acts ^ step_mask; // Invert clock - Arc Euro stepp motor driver only goes into half current mode this way
- tmp ^= stbits;
+ acc_spin += pir_spin; // Spindle NCO
+ if (acc_spin < 0) tmp |= SSt;
+ else tmp &= ~SSt;
+ if (!running) Steppers = tmp ^ direction_swappers; // Axes not moving, spindle may be turning or not
+ else { // running == true, Further manipulation of tmp follows, prior to rewriting to 'Steppers' IO Port
+// newSteps = 0L; // Added 6th Feb 14
+#if defined Fourth_Axis
+ acc_a += pir_a;
+ if (acc_a < 0) newSteps |= ASt;// Added 6th Feb 14
+#endif
+ acc_x += pir_x; // Update phase of signals in accumulators
+ if (acc_x < 0) newSteps |= XSt;// Added 6th Feb 14
+ acc_y += pir_y;
+ if (acc_y < 0) newSteps |= YSt;// Added 6th Feb 14
+ acc_z += pir_z;
+ if (acc_z < 0) newSteps |= ZSt;// Added 6th Feb 14
+ // newSteps has copy of all 4 'acc' MSBs shifted into port bit positions
+ oldSteps ^= newSteps; // Any bit of stbits set to initiate a Step pulse
+ tmp ^= oldSteps;
Steppers = tmp ^ direction_swappers; // Output signals to stepper motor drivers, next update dros from 'clocked' bits CLOCK IDLES HIGH
-
- if(acts & XSt) { // got clk edge for axis X
- if (acts & XDi)
- dro_out.x--;
- else dro_out.x++;
- }
- if(acts & YSt) { // got clk edge for axis Y
- if (acts & YDi)
- dro_out.y--;
- else dro_out.y++;
- }
- if(acts & ZSt) { // got clk edge for axis Z
- if (acts & ZDi)
- dro_out.z--;
- else dro_out.z++;
- }
- if (tickrun <= ticks) { // End of a machine movement detected, start next move here if possible
- if (new_run_pending) {
- pir_a = pir_a_next;
- pir_x = pir_x_next;
- pir_y = pir_y_next;
- pir_z = pir_z_next;
- dir_bits = 0; // Added 6th Feb 14
- if (pir_a & 0x80000000) dir_bits |= ADi;// Added 6th Feb 14 read sign bits
- if (pir_x & 0x80000000) dir_bits |= XDi;// Added 6th Feb 14
- if (pir_y & 0x80000000) dir_bits |= YDi;// Added 6th Feb 14
- if (pir_z & 0x80000000) dir_bits |= ZDi;// Added 6th Feb 14
- acts = Steppers ^ direction_swappers; // read output lines
- acts &= ~dir_mask;
- acts |= dir_bits;
- Steppers = acts ^ direction_swappers;
- tickrun = ticks + ticks_next;
- running = true; // Start the new run
- new_run_pending = false; // Clear the flag which initiated this update
- idle = false;
- } // End of if (new_run_pending) {
- else { // End of machine movement AND no new_run_pending
+ if(oldSteps & XSt) dro_out.x += inc_x; // got clk edge for axis X
+ if(oldSteps & YSt) dro_out.y += inc_y; // got clk edge for axis Y
+ if(oldSteps & ZSt) dro_out.z += inc_z; // got clk edge for axis Z
+ oldSteps = newSteps; // Added 6th Feb 14
+ if (tickrun <= ticks & !new_run_pending) { // End of a machine movement detected, start next move here if possible
running = false;
move_ended = true;
pir_x = 0L; // stop all stepper motors
pir_y = 0L;
pir_z = 0L;
+#if defined Fourth_Axis
pir_a = 0L;
+#endif
// ticks = 0L; // Simply to avoid having to think about overflow problems
- }
- }
- }
- else { // Not running. Grab next data here when or if available
+ } // end of if (tickrun <= ticks) {
+ } // end of else is (running) {
+ if (!running & new_run_pending) { // Start axis movement
+ dir_bits= dir_bits_next;
+#if defined Fourth_Axis
+ pir_a = pir_a_next;
+#endif
+ pir_x = pir_x_next;
+ pir_y = pir_y_next;
+ pir_z = pir_z_next;
+ inc_x = inc_x_next;
+ inc_y = inc_y_next;
+ inc_z = inc_z_next;
+ tmp = Steppers ^ direction_swappers; // read output lines
+ tmp &= ~dir_mask;
+ tmp |= dir_bits;
Steppers = tmp ^ direction_swappers;
- if (new_run_pending) { // Pick up on flag set elsewhere
- pir_a = pir_a_next;
- pir_x = pir_x_next;
- pir_y = pir_y_next;
- pir_z = pir_z_next;
- dir_bits = 0; // Added 6th Feb 14
- if (pir_a & 0x80000000) dir_bits |= ADi;// Added 6th Feb 14 read sign bits
- if (pir_x & 0x80000000) dir_bits |= XDi;// Added 6th Feb 14
- if (pir_y & 0x80000000) dir_bits |= YDi;// Added 6th Feb 14
- if (pir_z & 0x80000000) dir_bits |= ZDi;// Added 6th Feb 14
- acts = Steppers ^ direction_swappers; // read output lines
- acts &= ~dir_mask;
- acts |= dir_bits;
- Steppers = acts ^ direction_swappers;
- tickrun = ticks + ticks_next;
- running = true; // Start the new run
- new_run_pending = false; // Clear the flag which initiated this update
- idle = false;
- } // end of if (new_run_pending) { // Pick up on flag set elsewhere
+ tickrun = ticks + ticks_next;
+ running = true; // Start the new run
+ new_run_pending = false; // Clear the flag which initiated this update
+ idle = false;
} // end of else { // Not running. Grab next data here when or if available
- charge_pumpD25pin1 = ticks & 0x01; // Can use 0x01 or 0x02 here to alter charge pump freq
intled = 0; // LED off
} // end of interrupt handler
@@ -526,47 +443,33 @@
* End of Interrupt Service Routine
*/
bool spindle_running () {
- return pir_spin;
+ if (pir_spin == 0) return false;
+ return true;
}
-void spindle_control (double ss) {
+void spindle_control (signed long ss) {
long t, p;
- pir_spin = (signed long) (ss * spindle_factor);
+ pir_spin = ss * spindle_factor;
t = ticks;
while (t == ticks) {} // wait until just after next interrupt
p = Steppers ^ direction_swappers;
- if (pir_spin & 0x80000000)
- p |= SDi;
- else
- p &= ~SDi;
+ if (pir_spin & 0x80000000) p |= SDi;
+ else p &= ~SDi;
Steppers = p ^ direction_swappers;
- pc.printf("Done setting spindle speed %.0f, delay = %d\r\n", ss, ticks - t);
+ pc.printf("Done setting spindle speed %d, pir_spin %d, delay = %d\r\n", ss, pir_spin, ticks - t);
}
-
-/*
-void scmd (struct singleGparam * a) {
- long t, p;
- if (fabs(a[1].dbl) > spindle_max) {
- pc.printf ("Errror setting spindle RPM, can't set to %.0f, ignoring request\r\n", a[1].dbl);
- return;
- }
- pc.printf ("Setting spindle RPM to %.0f Can set Pos or Neg for fwd/rev\r\n", a[1].dbl);
- pir_spin = (signed long) (a[1].dbl * spindle_factor);
- t = ticks;
- while (t == ticks) {} // wait until just after next interrupt
- p = mysteppers;
- if (pir_spin & 0x80000000)
- p |= SDi;
- else
- p &= ~SDi;
- mysteppers = p;
- pc.printf("Done setting spindle, delay = %d", ticks - t);
-}
+/**
+void pir_updater_task (void const * name) {
+ A task handed to the RTOS Round Robin
+ ISR controls certain flags.
+ This task responds after 'move_ended' flag asserted by NCO ISR
+ If CircBuff not empty, set of floating point data for next point is fetched,
+ remaining calcs performed and translated into signed longs, results placed in
+ set of '_next' locations to be picked up by future NCO ISR
*/
-void newpir_updater (void const * name) {
+void pir_updater_task (void const * name) {
static long x, y, z;//, count = 0;
struct pirbufgrain outs;
-// pc.printf("Arrived at newpir_updater\r\n");
while (true) {
// while (!move_ended || !CircBuff.readable()) { ** TO DO ** Solve problem with empty flag
while (!move_ended || CircBuff.On_Q() == 0) {
@@ -576,16 +479,28 @@
x = (long)(outs.f_rate * outs.x); // These take much CPU time !!
y = (long)(outs.f_rate * outs.y);
z = (long)(outs.f_rate * outs.z);
- ticks_next = (unsigned long)(outs.c / outs.f_rate);
+ ticks_next = (unsigned long)(outs.distance_code / outs.f_rate);
pir_x_next = x;
pir_y_next = y;
pir_z_next = z;
+ dir_bits_next = 0;
+ //if (pir_a & 0x80000000) dir_bits_next |= ADi;// Added 6th Feb 14 read sign bits
+ inc_x_next = inc_y_next = inc_z_next = 1L;
+ if (x < 0) {
+ dir_bits_next |= XDi;
+ inc_x_next = -1L;
+ }
+ if (y < 0) {
+ dir_bits_next |= YDi;
+ inc_y_next = -1L;
+ }
+ if (z < 0) {
+ dir_bits_next |= ZDi;
+ inc_z_next = -1L;
+ }
move_ended = idle = false;
new_run_pending = true; // cleared and 'running' flag set in interrupt handler
-// idle = false;
-// count++;
-// pc.printf("CircB tot %d\r\n", count);
- }
+ } // end of while (true) {
}
@@ -598,32 +513,6 @@
pc.printf("On CircBuff %d\r\n", CircBuff.On_Q());
}
-/*void taskone (void const * name)
-{
- static int i = 0;
- while (true) {
- pc.printf("%s %d\r\n", name, i++);
- Thread::wait(9500);
- osThreadYield();
- }
-}
-
-void tasktwo (void const * name)
-{
- pc.printf("Task Two runs once and exits\r\n");
- Thread::wait(700);
- osThreadYield();
-}
-
-void taskthree (void const * name)
-{
- static int i = 0;
- while (true) {
- pc.printf("%s %d\r\n", name, i++);
- Thread::wait(3500);
- osThreadYield();
- }
-}*/
/*
#define ESTOP 0x100
#define XLIM 1
@@ -658,10 +547,15 @@
//void Emergency_Stop_Interrupt () {
// pc.printf("Emergency Stop Activated !!\r\n");
-// spindle_control (0.0); // Stop spindle rotation
+// spindle_control (0); // Stop spindle rotation
//}
+/**
int main() {
+
+*/
+int main() {
+ long ins, ins_old, ins_changed = 0;
pc.baud(BAUD); // comms to 'PuTTY' serial terminal via mbed usb
// D25pin11_XLim.mode (PullDown); External resistors now fitted
// D25pin12_YLim.mode (PullDown);
@@ -670,24 +564,31 @@
// D25pin10_EStop.mode (PullDown);
// D25pin10_EStop.rise (& Emergency_Stop_Interrupt);
// D25pin10_EStop.fall (& Emergency_Stop_Interrupt);
-
- more_setup () ; // Zeros one 'pirs' structure 'last_position'
- dro_out.init();
+
+ Gparams_clr (last_position);
+ dro_out.init ();
CircBuff.init ();
Inputs_From_Machine.init ();
+#if defined SPI_Enable
spidevice.format(8, 0); // 8 bits mode 0, // p11 mosi, p12 miso, p13 sclk ** ONLY 8 BIT **
spidevice.frequency(12000000); // 12MHz bit rate
+#endif
pc.printf("\r\n*\n*\nFound Computer %s\r\n", Target);
pc.printf("Welcome to the CNC tester\r\nStep pulses required to move 1.0mm = %9.0f\r\n", pulses_per_mm);
- pc.printf("PIR 'n' for 1mm per min = %9.0f\r\ntop speed = %6.1f mm per min\r\n\n", n_for_onemmpermin, max_mm_per_min);
- NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, (long)interrupt_period_us);// Have setup timed interrupts, let other code deal
-// Thread threadnametaskone (taskone, (void *)"task one stuff");
-// Thread t8 (tasktwo, (void *)"task two");
- Thread tsr2 (newpir_updater, (void *)"read from CircBuff and move");
-// Thread tthree (taskthree, (void *)"task three");
-// Thread patterngen (lissajous, (void *)"Loading Lissajous") ;
+ pc.printf("PIR 'n' for 1mm per min = %9.0f\r\ntop speed = %d mm per min\r\n\n", n_for_onemmpermin, feed_rate_max);
+
+
+// NVIC_SetPriority(TIMER3_IRQn, 255); // set mbed tickers to lower priority than other things ONLY COMPILES FOR LPC1768
+
+
+ NCO_gen.attach_us(&Numerically_Controlled_Oscillators_ISR, interrupt_period_us);// Have setup timed interrupts, let other code deal
+ msec.attach_us(&millisec_update_ISR, 1001);
+
+ Thread tsr2 (pir_updater_task, (void *)"read from CircBuff and move");
Thread comlin (command_line_interpreter, (void *)"cli"); // Read any instructions arriving via serial port and act upon them
- long ins, ins_old, ins_changed = 0;
+#if defined I2C_Enable
+ Thread i2cstuff (i2c_handler, (void *)"i2c thing");
+#endif
ins = ins_old = Inputs_From_Machine.read ();
move_ended = true; // Needed to kickstart system
@@ -698,7 +599,6 @@
ins_old = ins;
if (ins_changed)
pc.printf("Inputs Have Changed 0x%x, read 0x%x\r\n", ins_changed, ins);
-// pc.printf(".");
osThreadYield(); //
} // end of Round Robin loop
} // end of int main()