File content as of revision 23:93a427b242ce:

#include "main.h"
#include "herkulex_rob.h"


//---------------------------------------Petit robot---------------------------------------------------------
/*void gros_robot_init(void)
{
void   palet_accelerateur(){
    compteTour(ID,1000,1,1,PLED_ON,serialbarillet);
    }
void palet_balance(){
    compteTour(ID,-1000,1,1,PLED_ON,serialbarillet);
    }
void sortie_balance()
    {

    }

    */


#ifdef ROBOT_SMALL
//---------------------------------------------------------------------------------------------
void deverouillage_torque (void)  //débloquer les servomoteurs
{
    deverouillage_torque_avant();
    deverouillage_torque_arriere();
    deverouillage_torque_sol();
}
void deverouillage_torque_avant (void)
{
    clear(AV_EP_G, 3);
    clear(AV_poigne_G,3);
    clear(AV_EP_C, 2);
    clear(AV_poigne_C, 2);
    clear(AV_EP_D, 1);
    clear(AV_poigne_D,1);

    setTorque(AV_EP_G, TORQUE_ON,3);
    setTorque(AV_poigne_G, TORQUE_ON,3);
    setTorque(AV_EP_C, TORQUE_ON,2);
    setTorque(AV_poigne_C, TORQUE_ON,2);
    setTorque(AV_EP_D, TORQUE_ON,1);
    setTorque(AV_poigne_D, TORQUE_ON,1);
}

void deverouillage_torque_arriere (void)
{
    clear(AR_EP_G, 3);
    clear(AR_poigne_G,3);
    clear(AR_EP_C, 2);
    clear(AR_poigne_C, 2);
    clear(AR_EP_D, 1);
    clear(AR_poigne_D,1);

    setTorque(AR_EP_G, TORQUE_ON,1);
    setTorque(AR_poigne_G, TORQUE_ON,1);
    setTorque(AR_EP_C, TORQUE_ON,2);
    setTorque(AR_poigne_C, TORQUE_ON,2);
    setTorque(AR_EP_D, TORQUE_ON,3);
    setTorque(AR_poigne_D, TORQUE_ON,3);
}


void deverouillage_torque_sol (void)
{

    clear(AR_sol,4);
    clear(AV_sol,4);

    setTorque(AR_sol, TORQUE_ON,4);
    setTorque(AV_sol, TORQUE_ON,4);

}

#endif 

#ifdef ROBOT_BIG

void deverouillage_torque (void)  //débloquer les servomoteurs
{
    deverouillage_torque_bras();
    deverouillage_torque_convoyeurs();
    deverouillage_torque_doight();
    deverouillage_torque_roues();
}


void deverouillage_torque_bras(void)
{
    clear(AV_EP_C,2);
    clear(AV_poigne_C,2);

    setTorque(AV_EP_C, TORQUE_ON,2);
    setTorque(AV_poigne_C, TORQUE_ON,2);
}

void deverouillage_torque_convoyeurs(void)
{
    clear(stockage_D,3);
    clear(stockage_G,1);

    setTorque(stockage_D, TORQUE_ON,3);
    setTorque(stockage_G, TORQUE_ON,1);
}

void deverouillage_torque_roues(void)
{
    clear(roue_D,3);
    clear(roue_G,1);

    setTorque(roue_D, TORQUE_ON,3);
    setTorque(roue_G, TORQUE_ON,1);
}


void deverouillage_torque_doigt(void)
{
    clear(doigt,2);
    setTorque(doigt, TORQUE_ON,2);
}

#endif