void rotation_us_avant ()
{
static char sens_D = 0, sens_G = 0;
static uint16_t position_D = 0, position_G = 0;
if (sens_D == 0)
{
position_D += 10;
if (position_D > 876)
sens_D = 1;
}
else
{
position_D -= 10;
if (position_D < 703)
sens_D = 0;
}
if (sens_G == 0)
{
position_G += 10;
if (position_G > 666)
sens_G = 1;
}
else
{
position_G -= 10;
if (position_G < 512)
sens_G = 0;
}
angle_AX12(AX_US_DROIT, position_D, 1023, SANS_ATTENTE);
angle_AX12(AX_US_GAUCHE, position_G, 1023, SANS_ATTENTE);
}
/**
* Bouge les AX12 à partir d'un angle donné.
* Cet angle est relatif à un plan, 0° = parallèle au terrain
* \n
* La fonction prend aussi en compte la couleur de départ et inverse les bras à utiliser
* Elle permet aussi d'inverser les angles sur certains AX12
* \n
* Toute l'autom peut donc être fait pour en rouge et sera adaptée automatiquement en Jaune
*
* @param ID : ID de l'AX12 à déplacer
* @param angle : en ° (nombre réel)
* @param vitesse : VIT_AX_NORMAL, VIT_AX_REDUITE, VIT_AX_MOITIE ( de 0 à 1023)
* @param attente : AVEC_ATTENTE, SANS_ATTENTE, pour permettre de synchro les mouvements
*/
void synchro_AX12(uint8_t ID, float angle, uint16_t vitesse, uint8_t attente)
{
if(COULEUR == VIOLET)
{
if(decalage[ID].symetrique > 0)
{
ID = decalage[ID].symetrique;
}
else if(decalage[ID].symetrique < 0)
{
ID = -decalage[ID].symetrique;
angle = -angle;
}
}
angle_AX12(ID, calcul_position(ID, angle), vitesse, attente);
}
void faire_tourner_les_us_devant()
{
angle_AX12(AX_US_GAUCHE, 100, 200, AVEC_ATTENTE); //Position regarde coté droit
angle_AX12(AX_US_DROIT, 625, 1023, AVEC_ATTENTE); //Position regarde coté gauche
lancer_autom_AX12();
delay_ms(100);
angle_AX12(AX_US_DROIT, 400, 1023, AVEC_ATTENTE); //Position regarde avant droit
angle_AX12(AX_US_GAUCHE, 350, 1023, AVEC_ATTENTE); //Position regarde avant gauche
lancer_autom_AX12();
delay_ms(100);
angle_AX12(AX_US_GAUCHE, 650, 1023, AVEC_ATTENTE); //Position regarde derrière gauche
angle_AX12(AX_US_DROIT, 100, 1023, AVEC_ATTENTE); //Position regarde derrière droit
lancer_autom_AX12();
delay_ms(100);
}
void aller_chercher_cubes()
{
//delay_ms(1000);
angle_AX12(PINCE_D, 285, 300, AVEC_ATTENTE); //Position rangé
angle_AX12(PINCE_G, 735, 300, AVEC_ATTENTE); //Position rangé
angle_AX12(ASCENSEUR, 235, 512, AVEC_ATTENTE); //Position basse
lancer_autom_AX12();
delay_ms(10);
rejoindre(200, 1235,MARCHE_AVANT, 100);
//rejoindre(650, 1600,MARCHE_AVANT, 100);
angle_AX12(PINCE_D, 720, 300, AVEC_ATTENTE); //Position intermédiaire (où il est sur le point d'attraper)
angle_AX12(PINCE_G, 320, 300, AVEC_ATTENTE); //Position intermédiaire (où il est sur le point d'attraper)
lancer_autom_AX12();
rejoindre(885, 1600,MARCHE_AVANT, 100);
//cibler(885, 1900, 50);
rejoindre(885, 1735, MARCHE_AVANT, 30);
angle_AX12(PINCE_D, 815, 300, AVEC_ATTENTE); //Position où il attrappe
angle_AX12(PINCE_G, 180, 300, AVEC_ATTENTE); //Positions où il attrappe
lancer_autom_AX12();
delay_ms(100);
angle_AX12(ASCENSEUR, 380, 512, SANS_ATTENTE); //Position haut
}
void recuperer_poissons()
{
init_position_robot(78., 1235., 0.);
//Init de départ
angle_AX12(PINCE_D, 285, 300, AVEC_ATTENTE); //Position rangé
angle_AX12(PINCE_G, 735, 300, AVEC_ATTENTE); //Position rangé
angle_AX12(ASCENSEUR, 235, 512, AVEC_ATTENTE); //Position basse
angle_AX12(ROT_FILET, 85, 300, AVEC_ATTENTE); //Position relevé (Tout début)
angle_AX12(OUVERTURE_FILET, 256, 300, AVEC_ATTENTE); //Position fermé
angle_AX12(CALAGE_CONE, 750, 1023, AVEC_ATTENTE);
lancer_autom_AX12();
angle_AX12(DEPLOIMENT_BRAS_FILET, 820, 200, SANS_ATTENTE); //Position remonté
delay_ms(100);
passe_part(500, 1235, MARCHE_AVANT, 100, DEBUT_TRAJECTOIRE);
passe_part(1100, 250, MARCHE_AVANT, 60, MILIEU_TRAJECTOIRE);
passe_part(1000, 200,MARCHE_AVANT, 40, MILIEU_TRAJECTOIRE);
passe_part(900, 175,MARCHE_AVANT, 40, MILIEU_TRAJECTOIRE);
passe_part(800, 157,MARCHE_AVANT, 40, MILIEU_TRAJECTOIRE);
passe_part(430, 157,MARCHE_AVANT, 40, FIN_TRAJECTOIRE);
angle_AX12(DEPLOIMENT_BRAS_FILET, 530, 200, SANS_ATTENTE); //Position déployé
delay_ms(100);
angle_AX12(OUVERTURE_FILET, 860, 300, SANS_ATTENTE); //Position ouverte
delay_ms(100);
angle_AX12(ROT_FILET, 375, 300, SANS_ATTENTE); //Position Intermédiaire (avant de rentrer dans l'eau)
delay_ms(100);
rejoindre(635, 158, MARCHE_ARRIERE, 40);
angle_AX12(ROT_FILET, 690, 150, SANS_ATTENTE); //Position dans l'eau
delay_ms(100);
rejoindre(795, 158, MARCHE_ARRIERE, 40);
angle_AX12(ROT_FILET, 1005, 600, SANS_ATTENTE); //Position Fin (poissons récupérés)
delay_ms(100);
angle_AX12(DEPLOIMENT_BRAS_FILET, 600, 350, SANS_ATTENTE); //Position intermédiaire (pour passé la barre du filet)
delay_ms(200);
passe_part(900, 210, MARCHE_ARRIERE, 40, DEBUT_TRAJECTOIRE);
passe_part(1000, 210,MARCHE_ARRIERE, 70, MILIEU_TRAJECTOIRE);
passe_part(1200, 155,MARCHE_ARRIERE, 70, MILIEU_TRAJECTOIRE);
passe_part(1300, 158,MARCHE_ARRIERE, 70, FIN_TRAJECTOIRE);
angle_AX12(DEPLOIMENT_BRAS_FILET, 530, 150, AVEC_ATTENTE); //Position déployé
angle_AX12(ROT_FILET, 375, 300, AVEC_ATTENTE); //Position Intermédiaire (avant de rentrer dans l'eau)
lancer_autom_AX12();
delay_ms(1000);
}
void Action_attraper_cubes()
{
delay_ms(3000);
angle_AX12(PINCE_D, 285, 300, AVEC_ATTENTE); //Position rangé
delay_ms(20);
angle_AX12(PINCE_G, 735, 300, AVEC_ATTENTE); //Position rangé
delay_ms(20);
angle_AX12(CALAGE_CONE, 750, 1023, AVEC_ATTENTE); //Position replié
delay_ms(20);
angle_AX12(ASCENSEUR, 265, 512, AVEC_ATTENTE); //Position basse
delay_ms(20);
lancer_autom_AX12();
delay_ms(2000);
angle_AX12(PINCE_D, 720, 300, AVEC_ATTENTE); //Position intermédiaire (où il est sur le point d'attraper)
delay_ms(10);
angle_AX12(PINCE_G, 320, 300, AVEC_ATTENTE); //Position intermédiaire (où il est sur le point d'attraper)
delay_ms(10);
lancer_autom_AX12();
delay_ms(5000);
angle_AX12(PINCE_D, 835, 300, AVEC_ATTENTE); //Position où il attrappe
delay_ms(10);
angle_AX12(PINCE_G, 200, 300, AVEC_ATTENTE); //Positions où il attrappe
delay_ms(10);
lancer_autom_AX12();
delay_ms(1500);
angle_AX12(CALAGE_CONE, 450, 1023, SANS_ATTENTE); //Position déplié
delay_ms(1500);
angle_AX12(ASCENSEUR, 500, 200, SANS_ATTENTE); //Position haut
delay_ms(3000);
angle_AX12(ASCENSEUR, 265, 512, SANS_ATTENTE); //Position basse
delay_ms(3000);
}
void Action_poisson_rapide()
{
delay_ms(3000);
angle_AX12(DEPLOIMENT_BRAS_FILET, 530, 200, SANS_ATTENTE); //Position déployé
delay_ms(1000);
angle_AX12(OUVERTURE_FILET, 860, 300, AVEC_ATTENTE); //Position ouverte
delay_ms(10);
angle_AX12(ROT_FILET, 375, 300, AVEC_ATTENTE); //Position Intermédiaire (avant de rentrer dans l'eau)
delay_ms(10);
lancer_autom_AX12();
delay_ms(1000);
angle_AX12(ROT_FILET, 690, 300, SANS_ATTENTE); //Position dans l'eau
delay_ms(1000);
angle_AX12(ROT_FILET, 1005, 300, SANS_ATTENTE); //Position Fin (poissons récupérés)
delay_ms(2000);
angle_AX12(DEPLOIMENT_BRAS_FILET, 600, 150, SANS_ATTENTE); //Position intermédiaire (pour passé la barre du filet)
delay_ms(500);
angle_AX12(DEPLOIMENT_BRAS_FILET, 530, 150, SANS_ATTENTE); //Position déployé
delay_ms(500);
angle_AX12(ROT_FILET, 375, 300, SANS_ATTENTE); //Position Intermédiaire (avant de rentrer dans l'eau)
delay_ms(1500);
angle_AX12(ROT_FILET, 85, 300, AVEC_ATTENTE); //Position relevé (Tout début)
delay_ms(10);
angle_AX12(OUVERTURE_FILET, 256, 300, AVEC_ATTENTE); //Position fermé
delay_ms(10);
lancer_autom_AX12();
delay_ms(1000);
angle_AX12(DEPLOIMENT_BRAS_FILET, 820, 200, SANS_ATTENTE); //Position remonté
delay_ms(3000);
}
void init_pinces_jack()
{
angle_AX12(PORTE_D, 350, 1023, SANS_ATTENTE);
delay_ms(1000);
angle_AX12(PORTE_G, 655, 1023, SANS_ATTENTE);
}
