File content as of revision 58:02dc8328975a:

#include "defines.h"
#include "ControlleurPince.h"

ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL):
    RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL)
{
    pos_r = 0;
    pos_z = 0;
    pos_l = 0;
}


void ControlleurPince::home(bool r, bool z, bool l)
{
    if(r)
    {
        while(EnR==true) RMot.step(1, BAS, DELAY);
        pos_r = 0;
    }
    if(z)
    {
        while(EnZ==true) ZMot.step(1, BAS, DELAY);
        pos_z = 0;
    }
    if(l)
    {
        while(EnL==true) LMot.step(1, BAS, DELAY);
        pos_l = 0;
    }
}

void ControlleurPince::setPos(float z, float delta, float center)
{
    if(z > 0.f && z < 100.f)
    {
        logger.printf("Move by %f \n\r",z-pos_z);
        ZMot.mm(z-pos_z);
        
        pos_z = z;
    }
}