File content as of revision 71:5590dbe8393a:

#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, AX12 &_Lax12, AX12 &_Rax12) :
    RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL), Lax12(_Lax12), Rax12(_Rax12)
{
    pos_r = 0;
    pos_z = 0;
    pos_l = 0;
    
    
}

void ControlleurPince::init()
{
    Lax12.setMode(0);
    wait(0.01);
    Rax12.setMode(0);
    wait(0.01);
    
    Lax12.setMaxTorque(200);
    wait(0.01);
    Rax12.setMaxTorque(200);
    wait(0.01);
}


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)
    {
        ZMot.mm(z-pos_z,true);
        pos_z = z;
    }
    
    if(delta >= 0 || center >= -500)
    {
        if(delta < 0)
            delta = 130 - pos_r - pos_l;
        if(center < -500)
            center = - pos_r/2 + pos_l/2;
            
        
        
        /*
        => pos_l = center*2 + pos_r
        delta = 130 - pos_r - center*2 - pos_r
        => 2*pos_r = 130 - center*2 - delta
        => pos_r = (130-delta)/2 - center
        => pos_l = (130-delta)/2 + center
        */
        
        float r = (130.f-delta)/2.f - center;
        float l = (130.f-delta)/2.f + center;
        
        RMot.mm(r-pos_r,true);
        pos_r = r;
        
        LMot.mm(l-pos_l,true);
        pos_l = l;
        
    }
    
    while(! (RMot.done() && ZMot.done() && LMot.done()))
        wait(DELAY);
}

void ControlleurPince::close()
{
    Lax12.setGoal(150);
    Rax12.setGoal(150);
    wait(0.2);
}


void ControlleurPince::open()
{
    Lax12.setGoal(100);
    Rax12.setGoal(200);
    wait(0.2);
}