File content as of revision 18:972d29c668a5:

#include "class_ventouse.h"

#ifdef ROBOT_SMALL
//float seuil[NB_VENTOUSES] = {0.10, 0.3, 0.3, 0.3, 0.10, 0.3, 0.3, 0.3};
float seuil[NB_VENTOUSES] = {0.1, 0.1, 0.1, 0.1, 0.10, 0.1, 0.1, 0.1};
#define PWM_POMPE 0.7
#endif

#ifdef ROBOT_BIG
float seuil[NB_VENTOUSES] = {0.2, 0.1, 0.2};
#define PWM_POMPE 0.9
#endif


Ventouse::Ventouse(PinName pinPompe, PinName pinCapteurPression, char index) : m_pinPompe(pinPompe),
m_pinCapteurPression(pinCapteurPression), m_index(index)
{
    m_pompe = new PwmOut(m_pinPompe);
    m_pression = new AnalogIn(m_pinCapteurPression);
    
    m_pompe->period_us(50);
    
    m_pompe->write(0);
    
    flag_attraper = 0;
    flag_relacher = 0;
    
    etat = Attente; 
    m_ack = 0;
    flag_electrovanne = 0;
}


Ventouse::Ventouse(PinName pinPompe, PinName pinElectrovanne, PinName pinCapteurPression, char index) : m_pinPompe(pinPompe),
m_pinElectrovanne(pinElectrovanne), m_pinCapteurPression(pinCapteurPression), m_index(index)
{
    m_pompe = new PwmOut(m_pinPompe);
    m_electrovanne = new DigitalOut(m_pinElectrovanne);
    m_pression = new AnalogIn(m_pinCapteurPression);
    
    m_pompe->period_us(50);
    //m_electrovanne->period_us(50);
    
    m_pompe->write(0);
    m_electrovanne->write(0);
    
    flag_attraper = 0;
    flag_relacher = 0;
    
    etat = Attente; 
    m_ack = 0;
    flag_electrovanne = 1;
}

void Ventouse::automate()
{
    switch(etat)
    {
        case Attente :
            if(flag_attraper)
            {   
                attraper();
                etat = Attraper;
            }
            break;
            
        case Attraper :
            wait_ms(10.0);
            if(getPression()==1)
            {   
                can.write(CANMessage(HACHEUR_GET_ATOM_ACK,&m_index,1));    
                etat = Attraper_ok;
            }else if(flag_relacher)
            {
                relacher();
                etat = Relacher;
            }
            break;
            
        case Attraper_ok :
            if(flag_relacher)
            {
                relacher();
                etat = Relacher;
            }
            break;
            
        case Relacher :
            wait_ms(10.0);
            if(getPression() == 0)
            {
                can.write(CANMessage(HACHEUR_RELEASE_ATOM_ACK,&m_index,1));
                if(flag_electrovanne) m_electrovanne->write(0);
                etat = Attente;  
            }
            break;
            
    }
}


void Ventouse::action(bool action)
{
    flag_attraper = action;
    flag_relacher = !action;
}

void Ventouse::attraper()
{
    m_pompe->write(PWM_POMPE);
    if(flag_electrovanne) m_electrovanne->write(0);

}

void Ventouse::relacher()
{
    m_pompe->write(0.0);
    if(flag_electrovanne) m_electrovanne->write(1);
    wait_ms(100.0);

}

bool Ventouse::getPression()
{   
    if(m_pression->read() > seuil[m_index]){
        return 1;
    }
    else{
        return 0;
    }
 
}

bool Ventouse::getPompe()
{
    if(m_pompe->read() > 0){
        return 1;
    }
    else{
        return 0;
    }
}

void Ventouse::setPompe(float val)
{
    m_pompe->write(val);
}

void Ventouse::setElectrovanne(int val)
{
    if(flag_electrovanne)m_electrovanne->write(val);
}