Diff: arith.cpp

Revision:

0:5d0f270bfc87

Child:

1:66ee619f206b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/arith.cpp	Fri Jan 31 11:16:21 2014 +0000
@@ -0,0 +1,110 @@
+#include "mbed.h"
+#include "cnc.h"
+using namespace std;
+
+extern  Serial  pc;
+extern  double  feed_rate;
+const  long ball_screw_pitch_mm =   4.0,    //  KX3 has 4mm ball screws
+            motor_step_per_rev  =   200,    //  KX3 has 200 step per rev steppers
+            micro_steps         =   32,      //  Arc Eurotrade choice 2,4,5,8,10,16,20,25,32,40,50,64,100,125,128
+            pulses_per_mm       =   (micro_steps * motor_step_per_rev) / ball_screw_pitch_mm,
+
+            interrupt_period_us =   24, //16,
+            interrupt_freq_Hz   =   1000000 / interrupt_period_us,  //  Serious limit when doing all in software, no real limit with FPGA
+            max_pulse_freq_Hz   =   interrupt_freq_Hz / 6,  //  strictly 4, but allow a good margin
+            max_mm_per_min      =   (60 * max_pulse_freq_Hz) / pulses_per_mm;
+
+const   double  n_for_onemmpermin   = (double)(pulses_per_mm * interrupt_period_us) * pow(2.0,32) / 60000000.0, //  n pir to produce 1mm/min travel
+                feed_rate_max       = 300.0,
+                feed_rate_min       = 0.0,
+                spindle_max         = 5000.0,
+                spindle_min         = 0.0;
+//The output frequency F<sub>out</sub> = 'Kernel Speed (Hz)' * n / (2 to the power of 32)
+
+struct  Gparams    last_position;
+void    grain_clr   (struct singleGparam & g)  {
+    g.dbl = 0.0;
+    g.ul = 0L;
+    g.i = g.c = 0;
+    g.changed = false;
+}
+void    pirs_clr2    (struct Gparams & p)   {
+    grain_clr   (p.x);    grain_clr   (p.y);    grain_clr   (p.z);    grain_clr   (p.i);    grain_clr   (p.j);
+    grain_clr   (p.r);    grain_clr   (p.a);    grain_clr   (p.b);    grain_clr   (p.c);    grain_clr   (p.d);
+}
+void    init_last_position  ()  {
+    pirs_clr2    (last_position);
+}
+
+double  find_distance   (struct Gparams & from, struct Gparams & to, struct Gparams & distance)    {
+    distance.x.dbl    = to.x.dbl - from.x.dbl;
+    distance.y.dbl    = to.y.dbl - from.y.dbl;
+    distance.z.dbl    = to.z.dbl - from.z.dbl;   //  Yes, Pythagoras does work in 3D
+return  sqrt ((distance.x.dbl * distance.x.dbl) + (distance.y.dbl * distance.y.dbl) + (distance.z.dbl * distance.z.dbl));
+}
+
+double  find_traverse_time  (double dist, double rate)  {   //  dist mm, rate mm/min
+    return  60.0 * dist / rate;                             //  time secs
+}
+
+long    find_traverse_ticks (double dist, double rate)  {   //  dist mm, rate mm/min
+    return (long)(find_traverse_time(dist, rate) * 1000000.0) / interrupt_period_us;
+}
+
+/*void    craptest    ()  {
+    Gparams    to, distance;
+    double  dist;
+//    long    q = NCO_n_per_Hz;
+//    feed_rate   =   46.5;   //  global
+//    from.x.d    =   0.0;
+//    from.y.d    =   0.0;
+//    from.z.d    =   0.0;
+    to.x.d      =   45.0;
+    to.y.d      =   -12.375;
+    to.z.d      =   -3.142;
+    dist    = find_distance (last_position, to, distance);
+    pc.printf   ("From X %f Y %f Z %f to X %f Y %f Z %f\r\n",last_position.x.d, last_position.y.d, last_position.z.d, to.x.d, to.y.d, to.z.d);
+    pc.printf   ("Dist X %f Y %f Z %f, total %f\r\n", distance.x.d, distance.y.d, distance.z.d, dist);
+    pc.printf   ("To move %f mm at feed rate %f mm/min takes %f seconds\r\n", dist, feed_rate, find_traverse_time(dist, feed_rate));
+    pc.printf   ("This involves %d interrupt ticks\r\n", find_traverse_ticks(dist, feed_rate));
+//    pc.printf   ("NCO freq = %f when n = %d\r\n", NCO_freq_from_n(q), q);
+//    pc.printf   ("Nfor1mmpersec is %f, pulses_per_mm is %d\r\n",onemmpersec, pulses_per_mm);
+}*/
+
+void    copy_grain  (struct singleGparam & d, struct singleGparam & s)    {
+    d.dbl = s.dbl;
+    d.ul = s.ul;
+    d.i = s.i;
+    d.c = s.c;
+    d.changed = s.changed;
+}
+
+void    copy_pirs   (struct Gparams & d, struct Gparams & s)  {
+    copy_grain  (d.x, s.x);
+    copy_grain  (d.y, s.y);
+    copy_grain  (d.z, s.z);
+    copy_grain  (d.i, s.i);
+    copy_grain  (d.j, s.j);
+    copy_grain  (d.r, s.r);
+    copy_grain  (d.a, s.a);
+    copy_grain  (d.b, s.b);
+    copy_grain  (d.c, s.c);
+    copy_grain  (d.d, s.d);
+}
+
+/*void    swap_pirs   (struct pirs * d, struct pirs * s)  {
+//void    swap_pirs   ()  {
+    struct  pirs    pira, pirb;
+    struct  pirs *  ppa, * ppb, * pptmp;
+    ppa = & pira;
+    ppb = & pirb;
+    ppa->x.d = 1.0;
+    ppa->y.d = 2.0;
+    ppb->x.d = 2.0;
+    ppb->y.d = 1.0;
+    pc.printf   ("pira x = %f, y = %f, pirb x = %f, y = %f,\r\n", ppa->x.d, ppa->y.d, ppb->x.d, ppb->y.d);
+    pptmp = ppa;
+    ppa = ppb;
+    ppb = pptmp;
+    pc.printf   ("pira x = %f, y = %f, pirb x = %f, y = %f,\r\n", ppa->x.d, ppa->y.d, ppb->x.d, ppb->y.d);
+}*/