CRAC Team / command_AX12_petit_robot_V4tempo

code ax12 petit robot 12/05/2017

Fork of command_AX12_petit_robot_V3 by CRAC Team

main.cpp@3:6cbd7d2c78f4, 2017-04-19 (annotated)

Committer:
SquirrelGod
Date:
Wed Apr 19 15:53:45 2017 +0000
Revision:
3:6cbd7d2c78f4
Parent:
2:99b1cb0d9f5e
Child:
4:888b32bc5b63
AX12 control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mathieulegros 0:a2a44c043932 1 #include "mbed.h"
mathieulegros 0:a2a44c043932 2 #include "AX12.h"
mathieulegros 0:a2a44c043932 3 #include "cmsis.h"
SquirrelGod 2:99b1cb0d9f5e 4 #include "ident_crac.h"
mathieulegros 0:a2a44c043932 5
SquirrelGod 2:99b1cb0d9f5e 6 #define SIZE_FIFO 14
SquirrelGod 2:99b1cb0d9f5e 7 #define TIME 0.8
SquirrelGod 2:99b1cb0d9f5e 8
mathieulegros 0:a2a44c043932 9 /* DECLARATION VARIABLES */
SquirrelGod 2:99b1cb0d9f5e 10 CAN can1(p30,p29);
SquirrelGod 2:99b1cb0d9f5e 11 CANMessage msgRxBuffer[SIZE_FIFO];
SquirrelGod 2:99b1cb0d9f5e 12 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
SquirrelGod 2:99b1cb0d9f5e 13 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
SquirrelGod 2:99b1cb0d9f5e 14
SquirrelGod 2:99b1cb0d9f5e 15 extern "C" void mbed_reset();//Pour pouvoir reset la carte
SquirrelGod 2:99b1cb0d9f5e 16
SquirrelGod 2:99b1cb0d9f5e 17 unsigned char action = 0, choix = 0;
SquirrelGod 2:99b1cb0d9f5e 18 short vitesse=700;
SquirrelGod 2:99b1cb0d9f5e 19 float angle=0.0;
SquirrelGod 2:99b1cb0d9f5e 20 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;
SquirrelGod 3:6cbd7d2c78f4 21 AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
SquirrelGod 2:99b1cb0d9f5e 22
SquirrelGod 2:99b1cb0d9f5e 23
SquirrelGod 2:99b1cb0d9f5e 24 /* PROTOTYPES DE FONCTIONS ET POINTEURS */
SquirrelGod 2:99b1cb0d9f5e 25
SquirrelGod 2:99b1cb0d9f5e 26
SquirrelGod 2:99b1cb0d9f5e 27
SquirrelGod 2:99b1cb0d9f5e 28 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 29 /* FUNCTION NAME: canProcessRx */
SquirrelGod 2:99b1cb0d9f5e 30 /* DESCRIPTION : Fonction de traitement des messages CAN */
SquirrelGod 2:99b1cb0d9f5e 31 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 32 void canProcessRx(void);
SquirrelGod 2:99b1cb0d9f5e 33
SquirrelGod 2:99b1cb0d9f5e 34 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 35 /* FUNCTION NAME: canRx_ISR */
SquirrelGod 2:99b1cb0d9f5e 36 /* DESCRIPTION : Interruption en réception sur le CAN */
SquirrelGod 2:99b1cb0d9f5e 37 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 38 void canRx_ISR (void);
SquirrelGod 2:99b1cb0d9f5e 39
SquirrelGod 2:99b1cb0d9f5e 40 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 41 /* FUNCTION NAME: SendRawId */
SquirrelGod 2:99b1cb0d9f5e 42 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
SquirrelGod 2:99b1cb0d9f5e 43 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 44 void SendRawId (unsigned short id);
SquirrelGod 2:99b1cb0d9f5e 45
SquirrelGod 2:99b1cb0d9f5e 46 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 47 /* FUNCTION NAME: Initialisation_position */
SquirrelGod 3:6cbd7d2c78f4 48 /* DESCRIPTION : Fonction qui place les bras en position verticale */
SquirrelGod 3:6cbd7d2c78f4 49 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 50 void Initialisation_position(void);
SquirrelGod 3:6cbd7d2c78f4 51
SquirrelGod 3:6cbd7d2c78f4 52 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 53 /* FUNCTION NAME: Preparation_prise */
SquirrelGod 3:6cbd7d2c78f4 54 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
SquirrelGod 3:6cbd7d2c78f4 55 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 56 void Preparation_prise(unsigned char choix);
SquirrelGod 3:6cbd7d2c78f4 57
SquirrelGod 3:6cbd7d2c78f4 58 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 59 /* FUNCTION NAME: Stockage_haut */
SquirrelGod 3:6cbd7d2c78f4 60 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
SquirrelGod 3:6cbd7d2c78f4 61 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 62 void Stockage_haut(unsigned char choix);
SquirrelGod 2:99b1cb0d9f5e 63
SquirrelGod 2:99b1cb0d9f5e 64 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 65 /* FUNCTION NAME: Stockage_bas */
SquirrelGod 3:6cbd7d2c78f4 66 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
SquirrelGod 2:99b1cb0d9f5e 67 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 68 void Stockage_bas(unsigned char choix);
SquirrelGod 3:6cbd7d2c78f4 69
SquirrelGod 3:6cbd7d2c78f4 70 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 71 /* FUNCTION NAME: Deposer */
SquirrelGod 3:6cbd7d2c78f4 72 /* DESCRIPTION : Fonction qui permet de déposer un module */
SquirrelGod 3:6cbd7d2c78f4 73 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 74 void Deposer(unsigned char choix);
SquirrelGod 2:99b1cb0d9f5e 75
SquirrelGod 2:99b1cb0d9f5e 76 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 77 /* FUNCTION NAME: Preparation_depos_bas */
SquirrelGod 3:6cbd7d2c78f4 78 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
SquirrelGod 2:99b1cb0d9f5e 79 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 80 void Preparation_depos_bas(unsigned char choix);
SquirrelGod 2:99b1cb0d9f5e 81
SquirrelGod 2:99b1cb0d9f5e 82 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 83 /* FUNCTION NAME: Preparation_depos_haut */
SquirrelGod 3:6cbd7d2c78f4 84 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
SquirrelGod 2:99b1cb0d9f5e 85 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 86 void Preparation_depos_haut(unsigned char choix);
SquirrelGod 3:6cbd7d2c78f4 87
SquirrelGod 3:6cbd7d2c78f4 88 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 89 /* FUNCTION NAME: Pousser_module */
SquirrelGod 3:6cbd7d2c78f4 90 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
SquirrelGod 3:6cbd7d2c78f4 91 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 92 void Pousser_module(unsigned char choix);
SquirrelGod 2:99b1cb0d9f5e 93
mathieulegros 0:a2a44c043932 94 Timer t;
mathieulegros 0:a2a44c043932 95 Ticker flipper;
SquirrelGod 2:99b1cb0d9f5e 96
SquirrelGod 2:99b1cb0d9f5e 97 ///////////TABLEAU PINCE DROITE ///////////////////////////
SquirrelGod 3:6cbd7d2c78f4 98 static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
SquirrelGod 3:6cbd7d2c78f4 99 0x04,0xF4, 0x01, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 100 0x07,0x50, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 101 0x0F,0xB0, 0x03, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 102
SquirrelGod 3:6cbd7d2c78f4 103 static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
SquirrelGod 3:6cbd7d2c78f4 104 0x04,0xE9, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 105 0x07,0xF4, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 106 0x0F,0xF4, 0x01, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 107
SquirrelGod 3:6cbd7d2c78f4 108 static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
SquirrelGod 3:6cbd7d2c78f4 109 0x04,0xE9, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 110 0x07,0xB5, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 111 0x0F,0x39, 0x03, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 112
SquirrelGod 3:6cbd7d2c78f4 113 static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
SquirrelGod 3:6cbd7d2c78f4 114 0x04,0x00, 0x02, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 115 0x07,0xB5, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 116 0x0F,0x39, 0x03, 0xFF, 0x03};
SquirrelGod 3:6cbd7d2c78f4 117
SquirrelGod 3:6cbd7d2c78f4 118 static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
SquirrelGod 3:6cbd7d2c78f4 119 0x04,0x60, 0x01, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 120 0x07,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 121 0x0F,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 3:6cbd7d2c78f4 122
SquirrelGod 3:6cbd7d2c78f4 123 static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
SquirrelGod 3:6cbd7d2c78f4 124 0x04,0x50, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 125 0x07,0x0A, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 126 0x0F,0xDD, 0x02, 0xFF, 0x03};
SquirrelGod 3:6cbd7d2c78f4 127
SquirrelGod 3:6cbd7d2c78f4 128 static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
SquirrelGod 3:6cbd7d2c78f4 129 0x04,0xDE, 0x03, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 130 0x07,0xB5, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 131 0x0F,0x39, 0x03, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 132
SquirrelGod 3:6cbd7d2c78f4 133 static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras)
SquirrelGod 3:6cbd7d2c78f4 134 0x04,0x00, 0x03, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 135 0x07,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 136 0x0F,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 3:6cbd7d2c78f4 137
SquirrelGod 3:6cbd7d2c78f4 138 static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte)
SquirrelGod 3:6cbd7d2c78f4 139 0x04,0x00, 0x03, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 140 0x07,0xF4, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 141 0x0F,0xF4, 0x01, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 142
SquirrelGod 3:6cbd7d2c78f4 143 static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut)
SquirrelGod 3:6cbd7d2c78f4 144 0x04,0x00, 0x02, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 145 0x07,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 146 0x0F,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 3:6cbd7d2c78f4 147
SquirrelGod 3:6cbd7d2c78f4 148 static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
SquirrelGod 3:6cbd7d2c78f4 149 0x04,0x00, 0x03, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 150 0x07,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 151 0x0F,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 2:99b1cb0d9f5e 152
ConstantMBED 1:b3ff77670606 153
SquirrelGod 2:99b1cb0d9f5e 154 //////////////////TABLEAU PINCE GAUCHE /////////////////////////////
SquirrelGod 3:6cbd7d2c78f4 155 static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
SquirrelGod 3:6cbd7d2c78f4 156 0x03,0xF4, 0x01, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 157 0x0D,0x50, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 158 0x0E,0xB0, 0x03, 0xFF, 0x00};
SquirrelGod 2:99b1cb0d9f5e 159
SquirrelGod 3:6cbd7d2c78f4 160 static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
SquirrelGod 3:6cbd7d2c78f4 161 0x03,0xE9, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 162 0x0D,0xF4, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 163 0x0E,0xF4, 0x01, 0xFF, 0x03};
ConstantMBED 1:b3ff77670606 164
SquirrelGod 3:6cbd7d2c78f4 165 static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
SquirrelGod 3:6cbd7d2c78f4 166 0x03,0xE9, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 167 0x0D,0xB5, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 168 0x0E,0x39, 0x03, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 169
SquirrelGod 3:6cbd7d2c78f4 170 static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
SquirrelGod 3:6cbd7d2c78f4 171 0x03,0x00, 0x02, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 172 0x0D,0xB5, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 173 0x0E,0x39, 0x03, 0xFF, 0x03};
SquirrelGod 3:6cbd7d2c78f4 174
SquirrelGod 3:6cbd7d2c78f4 175 static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
SquirrelGod 3:6cbd7d2c78f4 176 0x03,0x60, 0x01, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 177 0x0D,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 178 0x0E,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 3:6cbd7d2c78f4 179
SquirrelGod 3:6cbd7d2c78f4 180 static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
SquirrelGod 3:6cbd7d2c78f4 181 0x03,0x50, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 182 0x0D,0x0A, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 183 0x0E,0xDD, 0x02, 0xFF, 0x03};
SquirrelGod 3:6cbd7d2c78f4 184
SquirrelGod 3:6cbd7d2c78f4 185 static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
SquirrelGod 3:6cbd7d2c78f4 186 0x03,0xDE, 0x03, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 187 0x0D,0xB5, 0x00, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 188 0x0E,0x39, 0x03, 0xFF, 0x03};
ConstantMBED 1:b3ff77670606 189
SquirrelGod 3:6cbd7d2c78f4 190 static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras)
SquirrelGod 3:6cbd7d2c78f4 191 0x03,0x00, 0x03, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 192 0x0D,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 193 0x0E,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 3:6cbd7d2c78f4 194
SquirrelGod 3:6cbd7d2c78f4 195 static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte)
SquirrelGod 3:6cbd7d2c78f4 196 0x03,0x00, 0x03, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 197 0x0D,0xF4, 0x01, 0xFF, 0x03,
SquirrelGod 3:6cbd7d2c78f4 198 0x0E,0xF4, 0x01, 0xFF, 0x03};
SquirrelGod 2:99b1cb0d9f5e 199
SquirrelGod 3:6cbd7d2c78f4 200 static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut)
SquirrelGod 3:6cbd7d2c78f4 201 0x03,0x00, 0x02, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 202 0x0D,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 203 0x0E,0x39, 0x03, 0xFF, 0x00};
SquirrelGod 3:6cbd7d2c78f4 204
SquirrelGod 3:6cbd7d2c78f4 205 static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
SquirrelGod 3:6cbd7d2c78f4 206 0x03,0x00, 0x03, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 207 0x0D,0xB5, 0x00, 0xFF, 0x00,
SquirrelGod 3:6cbd7d2c78f4 208 0x0E,0x39, 0x03, 0xFF, 0x00};
ConstantMBED 1:b3ff77670606 209
SquirrelGod 2:99b1cb0d9f5e 210
mathieulegros 0:a2a44c043932 211 /* ANGLE */
mathieulegros 0:a2a44c043932 212
mathieulegros 0:a2a44c043932 213 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
mathieulegros 0:a2a44c043932 214 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
mathieulegros 0:a2a44c043932 215 30° = 0x64, 0x00 | 130°= 0xB1, 0x01
mathieulegros 0:a2a44c043932 216 40° = 0x85, 0x00 | 140°= 0xD2, 0x01
mathieulegros 0:a2a44c043932 217 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
mathieulegros 0:a2a44c043932 218 60° = 0xC8, 0x00 | 160°= 0x15, 0x02
mathieulegros 0:a2a44c043932 219 70° = 0xE9, 0x00 | 170°= 0x36, 0x02
mathieulegros 0:a2a44c043932 220 80° = 0x0A, 0x01 | 180°= 0x58, 0x02
mathieulegros 0:a2a44c043932 221 90° = 0x2C, 0x01 | 190°= 0x79, 0x02
mathieulegros 0:a2a44c043932 222 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
mathieulegros 0:a2a44c043932 223
ConstantMBED 1:b3ff77670606 224 /* NUMERO AX12 */
ConstantMBED 1:b3ff77670606 225
ConstantMBED 1:b3ff77670606 226 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
ConstantMBED 1:b3ff77670606 227 1 = 0x01 | 10 = 0x0A
ConstantMBED 1:b3ff77670606 228 2 = 0x02 | 11 = 0x0B
ConstantMBED 1:b3ff77670606 229 3 = 0x03 | 12 = 0x0C
ConstantMBED 1:b3ff77670606 230 4 = 0x04 | 13 = 0x0D
ConstantMBED 1:b3ff77670606 231 5 = 0x05 | 14 = 0x0E
ConstantMBED 1:b3ff77670606 232 6 = 0x06 | 15 = 0x0F
ConstantMBED 1:b3ff77670606 233 7 = 0x07 | 16 = 0x10
ConstantMBED 1:b3ff77670606 234 8 = 0x08 | 17 = 0x11 */
mathieulegros 0:a2a44c043932 235
mathieulegros 0:a2a44c043932 236
SquirrelGod 2:99b1cb0d9f5e 237
SquirrelGod 2:99b1cb0d9f5e 238 /* MAIN */
mathieulegros 0:a2a44c043932 239
SquirrelGod 2:99b1cb0d9f5e 240 int main()
mathieulegros 0:a2a44c043932 241 {
SquirrelGod 2:99b1cb0d9f5e 242 can1.frequency(1000000); // fréquence de travail 1Mbit/s
SquirrelGod 2:99b1cb0d9f5e 243 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
SquirrelGod 2:99b1cb0d9f5e 244
SquirrelGod 2:99b1cb0d9f5e 245 CANMessage TrameTx = CANMessage();
SquirrelGod 2:99b1cb0d9f5e 246 TrameTx.format=CANStandard;
SquirrelGod 2:99b1cb0d9f5e 247 TrameTx.type=CANData;
SquirrelGod 3:6cbd7d2c78f4 248
SquirrelGod 3:6cbd7d2c78f4 249 trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche
SquirrelGod 3:6cbd7d2c78f4 250 treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche
SquirrelGod 3:6cbd7d2c78f4 251 quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche
SquirrelGod 3:6cbd7d2c78f4 252 huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche
SquirrelGod 2:99b1cb0d9f5e 253
SquirrelGod 3:6cbd7d2c78f4 254 quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite
SquirrelGod 3:6cbd7d2c78f4 255 sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite
SquirrelGod 3:6cbd7d2c78f4 256 quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite
SquirrelGod 3:6cbd7d2c78f4 257 dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite
SquirrelGod 3:6cbd7d2c78f4 258
SquirrelGod 2:99b1cb0d9f5e 259 multiple_myAX12 = new AX12(p13,p14,0xFE,1000000);
SquirrelGod 2:99b1cb0d9f5e 260 multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000);
SquirrelGod 2:99b1cb0d9f5e 261
SquirrelGod 3:6cbd7d2c78f4 262 //Initialisation de la position du bras
SquirrelGod 3:6cbd7d2c78f4 263 Initialisation_position();
SquirrelGod 2:99b1cb0d9f5e 264
SquirrelGod 2:99b1cb0d9f5e 265 while(true) {
SquirrelGod 2:99b1cb0d9f5e 266 canProcessRx();//Traitement des trames CAN en attente
SquirrelGod 2:99b1cb0d9f5e 267
SquirrelGod 2:99b1cb0d9f5e 268 if (action == 0){
SquirrelGod 3:6cbd7d2c78f4 269
SquirrelGod 2:99b1cb0d9f5e 270 /*
SquirrelGod 2:99b1cb0d9f5e 271 TrameTx.id = 0x092;
SquirrelGod 2:99b1cb0d9f5e 272 TrameTx.len = 1;
SquirrelGod 2:99b1cb0d9f5e 273 TrameTx.data[0] = 0;
SquirrelGod 2:99b1cb0d9f5e 274 can1.write(TrameTx);
SquirrelGod 2:99b1cb0d9f5e 275 */
SquirrelGod 2:99b1cb0d9f5e 276
SquirrelGod 2:99b1cb0d9f5e 277 action = 2;
SquirrelGod 2:99b1cb0d9f5e 278 }
SquirrelGod 2:99b1cb0d9f5e 279 }
SquirrelGod 2:99b1cb0d9f5e 280 }
SquirrelGod 2:99b1cb0d9f5e 281
SquirrelGod 2:99b1cb0d9f5e 282 /* FONCTIONS */
SquirrelGod 2:99b1cb0d9f5e 283
SquirrelGod 2:99b1cb0d9f5e 284 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 285 /* FUNCTION NAME: canProcessRx */
SquirrelGod 2:99b1cb0d9f5e 286 /* DESCRIPTION : Fonction de traitement des messages CAN */
SquirrelGod 2:99b1cb0d9f5e 287 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 288 void canProcessRx(void)
SquirrelGod 2:99b1cb0d9f5e 289 {
SquirrelGod 2:99b1cb0d9f5e 290 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
SquirrelGod 2:99b1cb0d9f5e 291 CANMessage msgTx=CANMessage();
SquirrelGod 2:99b1cb0d9f5e 292 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
SquirrelGod 2:99b1cb0d9f5e 293 if(FIFO_occupation<0)
SquirrelGod 2:99b1cb0d9f5e 294 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 295 if(FIFO_max_occupation<FIFO_occupation)
SquirrelGod 2:99b1cb0d9f5e 296 FIFO_max_occupation=FIFO_occupation;
SquirrelGod 2:99b1cb0d9f5e 297 if(FIFO_occupation!=0) {
SquirrelGod 2:99b1cb0d9f5e 298
SquirrelGod 2:99b1cb0d9f5e 299 switch(msgRxBuffer[FIFO_lecture].id) {
SquirrelGod 2:99b1cb0d9f5e 300 case SERVO_AX12_PROCESS :
SquirrelGod 2:99b1cb0d9f5e 301 action = 1;
SquirrelGod 2:99b1cb0d9f5e 302 break;
SquirrelGod 2:99b1cb0d9f5e 303
SquirrelGod 2:99b1cb0d9f5e 304 case CHECK_ACTIONNEURS:
SquirrelGod 2:99b1cb0d9f5e 305 SendRawId (ALIVE_ACTIONNEURS);
SquirrelGod 2:99b1cb0d9f5e 306 break;
SquirrelGod 2:99b1cb0d9f5e 307
SquirrelGod 2:99b1cb0d9f5e 308 }
SquirrelGod 2:99b1cb0d9f5e 309
SquirrelGod 2:99b1cb0d9f5e 310 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 311 }
SquirrelGod 2:99b1cb0d9f5e 312 }
SquirrelGod 2:99b1cb0d9f5e 313
SquirrelGod 2:99b1cb0d9f5e 314 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 315 /* FUNCTION NAME: canRx_ISR */
SquirrelGod 2:99b1cb0d9f5e 316 /* DESCRIPTION : Interruption en réception sur le CAN */
SquirrelGod 2:99b1cb0d9f5e 317 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 318 void canRx_ISR (void)
SquirrelGod 2:99b1cb0d9f5e 319 {
SquirrelGod 2:99b1cb0d9f5e 320 if (can1.read(msgRxBuffer[FIFO_ecriture])) {
SquirrelGod 2:99b1cb0d9f5e 321 if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
SquirrelGod 2:99b1cb0d9f5e 322 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
SquirrelGod 2:99b1cb0d9f5e 323 }
SquirrelGod 2:99b1cb0d9f5e 324 }
SquirrelGod 2:99b1cb0d9f5e 325
SquirrelGod 2:99b1cb0d9f5e 326 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 327 /* FUNCTION NAME: SendRawId */
SquirrelGod 2:99b1cb0d9f5e 328 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
SquirrelGod 2:99b1cb0d9f5e 329 /****************************************************************************************/
SquirrelGod 2:99b1cb0d9f5e 330 void SendRawId (unsigned short id)
SquirrelGod 2:99b1cb0d9f5e 331 {
SquirrelGod 2:99b1cb0d9f5e 332 CANMessage msgTx=CANMessage();
SquirrelGod 2:99b1cb0d9f5e 333 msgTx.id=id;
SquirrelGod 2:99b1cb0d9f5e 334 msgTx.len=0;
SquirrelGod 2:99b1cb0d9f5e 335 can1.write(msgTx);
SquirrelGod 2:99b1cb0d9f5e 336 }
SquirrelGod 2:99b1cb0d9f5e 337 void Init_AX12() // Initialisation des différents paramétres
SquirrelGod 2:99b1cb0d9f5e 338 {
SquirrelGod 2:99b1cb0d9f5e 339 deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
SquirrelGod 2:99b1cb0d9f5e 340 quatre_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 341 sept_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 342 quatorze_myAX12-> Set_Goal_speed(vitesse);
ConstantMBED 1:b3ff77670606 343 quinze_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 344 dixhuit_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 345 huit_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 346 trois_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 347 quatorze_myAX12-> Set_Goal_speed(vitesse);
SquirrelGod 2:99b1cb0d9f5e 348 treize_myAX12-> Set_Goal_speed(vitesse);
mathieulegros 0:a2a44c043932 349
SquirrelGod 2:99b1cb0d9f5e 350 deux_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 351 quatre_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 352 sept_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 353 quatorze_myAX12-> Set_Mode(0);
ConstantMBED 1:b3ff77670606 354 quinze_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 355 dixhuit_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 356 huit_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 357 trois_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 358 quatorze_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 359 treize_myAX12-> Set_Mode(0);
SquirrelGod 2:99b1cb0d9f5e 360 }
SquirrelGod 2:99b1cb0d9f5e 361
SquirrelGod 3:6cbd7d2c78f4 362
SquirrelGod 2:99b1cb0d9f5e 363 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 364 /* FUNCTION NAME: Initialisation_position */
SquirrelGod 3:6cbd7d2c78f4 365 /* DESCRIPTION : Fonction qui place les bras en position verticale */
SquirrelGod 3:6cbd7d2c78f4 366 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 367 void Initialisation_position(void){
SquirrelGod 3:6cbd7d2c78f4 368 multiple_myAX12->multiple_goal_and_speed(4,TAB1);
SquirrelGod 3:6cbd7d2c78f4 369 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 370 multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
SquirrelGod 3:6cbd7d2c78f4 371 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 372 }
SquirrelGod 3:6cbd7d2c78f4 373
SquirrelGod 3:6cbd7d2c78f4 374 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 375 /* FUNCTION NAME: Preparation_prise */
SquirrelGod 3:6cbd7d2c78f4 376 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
SquirrelGod 3:6cbd7d2c78f4 377 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 378 void Preparation_prise(unsigned char choix){
SquirrelGod 2:99b1cb0d9f5e 379 if (choix == 1){
SquirrelGod 2:99b1cb0d9f5e 380 multiple_myAX12->multiple_goal_and_speed(4,TAB2);
SquirrelGod 2:99b1cb0d9f5e 381 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 382 }
SquirrelGod 3:6cbd7d2c78f4 383
SquirrelGod 3:6cbd7d2c78f4 384 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 385 multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
SquirrelGod 3:6cbd7d2c78f4 386 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 387 }
SquirrelGod 3:6cbd7d2c78f4 388 }
SquirrelGod 3:6cbd7d2c78f4 389
SquirrelGod 3:6cbd7d2c78f4 390 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 391 /* FUNCTION NAME: Stockage_haut */
SquirrelGod 3:6cbd7d2c78f4 392 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
SquirrelGod 3:6cbd7d2c78f4 393 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 394 void Stockage_haut(unsigned char choix){
SquirrelGod 3:6cbd7d2c78f4 395 if (choix == 1){
SquirrelGod 3:6cbd7d2c78f4 396 multiple_myAX12->multiple_goal_and_speed(4,TAB3);
SquirrelGod 3:6cbd7d2c78f4 397 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 398 multiple_myAX12->multiple_goal_and_speed(4,TAB4);
SquirrelGod 3:6cbd7d2c78f4 399 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 400 multiple_myAX12->multiple_goal_and_speed(4,TAB5);
SquirrelGod 2:99b1cb0d9f5e 401 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 402 multiple_myAX12->multiple_goal_and_speed(4,TAB6);
SquirrelGod 2:99b1cb0d9f5e 403 wait(TIME);
SquirrelGod 2:99b1cb0d9f5e 404 }
mathieulegros 0:a2a44c043932 405
SquirrelGod 2:99b1cb0d9f5e 406 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 407 multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
SquirrelGod 3:6cbd7d2c78f4 408 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 409 multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
SquirrelGod 3:6cbd7d2c78f4 410 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 411 multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
SquirrelGod 3:6cbd7d2c78f4 412 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 413 multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
SquirrelGod 2:99b1cb0d9f5e 414 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 415 }
SquirrelGod 3:6cbd7d2c78f4 416 }
SquirrelGod 3:6cbd7d2c78f4 417
SquirrelGod 3:6cbd7d2c78f4 418 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 419 /* FUNCTION NAME: Stockage_bas */
SquirrelGod 3:6cbd7d2c78f4 420 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
SquirrelGod 3:6cbd7d2c78f4 421 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 422 void Stockage_bas(unsigned char choix){
SquirrelGod 3:6cbd7d2c78f4 423 if (choix == 1){
SquirrelGod 3:6cbd7d2c78f4 424 multiple_myAX12->multiple_goal_and_speed(4,TAB3);
SquirrelGod 2:99b1cb0d9f5e 425 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 426 multiple_myAX12->multiple_goal_and_speed(4,TAB7);
SquirrelGod 2:99b1cb0d9f5e 427 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 428 }
SquirrelGod 2:99b1cb0d9f5e 429
SquirrelGod 3:6cbd7d2c78f4 430 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 431 multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
SquirrelGod 3:6cbd7d2c78f4 432 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 433 multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
SquirrelGod 3:6cbd7d2c78f4 434 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 435 }
SquirrelGod 3:6cbd7d2c78f4 436 }
SquirrelGod 3:6cbd7d2c78f4 437
SquirrelGod 3:6cbd7d2c78f4 438 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 439 /* FUNCTION NAME: Deposer */
SquirrelGod 3:6cbd7d2c78f4 440 /* DESCRIPTION : Fonction qui permet de déposer le module */
SquirrelGod 3:6cbd7d2c78f4 441 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 442 void Deposer(unsigned char choix){
SquirrelGod 3:6cbd7d2c78f4 443 if (choix == 1){
SquirrelGod 3:6cbd7d2c78f4 444 multiple_myAX12->multiple_goal_and_speed(4,TAB9);
SquirrelGod 3:6cbd7d2c78f4 445 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 446 }
SquirrelGod 3:6cbd7d2c78f4 447
SquirrelGod 3:6cbd7d2c78f4 448 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 449 multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
SquirrelGod 3:6cbd7d2c78f4 450 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 451 }
SquirrelGod 2:99b1cb0d9f5e 452 }
mathieulegros 0:a2a44c043932 453
SquirrelGod 2:99b1cb0d9f5e 454 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 455 /* FUNCTION NAME: Preparation_depos_bas */
SquirrelGod 3:6cbd7d2c78f4 456 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
SquirrelGod 2:99b1cb0d9f5e 457 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 458 void Preparation_depos_bas(unsigned char choix){
SquirrelGod 3:6cbd7d2c78f4 459 if (choix == 1){
SquirrelGod 3:6cbd7d2c78f4 460 multiple_myAX12->multiple_goal_and_speed(4,TAB8);
SquirrelGod 3:6cbd7d2c78f4 461 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 462 }
SquirrelGod 3:6cbd7d2c78f4 463
SquirrelGod 3:6cbd7d2c78f4 464 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 465 multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
SquirrelGod 3:6cbd7d2c78f4 466 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 467 }
SquirrelGod 2:99b1cb0d9f5e 468 }
SquirrelGod 2:99b1cb0d9f5e 469
SquirrelGod 2:99b1cb0d9f5e 470 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 471 /* FUNCTION NAME: Preparation_depos_haut */
SquirrelGod 3:6cbd7d2c78f4 472 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
SquirrelGod 2:99b1cb0d9f5e 473 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 474 void Preparation_depos_haut(unsigned char choix){
SquirrelGod 3:6cbd7d2c78f4 475 if (choix == 1){
SquirrelGod 3:6cbd7d2c78f4 476 multiple_myAX12->multiple_goal_and_speed(4,TAB6);
SquirrelGod 3:6cbd7d2c78f4 477 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 478 multiple_myAX12->multiple_goal_and_speed(4,TAB5);
SquirrelGod 3:6cbd7d2c78f4 479 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 480 multiple_myAX12->multiple_goal_and_speed(4,TAB10);
SquirrelGod 3:6cbd7d2c78f4 481 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 482 multiple_myAX12->multiple_goal_and_speed(4,TAB8);
SquirrelGod 3:6cbd7d2c78f4 483 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 484 }
mathieulegros 0:a2a44c043932 485
SquirrelGod 3:6cbd7d2c78f4 486 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 487 multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
SquirrelGod 3:6cbd7d2c78f4 488 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 489 multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
SquirrelGod 3:6cbd7d2c78f4 490 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 491 multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
SquirrelGod 3:6cbd7d2c78f4 492 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 493 multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
SquirrelGod 3:6cbd7d2c78f4 494 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 495 }
SquirrelGod 3:6cbd7d2c78f4 496 }
SquirrelGod 2:99b1cb0d9f5e 497
SquirrelGod 3:6cbd7d2c78f4 498 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 499 /* FUNCTION NAME: Pousser_module */
SquirrelGod 3:6cbd7d2c78f4 500 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
SquirrelGod 3:6cbd7d2c78f4 501 /****************************************************************************************/
SquirrelGod 3:6cbd7d2c78f4 502 void Pousser_module(unsigned char choix){
SquirrelGod 3:6cbd7d2c78f4 503 if (choix == 1){
SquirrelGod 3:6cbd7d2c78f4 504 multiple_myAX12->multiple_goal_and_speed(4,TAB11);
SquirrelGod 3:6cbd7d2c78f4 505 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 506 }
SquirrelGod 3:6cbd7d2c78f4 507
SquirrelGod 3:6cbd7d2c78f4 508 else if (choix == 2){
SquirrelGod 3:6cbd7d2c78f4 509 multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
SquirrelGod 3:6cbd7d2c78f4 510 wait(TIME);
SquirrelGod 3:6cbd7d2c78f4 511 }
SquirrelGod 3:6cbd7d2c78f4 512 }
SquirrelGod 3:6cbd7d2c78f4 513
SquirrelGod 3:6cbd7d2c78f4 514
SquirrelGod 3:6cbd7d2c78f4 515