CRAC Team
code ax12 petit robot 12/05/2017
Fork of
command_AX12_petit_robot_V3
by 
CRAC Team
Diff: main.cpp
- Revision:
- 3:6cbd7d2c78f4
- Parent:
- 2:99b1cb0d9f5e
- Child:
- 4:888b32bc5b63
--- a/main.cpp Tue Apr 18 16:02:40 2017 +0000
+++ b/main.cpp Wed Apr 19 15:53:45 2017 +0000
@@ -18,7 +18,7 @@
short vitesse=700;
float angle=0.0;
float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
-AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *six_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
+AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
/* PROTOTYPES DE FONCTIONS ET POINTEURS */
@@ -44,94 +44,168 @@
void SendRawId (unsigned short id);
/****************************************************************************************/
-/* FUNCTION NAME: prendre_modules */
-/* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */
-/****************************************************************************************/
-void Prendre_modules(unsigned char choix);
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise */
+/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
+/****************************************************************************************/
+void Preparation_prise(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_haut */
+/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
+/****************************************************************************************/
+void Stockage_haut(unsigned char choix);
/****************************************************************************************/
-/* FUNCTION NAME: Poser_modules */
-/* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */
+/* FUNCTION NAME: Stockage_bas */
+/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
/****************************************************************************************/
-void Poser_modules(unsigned char choix);
+void Stockage_bas(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Deposer */
+/* DESCRIPTION : Fonction qui permet de déposer un module */
+/****************************************************************************************/
+void Deposer(unsigned char choix);
/****************************************************************************************/
-/* FUNCTION NAME: Funny_action */
-/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
+/* FUNCTION NAME: Preparation_depos_bas */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
/****************************************************************************************/
-void Funny_Action(void);
+void Preparation_depos_bas(unsigned char choix);
/****************************************************************************************/
-/* FUNCTION NAME: Tourner_modules */
-/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
+/* FUNCTION NAME: Preparation_depos_haut */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
/****************************************************************************************/
-void Tourner_modules(void);
+void Preparation_depos_haut(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Pousser_module */
+/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
+/****************************************************************************************/
+void Pousser_module(unsigned char choix);
Timer t;
Ticker flipper;
///////////TABLEAU PINCE DROITE ///////////////////////////
-static char TAB1[25]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte
- 0x04,0x00, 0x02, 0xFF, 0x03,
- 0x07,0x4D, 0x01, 0xFF, 0x03,
- 0x0F,0x90, 0x01, 0xFF, 0x03};
+static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
+ 0x04,0xF4, 0x01, 0xFF, 0x00,
+ 0x07,0x50, 0x00, 0xFF, 0x00,
+ 0x0F,0xB0, 0x03, 0xFF, 0x00};
-static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
- 0x04,0xC0, 0x00, 0xFF, 0x03,
- 0x07,0x4D, 0x01, 0xFF, 0x03,
- 0x0F,0x90, 0x01, 0xFF, 0x03};
+static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
+ 0x04,0xE9, 0x00, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x0F,0xF4, 0x01, 0xFF, 0x03};
-static char TAB3[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee
- 0x04,0x15, 0x02, 0xFF, 0x03,
- 0x07,0xC0, 0x00, 0xFF, 0x03,
- 0x0F,0x05, 0x02, 0xFF, 0x03};
+static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
+ 0x04,0xE9, 0x00, 0xFF, 0x03,
+ 0x07,0xB5, 0x00, 0xFF, 0x03,
+ 0x0F,0x39, 0x03, 0xFF, 0x03};
-static char TAB4[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement
- 0x04,0x30, 0x01, 0xFF, 0x03,
- 0x07,0xC0, 0x00, 0xFF, 0x03,
- 0x0F,0x05, 0x02, 0xFF, 0x03};
+static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
+ 0x04,0x00, 0x02, 0xFF, 0x03,
+ 0x07,0xB5, 0x00, 0xFF, 0x03,
+ 0x0F,0x39, 0x03, 0xFF, 0x03};
+
+static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
+ 0x04,0x60, 0x01, 0xFF, 0x00,
+ 0x07,0xB5, 0x00, 0xFF, 0x00,
+ 0x0F,0x39, 0x03, 0xFF, 0x00};
+
+static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x04,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0x0A, 0x01, 0xFF, 0x03,
+ 0x0F,0xDD, 0x02, 0xFF, 0x03};
+
+static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
+ 0x04,0xDE, 0x03, 0xFF, 0x03,
+ 0x07,0xB5, 0x00, 0xFF, 0x03,
+ 0x0F,0x39, 0x03, 0xFF, 0x03};
-static char TAB5[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
- 0x04,0xC0, 0x00, 0xFF, 0x03,
- 0x07,0xC0, 0x00, 0xFF, 0x03,
- 0x0F,0x05, 0x02, 0xFF, 0x03};
+static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras)
+ 0x04,0x00, 0x03, 0xFF, 0x00,
+ 0x07,0xB5, 0x00, 0xFF, 0x00,
+ 0x0F,0x39, 0x03, 0xFF, 0x00};
+
+static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte)
+ 0x04,0x00, 0x03, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x0F,0xF4, 0x01, 0xFF, 0x03};
-static char TAB6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin
- 0x04,0x4D, 0x01, 0xFF, 0x03,
- 0x07,0x4D, 0x01, 0xFF, 0x03,
- 0x0F,0x90, 0x01, 0xFF, 0x03};
+static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut)
+ 0x04,0x00, 0x02, 0xFF, 0x00,
+ 0x07,0xB5, 0x00, 0xFF, 0x00,
+ 0x0F,0x39, 0x03, 0xFF, 0x00};
+
+static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
+ 0x04,0x00, 0x03, 0xFF, 0x00,
+ 0x07,0xB5, 0x00, 0xFF, 0x00,
+ 0x0F,0x39, 0x03, 0xFF, 0x00};
//////////////////TABLEAU PINCE GAUCHE /////////////////////////////
-static char TAB1125]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte
- 0x04,0x00, 0x02, 0xFF, 0x03,
- 0x07,0x4D, 0x01, 0xFF, 0x03,
- 0x0F,0x90, 0x01, 0xFF, 0x03};
+static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
+ 0x03,0xF4, 0x01, 0xFF, 0x00,
+ 0x0D,0x50, 0x00, 0xFF, 0x00,
+ 0x0E,0xB0, 0x03, 0xFF, 0x00};
-static char TAB12[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
- 0x04,0xC0, 0x00, 0xFF, 0x03,
- 0x07,0x4D, 0x01, 0xFF, 0x03,
- 0x0F,0x90, 0x01, 0xFF, 0x03};
+static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
+ 0x03,0xE9, 0x00, 0xFF, 0x03,
+ 0x0D,0xF4, 0x01, 0xFF, 0x03,
+ 0x0E,0xF4, 0x01, 0xFF, 0x03};
-static char TAB13[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee
- 0x04,0x15, 0x02, 0xFF, 0x03,
- 0x07,0xC0, 0x00, 0xFF, 0x03,
- 0x0F,0x05, 0x02, 0xFF, 0x03};
+static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
+ 0x03,0xE9, 0x00, 0xFF, 0x03,
+ 0x0D,0xB5, 0x00, 0xFF, 0x03,
+ 0x0E,0x39, 0x03, 0xFF, 0x03};
-static char TAB14[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement
- 0x04,0x30, 0x01, 0xFF, 0x03,
- 0x07,0xC0, 0x00, 0xFF, 0x03,
- 0x0F,0x05, 0x02, 0xFF, 0x03};
+static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
+ 0x03,0x00, 0x02, 0xFF, 0x03,
+ 0x0D,0xB5, 0x00, 0xFF, 0x03,
+ 0x0E,0x39, 0x03, 0xFF, 0x03};
+
+static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
+ 0x03,0x60, 0x01, 0xFF, 0x00,
+ 0x0D,0xB5, 0x00, 0xFF, 0x00,
+ 0x0E,0x39, 0x03, 0xFF, 0x00};
+
+static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x03,0x50, 0x01, 0xFF, 0x03,
+ 0x0D,0x0A, 0x01, 0xFF, 0x03,
+ 0x0E,0xDD, 0x02, 0xFF, 0x03};
+
+static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
+ 0x03,0xDE, 0x03, 0xFF, 0x03,
+ 0x0D,0xB5, 0x00, 0xFF, 0x03,
+ 0x0E,0x39, 0x03, 0xFF, 0x03};
-static char TAB15[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
- 0x04,0xC0, 0x00, 0xFF, 0x03,
- 0x07,0xC0, 0x00, 0xFF, 0x03,
- 0x0F,0x05, 0x02, 0xFF, 0x03};
+static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras)
+ 0x03,0x00, 0x03, 0xFF, 0x00,
+ 0x0D,0xB5, 0x00, 0xFF, 0x00,
+ 0x0E,0x39, 0x03, 0xFF, 0x00};
+
+static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte)
+ 0x03,0x00, 0x03, 0xFF, 0x03,
+ 0x0D,0xF4, 0x01, 0xFF, 0x03,
+ 0x0E,0xF4, 0x01, 0xFF, 0x03};
-static char TAB1 6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin
- 0x04,0x4D, 0x01, 0xFF, 0x03,
- 0x07,0x4D, 0x01, 0xFF, 0x03,
- 0x0F,0x90, 0x01, 0xFF, 0x03};
+static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut)
+ 0x03,0x00, 0x02, 0xFF, 0x00,
+ 0x0D,0xB5, 0x00, 0xFF, 0x00,
+ 0x0E,0x39, 0x03, 0xFF, 0x00};
+
+static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
+ 0x03,0x00, 0x03, 0xFF, 0x00,
+ 0x0D,0xB5, 0x00, 0xFF, 0x00,
+ 0x0E,0x39, 0x03, 0xFF, 0x00};
/* ANGLE */
@@ -171,44 +245,29 @@
CANMessage TrameTx = CANMessage();
TrameTx.format=CANStandard;
TrameTx.type=CANData;
-
- /*
- deux_myAX12 = new AX12(p9, p10, 2, 1000000); //main tourneuse
- quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); // main tourneuse
- */
+
+ trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche
+ treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche
+ quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche
+ huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche
- trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince droite
- treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince droite
- quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince droite
- huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince droite
-
- quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince gauche
- sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince gauche
- quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince gauche
- dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince gauche
-
- six_myAX12 = new AX12(p28, p27, 6, 1000000); //funny action
-
+ quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite
+ sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite
+ quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite
+ dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite
+
multiple_myAX12 = new AX12(p13,p14,0xFE,1000000);
multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000);
+ //Initialisation de la position du bras
+ Initialisation_position();
while(true) {
canProcessRx();//Traitement des trames CAN en attente
if (action == 0){
- choix = 1;
- Prendre_modules(choix);
- choix = 2;
- Prendre_modules(choix);
- wait(2);
+
/*
- choix = 1;
- Poser_modules(choix);
- choix = 2;
- Poser_modules(choix);
- wait(2);
-
TrameTx.id = 0x092;
TrameTx.len = 1;
TrameTx.data[0] = 0;
@@ -279,7 +338,6 @@
{
deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
quatre_myAX12-> Set_Goal_speed(vitesse);
- six_myAX12-> Set_Goal_speed(vitesse);
sept_myAX12-> Set_Goal_speed(vitesse);
quatorze_myAX12-> Set_Goal_speed(vitesse);
quinze_myAX12-> Set_Goal_speed(vitesse);
@@ -291,7 +349,6 @@
deux_myAX12-> Set_Mode(0);
quatre_myAX12-> Set_Mode(0);
- six_myAX12-> Set_Mode(0);
sept_myAX12-> Set_Mode(0);
quatorze_myAX12-> Set_Mode(0);
quinze_myAX12-> Set_Mode(0);
@@ -302,79 +359,157 @@
treize_myAX12-> Set_Mode(0);
}
+
/****************************************************************************************/
-/* FUNCTION NAME: prendre_modules */
-/* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */
-/****************************************************************************************/
-void Prendre_modules(unsigned char choix){
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise */
+/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
+/****************************************************************************************/
+void Preparation_prise(unsigned char choix){
if (choix == 1){
- //multiple_myAX12->multiple_goal_and_speed(4,TAB1);
- //wait(TIME);
multiple_myAX12->multiple_goal_and_speed(4,TAB2);
wait(TIME);
+ }
+
+ else if (choix == 2){
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_haut */
+/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
+/****************************************************************************************/
+void Stockage_haut(unsigned char choix){
+ if (choix == 1){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB4);
+ wait(TIME);
multiple_myAX12->multiple_goal_and_speed(4,TAB5);
wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB6);
wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB4);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(5,TAB6);
- wait(TIME);
}
else if (choix == 2){
- //multiple2_myAX12->multiple_goal_and_speed(4,TAB11);
- //wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB12);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB15);
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_bas */
+/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
+/****************************************************************************************/
+void Stockage_bas(unsigned char choix){
+ if (choix == 1){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB13);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB7);
wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB14);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(5,TAB16);
- wait(TIME);
- }
+ }
- SendRawId(0x123);
+ else if (choix == 2){
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
+ wait(TIME);
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Deposer */
+/* DESCRIPTION : Fonction qui permet de déposer le module */
+/****************************************************************************************/
+void Deposer(unsigned char choix){
+ if (choix == 1){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB9);
+ wait(TIME);
+ }
+
+ else if (choix == 2){
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
+ wait(TIME);
+ }
}
/****************************************************************************************/
-/* FUNCTION NAME: Poser_modules */
-/* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */
+/* FUNCTION NAME: Preparation_depos_bas */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
/****************************************************************************************/
-void Poser_modules(unsigned char choix){
- multiple_myAX12->multiple_goal_and_speed(4,TAB4);
- wait(2);
- multiple_myAX12->multiple_goal_and_speed(4,TAB3);
- wait(2);
- multiple_myAX12->multiple_goal_and_speed(4,TAB2);
- wait(2);
- multiple_myAX12->multiple_goal_and_speed(4,TAB1);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Funny_action */
-/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
-/****************************************************************************************/
-void Funny_Action(void){
- six_myAX12 -> Set_Secure_Goal(150);
- wait(0.5);
- six_myAX12 -> Set_Secure_Goal(60);
- wait(0.5);
- six_myAX12 -> Set_Secure_Goal(150);
+void Preparation_depos_bas(unsigned char choix){
+ if (choix == 1){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
+ }
+
+ else if (choix == 2){
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
+ wait(TIME);
+ }
}
/****************************************************************************************/
-/* FUNCTION NAME: Tourner_modules */
-/* DESCRIPTION : Fonction qui permet de deployer la main tourneuse */
+/* FUNCTION NAME: Preparation_depos_haut */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
/****************************************************************************************/
-void Tourner_modules(void){
- multiple2_myAX12 -> multiple_goal_and_speed(2,TAB11);
- wait(1.5);
- multiple2_myAX12 -> multiple_goal_and_speed(2,TAB12);
-}
+void Preparation_depos_haut(unsigned char choix){
+ if (choix == 1){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB6);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB10);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
+ }
-
+ else if (choix == 2){
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
+ wait(TIME);
+ }
+}
+/****************************************************************************************/
+/* FUNCTION NAME: Pousser_module */
+/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
+/****************************************************************************************/
+void Pousser_module(unsigned char choix){
+ if (choix == 1){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB11);
+ wait(TIME);
+ }
+
+ else if (choix == 2){
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
+ wait(TIME);
+ }
+}
+
+
+