camfer-pr

C++ implementation

#include <math.h>
node {

            int NOD(int a, int b)
{
    while(a > 0 && b > 0)

        if(a > b)
            a %= b;

        else
            b %= a;

    return a + b;
}
    void evaluate(Context ctx) {


        auto enc = getValue<input_ENCD>(ctx);
        auto sx = getValue<input_SX>(ctx);
        auto sy = getValue<input_SY>(ctx);
        auto dx = getValue<input_DX>(ctx);
        auto dy = getValue<input_DY>(ctx);
        auto ex = getValue<input_EX>(ctx);
        auto ey = getValue<input_EY>(ctx);
        bool dirX = false;
        bool dirY = false;
        float len = getValue<input_LEN>(ctx);
        float dep = getValue<input_DEP>(ctx);
        int ymul = getValue<input_YMUL>(ctx);
        float tie = getValue<input_TIE>(ctx); //Глубина врезки за проход в мм
        unsigned int imp = getValue<input_IMP>(ctx); //Импульсов на 1 мм движения по оси Х
        int kx; //коэф. по оси X
        int ky; //коэф. по оси Y
        int res; // НОД двух чисел (результирующая)
        int lx = len * 10000;
        int ly = dep * 10000 * ymul;
        res = NOD(lx, ly);
        kx = lx / res;
        ky = ly / res;
        int kimp = 0;
        unsigned long countimp = 0;
        unsigned long countmin = 0;
        float gsin = dep / sqrt (len * len + dep * dep); // sin угла при прямой len
        unsigned long VALsfeeds =  round(gsin * len / tie) ; // кол-во подач поперпендикуляру гипотенузы
        unsigned long slen = round(imp * len);  // Кол-во шагов по оси X
        unsigned long sdep = ymul * round(imp * dep); // Общее кол-во шагов по оси Y
        unsigned long sfeedsY = round(sdep / VALsfeeds); // Кол-во шагов на подачу по оси Y
        unsigned long sfeedsX = round(slen / VALsfeeds); // Кол-во шагов на подачу по оси X
        unsigned long num = 0;
        unsigned long moove = 0;
        unsigned long numpass = 0;
        unsigned long numx = 0;
        unsigned long numy = 0;

        if (!isInputDirty<input_UPD>(ctx))
                return;

        if (ly >= lx) kimp = trunc(ly / lx);
        else kimp = trunc(lx / ly);




        pinMode(enc, INPUT);
        pinMode(sx, OUTPUT);
        pinMode(sy, OUTPUT);
        pinMode(dx, OUTPUT);
        pinMode(dy, OUTPUT);
        pinMode(ex, OUTPUT);
        pinMode(ey, OUTPUT);

        digitalWrite(ex, false);
        digitalWrite(ey, false);


// Если Ly>Lx
// Углубление резца на 1 подачу по оси Y
        //dirX = false;
        dirY = false;
        //digitalWrite(dx, dirX); // Движение X <
        digitalWrite(dy, dirY);
        num = 0;
        moove = sfeedsY;
     do {
            if (!digitalRead(enc)) {
            digitalWrite(sy, false);
            while (!digitalRead(enc)) {}
                                    }
            if (digitalRead(enc)) {
            digitalWrite(sy, true);
            while (digitalRead(enc)) {}
            num = num + 1;
                                    }
        } while (num <= moove);
// ТЕЛО движения по гипотенузе ---------------

// Проход по гипотинузе сзада вперед и справа налево
        dirX = true;
        dirY = true;
        digitalWrite(dx, dirX); 
        digitalWrite(dy, dirY);
        num = 0;
        moove = sfeedsX;
        countimp = 0;
        countmin = 0;
     do {
            if (!digitalRead(enc)) {
            digitalWrite(sx, false);
            digitalWrite(sy, false);
            while (!digitalRead(enc)) {}
                                }
            if (digitalRead(enc)) {
            //digitalWrite(sx, true);
            digitalWrite(sy, true);
            countimp ++
            if(countimp % kimp = 0 && countmin <= kx) {
                digitalWrite(sx, true); countmin ++;
            }

            if (countmin = kx) countmin = 0;
            if (countimp = ky) countimp = 0;
            num ++;
            while (digitalRead(enc)) {}                   }
        } while (num < moove);






// Конец ТЕЛА движения по гипотенузе ---------------

// Смещение резца на 1 подачу по оси X
        //dirX = false;
        dirX = true;
        digitalWrite(dx, dirX); // Движение X <
        //digitalWrite(dy, dirY);
        num = 0;
        moove = sfeedsX;
     do {
            if (!digitalRead(enc)) {
            digitalWrite(sx, false);
            //digitalWrite(sy, false);
            while (!digitalRead(enc)) {}
                                }
            if (digitalRead(enc)) {
            digitalWrite(sx, true);
            //digitalWrite(sy, true);
            while (digitalRead(enc)) {}
            num = num + 1;
                               }
        } while (num <= moove);


        emitValue<output_OUTX>(ctx, sfeedsX);
        emitValue<output_OUTY>(ctx, sfeedsY);
        emitValue<output_OUTR>(ctx, VALsfeeds);
    }
}