File content as of revision 99:1fcb088f8f36:

#include "ControlleurPince.h"

float max(float a, float b)
{
    return a<b?b:a;
}

float min(float a, float b)
{
    return a<b?a:b;
}

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 = min(130,z);
        ZMot.mm(z-pos_z,true);
        pos_z = z;
    }
    
    if(delta >= 0 || center >= -500)
    {
        if(delta < 0)
            delta = 150 - 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 = (150.f-delta)/2.f - center;
        float l = (150.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(2);*/
    
    angle(0);
}


void ControlleurPince::open()
{
    /*Lax12.setGoal(46);
    Rax12.setGoal(248);
    wait(2);*/
    
    angle(100);
}

void ControlleurPince::angle(int a)
{
    a = min(a,100);
    
    Lax12.setGoal(150-a);
    Rax12.setGoal(150+a);
    wait(2);
}