CRAC Team / CRAC-Strat_2017_fin_premier_match

code de start qui marche a la fin du premier match, base pour la suite

Fork of CRAC-Strat_2017_homologation_petit_rob by CRAC Team

Robots/Strategie_small.cpp@12:14729d584500, 2016-05-09 (annotated)

Committer:
antbig
Date:
Mon May 09 09:10:17 2016 +0000
Revision:
12:14729d584500
Parent:
11:ed13a480ddca
Child:
15:c2fc239e85df
1Version utilis? lors du match 5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 3:19f2285a4757 1 #include "StrategieManager.h"
antbig 3:19f2285a4757 2 #ifdef ROBOT_SMALL
antbig 3:19f2285a4757 3 #include "Config_small.h"
antbig 3:19f2285a4757 4
antbig 12:14729d584500 5 unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
antbig 12:14729d584500 6
antbig 3:19f2285a4757 7 /****************************************************************************************/
antbig 3:19f2285a4757 8 /* FUNCTION NAME: doFunnyAction */
antbig 3:19f2285a4757 9 /* DESCRIPTION : Permet de faire la funny action en fin de partie */
antbig 3:19f2285a4757 10 /****************************************************************************************/
antbig 3:19f2285a4757 11 void doFunnyAction(void) {
antbig 3:19f2285a4757 12
antbig 3:19f2285a4757 13
antbig 3:19f2285a4757 14 }
antbig 10:a788d9cf60f2 15 //L'angle est entre 0 et 1023
antbig 10:a788d9cf60f2 16 void XL320_setGoal(unsigned char id, unsigned short angle);
antbig 10:a788d9cf60f2 17
antbig 10:a788d9cf60f2 18 void XL320_setGoal(unsigned char id, unsigned short angle)
antbig 10:a788d9cf60f2 19 {
antbig 10:a788d9cf60f2 20 CANMessage msgTx=CANMessage();
antbig 10:a788d9cf60f2 21 msgTx.id=SERVO_XL320;
antbig 10:a788d9cf60f2 22 msgTx.len=3;
antbig 10:a788d9cf60f2 23 msgTx.format=CANStandard;
antbig 10:a788d9cf60f2 24 msgTx.type=CANData;
antbig 10:a788d9cf60f2 25 msgTx.data[0]=(unsigned char)id;
antbig 10:a788d9cf60f2 26 // from sur 2 octets
antbig 10:a788d9cf60f2 27 msgTx.data[1]=(unsigned char)angle;
antbig 10:a788d9cf60f2 28 msgTx.data[2]=(unsigned char)(angle>>8);
antbig 10:a788d9cf60f2 29
antbig 10:a788d9cf60f2 30 can1.write(msgTx);
antbig 10:a788d9cf60f2 31 }
antbig 3:19f2285a4757 32
antbig 3:19f2285a4757 33 /****************************************************************************************/
antbig 3:19f2285a4757 34 /* FUNCTION NAME: doAction */
antbig 3:19f2285a4757 35 /* DESCRIPTION : Effectuer une action specifique */
antbig 3:19f2285a4757 36 /****************************************************************************************/
antbig 3:19f2285a4757 37 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
antbig 3:19f2285a4757 38 switch(id) {
antbig 10:a788d9cf60f2 39 case 101://Descendre le bras pour pecher les poissons
antbig 11:ed13a480ddca 40 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 41 AX12_setGoal(
antbig 11:ed13a480ddca 42 AX12_ID_BRAS_BASE_DROIT,
antbig 11:ed13a480ddca 43 AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
antbig 11:ed13a480ddca 44 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 45 );
antbig 12:14729d584500 46 AX12_setGoal(
antbig 12:14729d584500 47 AX12_ID_BRAS_RELACHEUR_DROIT,
antbig 12:14729d584500 48 AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
antbig 12:14729d584500 49 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 50 );
antbig 3:19f2285a4757 51 AX12_processChange();
antbig 11:ed13a480ddca 52 waitingAckID = AX12_ID_BRAS_BASE_DROIT;
antbig 11:ed13a480ddca 53 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 54 } else {
antbig 11:ed13a480ddca 55 AX12_setGoal(
antbig 11:ed13a480ddca 56 AX12_ID_BRAS_BASE_GAUCHE,
antbig 11:ed13a480ddca 57 AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
antbig 11:ed13a480ddca 58 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 59 );
antbig 12:14729d584500 60 AX12_setGoal(
antbig 12:14729d584500 61 AX12_ID_BRAS_RELACHEUR_GAUCHE,
antbig 12:14729d584500 62 AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
antbig 12:14729d584500 63 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 64 );
antbig 5:dcd817534b57 65 AX12_processChange();
antbig 11:ed13a480ddca 66 waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
antbig 11:ed13a480ddca 67 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 68 }
antbig 3:19f2285a4757 69 break;
antbig 10:a788d9cf60f2 70 case 102://Remonter le bras avec les poissons dessus
antbig 11:ed13a480ddca 71 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 72 AX12_setGoal(
antbig 11:ed13a480ddca 73 AX12_ID_BRAS_BASE_DROIT,
antbig 11:ed13a480ddca 74 AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
antbig 11:ed13a480ddca 75 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 76 );
antbig 3:19f2285a4757 77 AX12_processChange();
antbig 11:ed13a480ddca 78 waitingAckID = AX12_ID_BRAS_BASE_DROIT;
antbig 11:ed13a480ddca 79 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 80 } else {
antbig 11:ed13a480ddca 81 AX12_setGoal(
antbig 11:ed13a480ddca 82 AX12_ID_BRAS_BASE_GAUCHE,
antbig 11:ed13a480ddca 83 AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
antbig 11:ed13a480ddca 84 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 85 );
antbig 5:dcd817534b57 86 AX12_processChange();
antbig 11:ed13a480ddca 87 waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
antbig 11:ed13a480ddca 88 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 89 }
antbig 3:19f2285a4757 90 break;
antbig 10:a788d9cf60f2 91 case 103://Decendre à 30°
antbig 11:ed13a480ddca 92 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 93 AX12_setGoal(
antbig 11:ed13a480ddca 94 AX12_ID_BRAS_BASE_DROIT,
antbig 11:ed13a480ddca 95 AX12_ANGLE_BRAS_BASE_DROIT_RELACHER,
antbig 11:ed13a480ddca 96 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 97 );
antbig 3:19f2285a4757 98 AX12_processChange();
antbig 11:ed13a480ddca 99 waitingAckID = AX12_ID_BRAS_BASE_DROIT;
antbig 11:ed13a480ddca 100 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 101 } else {
antbig 11:ed13a480ddca 102 AX12_setGoal(
antbig 11:ed13a480ddca 103 AX12_ID_BRAS_BASE_GAUCHE,
antbig 11:ed13a480ddca 104 AX12_ANGLE_BRAS_BASE_GAUCHE_RELACHER,
antbig 11:ed13a480ddca 105 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 106 );
antbig 5:dcd817534b57 107 AX12_processChange();
antbig 11:ed13a480ddca 108 waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
antbig 11:ed13a480ddca 109 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 110 }
antbig 3:19f2285a4757 111 break;
antbig 10:a788d9cf60f2 112 case 104://ouvrir séparateur
antbig 11:ed13a480ddca 113 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 114 AX12_setGoal(
antbig 11:ed13a480ddca 115 AX12_ID_BRAS_RELACHEUR_DROIT,
antbig 11:ed13a480ddca 116 AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
antbig 11:ed13a480ddca 117 AX12_SPEED_BRAS_RELACHEUR
antbig 11:ed13a480ddca 118 );
antbig 3:19f2285a4757 119 AX12_processChange();
antbig 11:ed13a480ddca 120 waitingAckID = AX12_ID_BRAS_RELACHEUR_DROIT;
antbig 11:ed13a480ddca 121 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 122 } else {
antbig 11:ed13a480ddca 123 AX12_setGoal(
antbig 11:ed13a480ddca 124 AX12_ID_BRAS_RELACHEUR_GAUCHE,
antbig 11:ed13a480ddca 125 AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
antbig 11:ed13a480ddca 126 AX12_SPEED_BRAS_RELACHEUR
antbig 11:ed13a480ddca 127 );
antbig 5:dcd817534b57 128 AX12_processChange();
antbig 11:ed13a480ddca 129 waitingAckID = AX12_ID_BRAS_RELACHEUR_GAUCHE;
antbig 11:ed13a480ddca 130 waitingAckFrom = SERVO_AX12_DONE;
antbig 3:19f2285a4757 131 }
antbig 3:19f2285a4757 132 break;
antbig 10:a788d9cf60f2 133 case 105://Rentrer le bras dans le robot, fermer le séparateur
antbig 11:ed13a480ddca 134 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 135 AX12_setGoal(
antbig 12:14729d584500 136 AX12_ID_BRAS_RELACHEUR_DROIT,
antbig 12:14729d584500 137 AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
antbig 12:14729d584500 138 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 139 );
antbig 12:14729d584500 140 AX12_processChange();
antbig 12:14729d584500 141 wait_ms(100);
antbig 12:14729d584500 142 AX12_setGoal(
antbig 11:ed13a480ddca 143 AX12_ID_BRAS_BASE_DROIT,
antbig 11:ed13a480ddca 144 AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
antbig 11:ed13a480ddca 145 AX12_SPEED_BRAS_BASE
antbig 11:ed13a480ddca 146 );
antbig 11:ed13a480ddca 147 AX12_processChange();
antbig 11:ed13a480ddca 148 waitingAckID = AX12_ID_BRAS_BASE_DROIT;
antbig 11:ed13a480ddca 149 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 150 } else {
antbig 11:ed13a480ddca 151 AX12_setGoal(
antbig 12:14729d584500 152 AX12_ID_BRAS_RELACHEUR_GAUCHE,
antbig 12:14729d584500 153 AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
antbig 11:ed13a480ddca 154 AX12_SPEED_BRAS_RELACHEUR
antbig 11:ed13a480ddca 155 );
antbig 12:14729d584500 156 AX12_processChange();
antbig 12:14729d584500 157 wait_ms(100);
antbig 11:ed13a480ddca 158 AX12_setGoal(
antbig 12:14729d584500 159 AX12_ID_BRAS_BASE_GAUCHE,
antbig 12:14729d584500 160 AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
antbig 11:ed13a480ddca 161 AX12_SPEED_BRAS_RELACHEUR
antbig 11:ed13a480ddca 162 );
antbig 11:ed13a480ddca 163 AX12_processChange();
antbig 11:ed13a480ddca 164 waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
antbig 11:ed13a480ddca 165 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 166 }
antbig 10:a788d9cf60f2 167 break;
antbig 11:ed13a480ddca 168 case 106://descendre l'attrape coquillages au niveau des rochets gauche si non inversé, droit si inversé
antbig 11:ed13a480ddca 169 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 170 AX12_setGoal(
antbig 11:ed13a480ddca 171 AX12_ID_PALET_DROIT,
antbig 11:ed13a480ddca 172 AX12_ANGLE_PALET_DROIT_ROCHET,
antbig 11:ed13a480ddca 173 AX12_SPEED_PALET
antbig 11:ed13a480ddca 174 );
antbig 11:ed13a480ddca 175 AX12_processChange();
antbig 11:ed13a480ddca 176 waitingAckID = AX12_ID_PALET_DROIT;
antbig 11:ed13a480ddca 177 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 178 } else {
antbig 11:ed13a480ddca 179 AX12_setGoal(
antbig 11:ed13a480ddca 180 AX12_ID_PALET_GAUCHE,
antbig 11:ed13a480ddca 181 AX12_ANGLE_PALET_GAUCHE_ROCHET,
antbig 11:ed13a480ddca 182 AX12_SPEED_PALET
antbig 11:ed13a480ddca 183 );
antbig 11:ed13a480ddca 184 AX12_processChange();
antbig 11:ed13a480ddca 185 waitingAckID = AX12_ID_PALET_GAUCHE;
antbig 11:ed13a480ddca 186 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 187 }
antbig 11:ed13a480ddca 188 break;
antbig 11:ed13a480ddca 189 case 107://descendre l'attrape coquillages au niveau de la table gauche si non inversé, droit si inversé
antbig 11:ed13a480ddca 190 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 191 AX12_setGoal(
antbig 11:ed13a480ddca 192 AX12_ID_PALET_DROIT,
antbig 11:ed13a480ddca 193 AX12_ANGLE_PALET_DROIT_OUVERT,
antbig 11:ed13a480ddca 194 AX12_SPEED_PALET
antbig 11:ed13a480ddca 195 );
antbig 11:ed13a480ddca 196 AX12_processChange();
antbig 11:ed13a480ddca 197 waitingAckID = AX12_ID_PALET_DROIT;
antbig 11:ed13a480ddca 198 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 199 } else {
antbig 11:ed13a480ddca 200 AX12_setGoal(
antbig 11:ed13a480ddca 201 AX12_ID_PALET_GAUCHE,
antbig 11:ed13a480ddca 202 AX12_ANGLE_PALET_GAUCHE_OUVERT,
antbig 11:ed13a480ddca 203 AX12_SPEED_PALET
antbig 11:ed13a480ddca 204 );
antbig 11:ed13a480ddca 205 AX12_processChange();
antbig 11:ed13a480ddca 206 waitingAckID = AX12_ID_PALET_GAUCHE;
antbig 11:ed13a480ddca 207 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 208 }
antbig 11:ed13a480ddca 209 break;
antbig 11:ed13a480ddca 210 case 108://Remonter l'attrape coquillages gauche si non inversé, droit si inversé
antbig 11:ed13a480ddca 211 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
antbig 11:ed13a480ddca 212 AX12_setGoal(
antbig 11:ed13a480ddca 213 AX12_ID_PALET_DROIT,
antbig 11:ed13a480ddca 214 AX12_ANGLE_PALET_DROIT_FERMER,
antbig 11:ed13a480ddca 215 AX12_SPEED_PALET
antbig 11:ed13a480ddca 216 );
antbig 11:ed13a480ddca 217 AX12_processChange();
antbig 11:ed13a480ddca 218 waitingAckID = AX12_ID_PALET_DROIT;
antbig 11:ed13a480ddca 219 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 220 } else {
antbig 11:ed13a480ddca 221 AX12_setGoal(
antbig 11:ed13a480ddca 222 AX12_ID_PALET_GAUCHE,
antbig 11:ed13a480ddca 223 AX12_ANGLE_PALET_GAUCHE_FERMER,
antbig 11:ed13a480ddca 224 AX12_SPEED_PALET
antbig 11:ed13a480ddca 225 );
antbig 11:ed13a480ddca 226 AX12_processChange();
antbig 11:ed13a480ddca 227 waitingAckID = AX12_ID_PALET_GAUCHE;
antbig 11:ed13a480ddca 228 waitingAckFrom = SERVO_AX12_DONE;
antbig 11:ed13a480ddca 229 }
antbig 5:dcd817534b57 230 break;
antbig 10:a788d9cf60f2 231
antbig 11:ed13a480ddca 232
antbig 10:a788d9cf60f2 233 case 200://ouvir la pince gauche
antbig 11:ed13a480ddca 234 if(InversStrat == 1) {
antbig 12:14729d584500 235 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
antbig 11:ed13a480ddca 236 } else {
antbig 12:14729d584500 237 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
antbig 11:ed13a480ddca 238 }
antbig 10:a788d9cf60f2 239 break;
antbig 10:a788d9cf60f2 240 case 205://Sécuriser le palet gauche
antbig 11:ed13a480ddca 241 if(InversStrat == 1) {
antbig 11:ed13a480ddca 242 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
antbig 11:ed13a480ddca 243 } else {
antbig 11:ed13a480ddca 244 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
antbig 11:ed13a480ddca 245 }
antbig 10:a788d9cf60f2 246 break;
antbig 10:a788d9cf60f2 247 case 202://fermer pince gauche
antbig 11:ed13a480ddca 248 if(InversStrat == 1) {
antbig 11:ed13a480ddca 249 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
antbig 11:ed13a480ddca 250 } else {
antbig 11:ed13a480ddca 251 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
antbig 11:ed13a480ddca 252 }
antbig 10:a788d9cf60f2 253 break;
antbig 10:a788d9cf60f2 254
antbig 10:a788d9cf60f2 255 case 201://ouvir la pince droite
antbig 11:ed13a480ddca 256 if(InversStrat == 1) {
antbig 11:ed13a480ddca 257 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
antbig 11:ed13a480ddca 258 } else {
antbig 11:ed13a480ddca 259 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
antbig 11:ed13a480ddca 260 }
antbig 10:a788d9cf60f2 261 break;
antbig 10:a788d9cf60f2 262 case 204://Sécuriser le palet droit
antbig 11:ed13a480ddca 263 if(InversStrat == 1) {
antbig 11:ed13a480ddca 264 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
antbig 11:ed13a480ddca 265 } else {
antbig 11:ed13a480ddca 266 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
antbig 11:ed13a480ddca 267 }
antbig 10:a788d9cf60f2 268 break;
antbig 10:a788d9cf60f2 269 case 203://fermer pince droite
antbig 11:ed13a480ddca 270 if(InversStrat == 1) {
antbig 11:ed13a480ddca 271 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
antbig 11:ed13a480ddca 272 } else {
antbig 11:ed13a480ddca 273 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
antbig 11:ed13a480ddca 274 }
antbig 10:a788d9cf60f2 275 break;
antbig 12:14729d584500 276 case 10://Désactiver le stop
antbig 12:14729d584500 277 isStopEnable = 0;
antbig 12:14729d584500 278 break;
antbig 12:14729d584500 279 case 11://Activer le stop
antbig 12:14729d584500 280 isStopEnable = 1;
antbig 12:14729d584500 281 break;
antbig 12:14729d584500 282 case 20://Désactiver l'asservissement
antbig 12:14729d584500 283 setAsservissementEtat(0);
antbig 12:14729d584500 284 break;
antbig 12:14729d584500 285 case 21://Activer l'asservissement
antbig 12:14729d584500 286 setAsservissementEtat(1);
antbig 12:14729d584500 287 break;
antbig 10:a788d9cf60f2 288
antbig 3:19f2285a4757 289 default:
antbig 10:a788d9cf60f2 290 return 0;//L'action n'existe pas, il faut utiliser le CAN
antbig 3:19f2285a4757 291
antbig 3:19f2285a4757 292 }
antbig 11:ed13a480ddca 293 return 1;//L'action est spécifique.
antbig 3:19f2285a4757 294
antbig 3:19f2285a4757 295 }
antbig 3:19f2285a4757 296
antbig 3:19f2285a4757 297 /****************************************************************************************/
antbig 3:19f2285a4757 298 /* FUNCTION NAME: initRobot */
antbig 3:19f2285a4757 299 /* DESCRIPTION : initialiser le robot */
antbig 3:19f2285a4757 300 /****************************************************************************************/
antbig 3:19f2285a4757 301 void initRobot(void) {
antbig 3:19f2285a4757 302 /**
antbig 3:19f2285a4757 303 On enregistre les id des AX12 présent sur la carte
antbig 3:19f2285a4757 304 **/
antbig 5:dcd817534b57 305 AX12_register(1,AX12_SERIAL1,0x0FF);
antbig 5:dcd817534b57 306 AX12_register(2,AX12_SERIAL1);
antbig 5:dcd817534b57 307 AX12_register(18,AX12_SERIAL1);
antbig 5:dcd817534b57 308 AX12_register(4,AX12_SERIAL2);
antbig 5:dcd817534b57 309 AX12_register(16,AX12_SERIAL2);
antbig 5:dcd817534b57 310 AX12_register(17,AX12_SERIAL2,0x0FF);
antbig 12:14729d584500 311
antbig 12:14729d584500 312 //runRobotTest();
antbig 12:14729d584500 313
antbig 5:dcd817534b57 314 /*
antbig 5:dcd817534b57 315 AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
antbig 5:dcd817534b57 316 AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
antbig 5:dcd817534b57 317 AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF);
antbig 5:dcd817534b57 318 AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
antbig 5:dcd817534b57 319 AX12_processChange();*/
antbig 3:19f2285a4757 320 }
antbig 3:19f2285a4757 321
antbig 4:88431b537477 322 /****************************************************************************************/
antbig 12:14729d584500 323 /* FUNCTION NAME: initRobotActionneur */
antbig 12:14729d584500 324 /* DESCRIPTION : Initialiser la position des actionneurs du robot */
antbig 12:14729d584500 325 /****************************************************************************************/
antbig 12:14729d584500 326 void initRobotActionneur(void)
antbig 12:14729d584500 327 {
antbig 12:14729d584500 328 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
antbig 12:14729d584500 329 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
antbig 12:14729d584500 330
antbig 12:14729d584500 331 AX12_setGoal(
antbig 12:14729d584500 332 AX12_ID_BRAS_RELACHEUR_DROIT,
antbig 12:14729d584500 333 AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
antbig 12:14729d584500 334 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 335 );
antbig 12:14729d584500 336 AX12_setGoal(
antbig 12:14729d584500 337 AX12_ID_BRAS_RELACHEUR_GAUCHE,
antbig 12:14729d584500 338 AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
antbig 12:14729d584500 339 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 340 );
antbig 12:14729d584500 341 AX12_setGoal(
antbig 12:14729d584500 342 AX12_ID_BRAS_BASE_DROIT,
antbig 12:14729d584500 343 AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
antbig 12:14729d584500 344 AX12_SPEED_BRAS_BASE
antbig 12:14729d584500 345 );
antbig 12:14729d584500 346 AX12_setGoal(
antbig 12:14729d584500 347 AX12_ID_BRAS_BASE_GAUCHE,
antbig 12:14729d584500 348 AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
antbig 12:14729d584500 349 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 350 );
antbig 12:14729d584500 351 AX12_setGoal(
antbig 12:14729d584500 352 AX12_ID_PALET_DROIT,
antbig 12:14729d584500 353 AX12_ANGLE_PALET_DROIT_FERMER,
antbig 12:14729d584500 354 AX12_SPEED_PALET
antbig 12:14729d584500 355 );
antbig 12:14729d584500 356 AX12_setGoal(
antbig 12:14729d584500 357 AX12_ID_PALET_GAUCHE,
antbig 12:14729d584500 358 AX12_ANGLE_PALET_GAUCHE_FERMER,
antbig 12:14729d584500 359 AX12_SPEED_PALET
antbig 12:14729d584500 360 );
antbig 12:14729d584500 361 AX12_processChange();
antbig 12:14729d584500 362 }
antbig 12:14729d584500 363
antbig 12:14729d584500 364 /****************************************************************************************/
antbig 9:d0042422d95a 365 /* FUNCTION NAME: runTest */
antbig 9:d0042422d95a 366 /* DESCRIPTION : tester l'ensemble des actionneurs du robot */
antbig 9:d0042422d95a 367 /****************************************************************************************/
antbig 9:d0042422d95a 368 void runRobotTest(void)
antbig 9:d0042422d95a 369 {
antbig 12:14729d584500 370 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
antbig 12:14729d584500 371 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
antbig 9:d0042422d95a 372
antbig 12:14729d584500 373 wait_ms(200);
antbig 12:14729d584500 374
antbig 12:14729d584500 375 XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
antbig 12:14729d584500 376 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
antbig 12:14729d584500 377
antbig 12:14729d584500 378 wait_ms(200);
antbig 12:14729d584500 379
antbig 12:14729d584500 380 AX12_setGoal(
antbig 12:14729d584500 381 AX12_ID_BRAS_BASE_DROIT,
antbig 12:14729d584500 382 AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
antbig 12:14729d584500 383 AX12_SPEED_BRAS_BASE
antbig 12:14729d584500 384 );
antbig 12:14729d584500 385 AX12_setGoal(
antbig 12:14729d584500 386 AX12_ID_BRAS_BASE_GAUCHE,
antbig 12:14729d584500 387 AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
antbig 12:14729d584500 388 AX12_SPEED_BRAS_BASE
antbig 12:14729d584500 389 );
antbig 12:14729d584500 390 AX12_processChange();
antbig 12:14729d584500 391
antbig 12:14729d584500 392 wait_ms(500);
antbig 12:14729d584500 393
antbig 12:14729d584500 394 AX12_setGoal(
antbig 12:14729d584500 395 AX12_ID_BRAS_RELACHEUR_DROIT,
antbig 12:14729d584500 396 AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
antbig 12:14729d584500 397 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 398 );
antbig 12:14729d584500 399 AX12_setGoal(
antbig 12:14729d584500 400 AX12_ID_BRAS_RELACHEUR_GAUCHE,
antbig 12:14729d584500 401 AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
antbig 12:14729d584500 402 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 403 );
antbig 12:14729d584500 404 AX12_processChange();
antbig 12:14729d584500 405
antbig 12:14729d584500 406 wait_ms(600);
antbig 12:14729d584500 407
antbig 12:14729d584500 408 AX12_setGoal(
antbig 12:14729d584500 409 AX12_ID_BRAS_RELACHEUR_DROIT,
antbig 12:14729d584500 410 AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
antbig 12:14729d584500 411 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 412 );
antbig 12:14729d584500 413 AX12_setGoal(
antbig 12:14729d584500 414 AX12_ID_BRAS_RELACHEUR_GAUCHE,
antbig 12:14729d584500 415 AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
antbig 12:14729d584500 416 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 417 );
antbig 12:14729d584500 418 AX12_processChange();
antbig 12:14729d584500 419
antbig 12:14729d584500 420 wait_ms(500);
antbig 12:14729d584500 421
antbig 12:14729d584500 422 AX12_setGoal(
antbig 12:14729d584500 423 AX12_ID_BRAS_BASE_DROIT,
antbig 12:14729d584500 424 AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
antbig 12:14729d584500 425 AX12_SPEED_BRAS_BASE
antbig 12:14729d584500 426 );
antbig 12:14729d584500 427 AX12_setGoal(
antbig 12:14729d584500 428 AX12_ID_BRAS_BASE_GAUCHE,
antbig 12:14729d584500 429 AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
antbig 12:14729d584500 430 AX12_SPEED_BRAS_RELACHEUR
antbig 12:14729d584500 431 );
antbig 12:14729d584500 432 AX12_processChange();
antbig 12:14729d584500 433
antbig 12:14729d584500 434 wait_ms(600);
antbig 12:14729d584500 435 AX12_setGoal(
antbig 12:14729d584500 436 AX12_ID_PALET_DROIT,
antbig 12:14729d584500 437 AX12_ANGLE_PALET_DROIT_ROCHET,
antbig 12:14729d584500 438 AX12_SPEED_PALET
antbig 12:14729d584500 439 );
antbig 12:14729d584500 440 AX12_setGoal(
antbig 12:14729d584500 441 AX12_ID_PALET_GAUCHE,
antbig 12:14729d584500 442 AX12_ANGLE_PALET_GAUCHE_ROCHET,
antbig 12:14729d584500 443 AX12_SPEED_PALET
antbig 12:14729d584500 444 );
antbig 12:14729d584500 445 AX12_processChange();
antbig 12:14729d584500 446
antbig 12:14729d584500 447 wait_ms(500);
antbig 12:14729d584500 448
antbig 12:14729d584500 449 AX12_setGoal(
antbig 12:14729d584500 450 AX12_ID_PALET_DROIT,
antbig 12:14729d584500 451 AX12_ANGLE_PALET_DROIT_FERMER,
antbig 12:14729d584500 452 AX12_SPEED_PALET
antbig 12:14729d584500 453 );
antbig 12:14729d584500 454 AX12_setGoal(
antbig 12:14729d584500 455 AX12_ID_PALET_GAUCHE,
antbig 12:14729d584500 456 AX12_ANGLE_PALET_GAUCHE_FERMER,
antbig 12:14729d584500 457 AX12_SPEED_PALET
antbig 12:14729d584500 458 );
antbig 12:14729d584500 459 AX12_processChange();
antbig 9:d0042422d95a 460 }
antbig 9:d0042422d95a 461
antbig 9:d0042422d95a 462 /****************************************************************************************/
antbig 4:88431b537477 463 /* FUNCTION NAME: SelectStrategy */
antbig 4:88431b537477 464 /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
antbig 4:88431b537477 465 /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
antbig 4:88431b537477 466 /****************************************************************************************/
antbig 4:88431b537477 467 int SelectStrategy(unsigned char id)
antbig 4:88431b537477 468 {
antbig 12:14729d584500 469
antbig 12:14729d584500 470
antbig 4:88431b537477 471 switch(id)
antbig 4:88431b537477 472 {
antbig 4:88431b537477 473 case 1:
antbig 4:88431b537477 474 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 4:88431b537477 475 return FileExists(cheminFileStart);
antbig 4:88431b537477 476 case 2:
antbig 4:88431b537477 477 strcpy(cheminFileStart,"/local/strat2.txt");
antbig 4:88431b537477 478 return FileExists(cheminFileStart);
antbig 7:dcce34c7e06e 479 case 3:
antbig 7:dcce34c7e06e 480 strcpy(cheminFileStart,"/local/strat3.txt");
antbig 7:dcce34c7e06e 481 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 482 case 4:
antbig 11:ed13a480ddca 483 strcpy(cheminFileStart,"/local/strat4.txt");
antbig 11:ed13a480ddca 484 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 485 case 5:
antbig 11:ed13a480ddca 486 strcpy(cheminFileStart,"/local/strat5.txt");
antbig 11:ed13a480ddca 487 return FileExists(cheminFileStart);
antbig 12:14729d584500 488 case 6:
antbig 12:14729d584500 489 strcpy(cheminFileStart,"/local/strat6.txt");
antbig 12:14729d584500 490 return FileExists(cheminFileStart);
antbig 12:14729d584500 491 case 10:
antbig 12:14729d584500 492 strcpy(cheminFileStart,"/local/grand_8.txt");
antbig 12:14729d584500 493 return FileExists(cheminFileStart);
antbig 4:88431b537477 494 default:
antbig 12:14729d584500 495 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 4:88431b537477 496 return 0;
antbig 4:88431b537477 497 }
antbig 4:88431b537477 498 }
antbig 4:88431b537477 499
antbig 12:14729d584500 500 /****************************************************************************************/
antbig 12:14729d584500 501 /* FUNCTION NAME: needToStop */
antbig 12:14729d584500 502 /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
antbig 12:14729d584500 503 /****************************************************************************************/
antbig 12:14729d584500 504 unsigned char needToStop(void)
antbig 12:14729d584500 505 {
antbig 12:14729d584500 506 return isStopEnable;
antbig 12:14729d584500 507 }
antbig 12:14729d584500 508
antbig 12:14729d584500 509 /****************************************************************************************/
antbig 12:14729d584500 510 /* FUNCTION NAME: doBeforeEndAction */
antbig 12:14729d584500 511 /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
antbig 12:14729d584500 512 /****************************************************************************************/
antbig 12:14729d584500 513 void doBeforeEndAction(void)
antbig 12:14729d584500 514 {
antbig 12:14729d584500 515
antbig 12:14729d584500 516 }
antbig 12:14729d584500 517
antbig 3:19f2285a4757 518 #endif