File content as of revision 0:5d0f270bfc87:

#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);
}*/