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

arith.cpp@0:5d0f270bfc87, 2014-01-31 (annotated)

Committer:
JonFreeman
Date:
Fri Jan 31 11:16:21 2014 +0000
Revision:
0:5d0f270bfc87
Child:
1:66ee619f206b
First wip, tested on KL25 and KL46

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:5d0f270bfc87 1 #include "mbed.h"
JonFreeman 0:5d0f270bfc87 2 #include "cnc.h"
JonFreeman 0:5d0f270bfc87 3 using namespace std;
JonFreeman 0:5d0f270bfc87 4
JonFreeman 0:5d0f270bfc87 5 extern Serial pc;
JonFreeman 0:5d0f270bfc87 6 extern double feed_rate;
JonFreeman 0:5d0f270bfc87 7 const long ball_screw_pitch_mm = 4.0, // KX3 has 4mm ball screws
JonFreeman 0:5d0f270bfc87 8 motor_step_per_rev = 200, // KX3 has 200 step per rev steppers
JonFreeman 0:5d0f270bfc87 9 micro_steps = 32, // Arc Eurotrade choice 2,4,5,8,10,16,20,25,32,40,50,64,100,125,128
JonFreeman 0:5d0f270bfc87 10 pulses_per_mm = (micro_steps * motor_step_per_rev) / ball_screw_pitch_mm,
JonFreeman 0:5d0f270bfc87 11
JonFreeman 0:5d0f270bfc87 12 interrupt_period_us = 24, //16,
JonFreeman 0:5d0f270bfc87 13 interrupt_freq_Hz = 1000000 / interrupt_period_us, // Serious limit when doing all in software, no real limit with FPGA
JonFreeman 0:5d0f270bfc87 14 max_pulse_freq_Hz = interrupt_freq_Hz / 6, // strictly 4, but allow a good margin
JonFreeman 0:5d0f270bfc87 15 max_mm_per_min = (60 * max_pulse_freq_Hz) / pulses_per_mm;
JonFreeman 0:5d0f270bfc87 16
JonFreeman 0:5d0f270bfc87 17 const double n_for_onemmpermin = (double)(pulses_per_mm * interrupt_period_us) * pow(2.0,32) / 60000000.0, // n pir to produce 1mm/min travel
JonFreeman 0:5d0f270bfc87 18 feed_rate_max = 300.0,
JonFreeman 0:5d0f270bfc87 19 feed_rate_min = 0.0,
JonFreeman 0:5d0f270bfc87 20 spindle_max = 5000.0,
JonFreeman 0:5d0f270bfc87 21 spindle_min = 0.0;
JonFreeman 0:5d0f270bfc87 22 //The output frequency F<sub>out</sub> = 'Kernel Speed (Hz)' * n / (2 to the power of 32)
JonFreeman 0:5d0f270bfc87 23
JonFreeman 0:5d0f270bfc87 24 struct Gparams last_position;
JonFreeman 0:5d0f270bfc87 25 void grain_clr (struct singleGparam & g) {
JonFreeman 0:5d0f270bfc87 26 g.dbl = 0.0;
JonFreeman 0:5d0f270bfc87 27 g.ul = 0L;
JonFreeman 0:5d0f270bfc87 28 g.i = g.c = 0;
JonFreeman 0:5d0f270bfc87 29 g.changed = false;
JonFreeman 0:5d0f270bfc87 30 }
JonFreeman 0:5d0f270bfc87 31 void pirs_clr2 (struct Gparams & p) {
JonFreeman 0:5d0f270bfc87 32 grain_clr (p.x); grain_clr (p.y); grain_clr (p.z); grain_clr (p.i); grain_clr (p.j);
JonFreeman 0:5d0f270bfc87 33 grain_clr (p.r); grain_clr (p.a); grain_clr (p.b); grain_clr (p.c); grain_clr (p.d);
JonFreeman 0:5d0f270bfc87 34 }
JonFreeman 0:5d0f270bfc87 35 void init_last_position () {
JonFreeman 0:5d0f270bfc87 36 pirs_clr2 (last_position);
JonFreeman 0:5d0f270bfc87 37 }
JonFreeman 0:5d0f270bfc87 38
JonFreeman 0:5d0f270bfc87 39 double find_distance (struct Gparams & from, struct Gparams & to, struct Gparams & distance) {
JonFreeman 0:5d0f270bfc87 40 distance.x.dbl = to.x.dbl - from.x.dbl;
JonFreeman 0:5d0f270bfc87 41 distance.y.dbl = to.y.dbl - from.y.dbl;
JonFreeman 0:5d0f270bfc87 42 distance.z.dbl = to.z.dbl - from.z.dbl; // Yes, Pythagoras does work in 3D
JonFreeman 0:5d0f270bfc87 43 return sqrt ((distance.x.dbl * distance.x.dbl) + (distance.y.dbl * distance.y.dbl) + (distance.z.dbl * distance.z.dbl));
JonFreeman 0:5d0f270bfc87 44 }
JonFreeman 0:5d0f270bfc87 45
JonFreeman 0:5d0f270bfc87 46 double find_traverse_time (double dist, double rate) { // dist mm, rate mm/min
JonFreeman 0:5d0f270bfc87 47 return 60.0 * dist / rate; // time secs
JonFreeman 0:5d0f270bfc87 48 }
JonFreeman 0:5d0f270bfc87 49
JonFreeman 0:5d0f270bfc87 50 long find_traverse_ticks (double dist, double rate) { // dist mm, rate mm/min
JonFreeman 0:5d0f270bfc87 51 return (long)(find_traverse_time(dist, rate) * 1000000.0) / interrupt_period_us;
JonFreeman 0:5d0f270bfc87 52 }
JonFreeman 0:5d0f270bfc87 53
JonFreeman 0:5d0f270bfc87 54 /*void craptest () {
JonFreeman 0:5d0f270bfc87 55 Gparams to, distance;
JonFreeman 0:5d0f270bfc87 56 double dist;
JonFreeman 0:5d0f270bfc87 57 // long q = NCO_n_per_Hz;
JonFreeman 0:5d0f270bfc87 58 // feed_rate = 46.5; // global
JonFreeman 0:5d0f270bfc87 59 // from.x.d = 0.0;
JonFreeman 0:5d0f270bfc87 60 // from.y.d = 0.0;
JonFreeman 0:5d0f270bfc87 61 // from.z.d = 0.0;
JonFreeman 0:5d0f270bfc87 62 to.x.d = 45.0;
JonFreeman 0:5d0f270bfc87 63 to.y.d = -12.375;
JonFreeman 0:5d0f270bfc87 64 to.z.d = -3.142;
JonFreeman 0:5d0f270bfc87 65 dist = find_distance (last_position, to, distance);
JonFreeman 0:5d0f270bfc87 66 pc.printf ("From X %f Y %f Z %f to X %f Y %f Z %f\r\n",last_position.x.d, last_position.y.d, last_position.z.d, to.x.d, to.y.d, to.z.d);
JonFreeman 0:5d0f270bfc87 67 pc.printf ("Dist X %f Y %f Z %f, total %f\r\n", distance.x.d, distance.y.d, distance.z.d, dist);
JonFreeman 0:5d0f270bfc87 68 pc.printf ("To move %f mm at feed rate %f mm/min takes %f seconds\r\n", dist, feed_rate, find_traverse_time(dist, feed_rate));
JonFreeman 0:5d0f270bfc87 69 pc.printf ("This involves %d interrupt ticks\r\n", find_traverse_ticks(dist, feed_rate));
JonFreeman 0:5d0f270bfc87 70 // pc.printf ("NCO freq = %f when n = %d\r\n", NCO_freq_from_n(q), q);
JonFreeman 0:5d0f270bfc87 71 // pc.printf ("Nfor1mmpersec is %f, pulses_per_mm is %d\r\n",onemmpersec, pulses_per_mm);
JonFreeman 0:5d0f270bfc87 72 }*/
JonFreeman 0:5d0f270bfc87 73
JonFreeman 0:5d0f270bfc87 74 void copy_grain (struct singleGparam & d, struct singleGparam & s) {
JonFreeman 0:5d0f270bfc87 75 d.dbl = s.dbl;
JonFreeman 0:5d0f270bfc87 76 d.ul = s.ul;
JonFreeman 0:5d0f270bfc87 77 d.i = s.i;
JonFreeman 0:5d0f270bfc87 78 d.c = s.c;
JonFreeman 0:5d0f270bfc87 79 d.changed = s.changed;
JonFreeman 0:5d0f270bfc87 80 }
JonFreeman 0:5d0f270bfc87 81
JonFreeman 0:5d0f270bfc87 82 void copy_pirs (struct Gparams & d, struct Gparams & s) {
JonFreeman 0:5d0f270bfc87 83 copy_grain (d.x, s.x);
JonFreeman 0:5d0f270bfc87 84 copy_grain (d.y, s.y);
JonFreeman 0:5d0f270bfc87 85 copy_grain (d.z, s.z);
JonFreeman 0:5d0f270bfc87 86 copy_grain (d.i, s.i);
JonFreeman 0:5d0f270bfc87 87 copy_grain (d.j, s.j);
JonFreeman 0:5d0f270bfc87 88 copy_grain (d.r, s.r);
JonFreeman 0:5d0f270bfc87 89 copy_grain (d.a, s.a);
JonFreeman 0:5d0f270bfc87 90 copy_grain (d.b, s.b);
JonFreeman 0:5d0f270bfc87 91 copy_grain (d.c, s.c);
JonFreeman 0:5d0f270bfc87 92 copy_grain (d.d, s.d);
JonFreeman 0:5d0f270bfc87 93 }
JonFreeman 0:5d0f270bfc87 94
JonFreeman 0:5d0f270bfc87 95 /*void swap_pirs (struct pirs * d, struct pirs * s) {
JonFreeman 0:5d0f270bfc87 96 //void swap_pirs () {
JonFreeman 0:5d0f270bfc87 97 struct pirs pira, pirb;
JonFreeman 0:5d0f270bfc87 98 struct pirs * ppa, * ppb, * pptmp;
JonFreeman 0:5d0f270bfc87 99 ppa = & pira;
JonFreeman 0:5d0f270bfc87 100 ppb = & pirb;
JonFreeman 0:5d0f270bfc87 101 ppa->x.d = 1.0;
JonFreeman 0:5d0f270bfc87 102 ppa->y.d = 2.0;
JonFreeman 0:5d0f270bfc87 103 ppb->x.d = 2.0;
JonFreeman 0:5d0f270bfc87 104 ppb->y.d = 1.0;
JonFreeman 0:5d0f270bfc87 105 pc.printf ("pira x = %f, y = %f, pirb x = %f, y = %f,\r\n", ppa->x.d, ppa->y.d, ppb->x.d, ppb->y.d);
JonFreeman 0:5d0f270bfc87 106 pptmp = ppa;
JonFreeman 0:5d0f270bfc87 107 ppa = ppb;
JonFreeman 0:5d0f270bfc87 108 ppb = pptmp;
JonFreeman 0:5d0f270bfc87 109 pc.printf ("pira x = %f, y = %f, pirb x = %f, y = %f,\r\n", ppa->x.d, ppa->y.d, ppb->x.d, ppb->y.d);
JonFreeman 0:5d0f270bfc87 110 }*/