CRAC Team / CRAC-Strat_2017_fin_premier_match

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CRAC-Strat_2017_homologation_petit_rob by CRAC Team

Robots/Strategie_big.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:
14:c8fc06c4887f
1Version utilis? lors du match 5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 1:116040d14164 1 #include "StrategieManager.h"
antbig 3:19f2285a4757 2 #ifdef ROBOT_BIG
antbig 3:19f2285a4757 3 #include "Config_big.h"
antbig 0:ad97421fb1fb 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 0:ad97421fb1fb 7 /****************************************************************************************/
antbig 0:ad97421fb1fb 8 /* FUNCTION NAME: doFunnyAction */
antbig 0:ad97421fb1fb 9 /* DESCRIPTION : Permet de faire la funny action en fin de partie */
antbig 0:ad97421fb1fb 10 /****************************************************************************************/
antbig 0:ad97421fb1fb 11 void doFunnyAction(void) {
antbig 12:14729d584500 12 AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
antbig 9:d0042422d95a 13 AX12_processChange();
antbig 0:ad97421fb1fb 14
antbig 0:ad97421fb1fb 15 }
antbig 0:ad97421fb1fb 16
antbig 0:ad97421fb1fb 17 /****************************************************************************************/
antbig 0:ad97421fb1fb 18 /* FUNCTION NAME: doAction */
antbig 0:ad97421fb1fb 19 /* DESCRIPTION : Effectuer une action specifique */
antbig 0:ad97421fb1fb 20 /****************************************************************************************/
antbig 0:ad97421fb1fb 21 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
antbig 12:14729d584500 22 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 23 switch(id) {
antbig 3:19f2285a4757 24
antbig 12:14729d584500 25 case 100://Ouvrir les portes avant
antbig 12:14729d584500 26 AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE);
antbig 12:14729d584500 27 AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE);
antbig 12:14729d584500 28 AX12_processChange();
antbig 12:14729d584500 29 break;
antbig 12:14729d584500 30 case 101://Fermer les portes avant
antbig 12:14729d584500 31 AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER);
antbig 12:14729d584500 32 AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER);
antbig 12:14729d584500 33 AX12_processChange();
antbig 12:14729d584500 34 break;
antbig 12:14729d584500 35
antbig 12:14729d584500 36 case 102://Remonter le peigne
antbig 12:14729d584500 37 AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
antbig 12:14729d584500 38 AX12_processChange();
antbig 12:14729d584500 39 break;
antbig 12:14729d584500 40 case 103://Descendre le peigne
antbig 12:14729d584500 41 AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
antbig 12:14729d584500 42 AX12_processChange();
antbig 12:14729d584500 43 break;
antbig 12:14729d584500 44
antbig 12:14729d584500 45 case 104://Monter le support ventouse haut
antbig 12:14729d584500 46 AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP);
antbig 12:14729d584500 47 AX12_processChange();
antbig 12:14729d584500 48 break;
antbig 12:14729d584500 49 case 105://Descendre le support ventouse haut
antbig 12:14729d584500 50 AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN);
antbig 12:14729d584500 51 AX12_processChange();
antbig 12:14729d584500 52 break;
antbig 12:14729d584500 53 case 106://Remonter le support du cone arriere
antbig 12:14729d584500 54 AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
antbig 12:14729d584500 55 AX12_processChange();
antbig 12:14729d584500 56 break;
antbig 12:14729d584500 57 case 107://Descendre le support du cone arriere
antbig 12:14729d584500 58 AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
antbig 12:14729d584500 59 AX12_processChange();
antbig 12:14729d584500 60 break;
antbig 12:14729d584500 61
antbig 8:0edc7dfb7f7e 62 case 110://Ouvrir la pince arrière haute
antbig 8:0edc7dfb7f7e 63 AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
antbig 8:0edc7dfb7f7e 64 AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
antbig 8:0edc7dfb7f7e 65 AX12_processChange();
antbig 8:0edc7dfb7f7e 66 break;
antbig 8:0edc7dfb7f7e 67 case 111://Fermer la pince arrière haute
antbig 8:0edc7dfb7f7e 68 AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145);
antbig 8:0edc7dfb7f7e 69 AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
antbig 8:0edc7dfb7f7e 70 AX12_processChange();
antbig 12:14729d584500 71 /*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE;
antbig 12:14729d584500 72 waitingAckFrom = SERVO_AX12_DONE;*/
antbig 8:0edc7dfb7f7e 73 break;
antbig 9:d0042422d95a 74
antbig 8:0edc7dfb7f7e 75 case 112://Ouvrir la pince arrière basse
antbig 12:14729d584500 76 AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215);
antbig 12:14729d584500 77 AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85);
antbig 9:d0042422d95a 78 AX12_processChange();
antbig 8:0edc7dfb7f7e 79 break;
antbig 8:0edc7dfb7f7e 80 case 113://Fermer la pince arrière basse
antbig 9:d0042422d95a 81 AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
antbig 9:d0042422d95a 82 AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
antbig 9:d0042422d95a 83 AX12_processChange();
antbig 12:14729d584500 84 /* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE;
antbig 12:14729d584500 85 waitingAckFrom = SERVO_AX12_DONE;*/
antbig 9:d0042422d95a 86 break;
antbig 8:0edc7dfb7f7e 87
antbig 8:0edc7dfb7f7e 88 case 114://Ouvrir les portes arrières
antbig 8:0edc7dfb7f7e 89 AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
antbig 8:0edc7dfb7f7e 90 AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
antbig 8:0edc7dfb7f7e 91 AX12_processChange();
antbig 8:0edc7dfb7f7e 92 break;
antbig 8:0edc7dfb7f7e 93 case 115://Fermer les portes arrière
antbig 12:14729d584500 94 //AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
antbig 12:14729d584500 95 //AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
antbig 12:14729d584500 96 AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142);
antbig 12:14729d584500 97 AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158);
antbig 12:14729d584500 98 AX12_processChange();
antbig 12:14729d584500 99 //waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE;
antbig 12:14729d584500 100 //waitingAckFrom = SERVO_AX12_DONE;
antbig 12:14729d584500 101 break;
antbig 12:14729d584500 102
antbig 12:14729d584500 103 case 120://Activer les pompes
antbig 12:14729d584500 104 AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
antbig 8:0edc7dfb7f7e 105 AX12_processChange();
antbig 12:14729d584500 106
antbig 12:14729d584500 107
antbig 12:14729d584500 108 msgTx.id=POMPE_PWM;
antbig 12:14729d584500 109 msgTx.format=CANStandard;
antbig 12:14729d584500 110 msgTx.type=CANData;
antbig 12:14729d584500 111 msgTx.len=6;
antbig 12:14729d584500 112
antbig 12:14729d584500 113 // x sur 2 octets
antbig 12:14729d584500 114 msgTx.data[0]=(unsigned char)POMPES_PWM;
antbig 12:14729d584500 115 msgTx.data[1]=(unsigned char)POMPES_PWM;
antbig 12:14729d584500 116 msgTx.data[2]=(unsigned char)POMPES_PWM;
antbig 12:14729d584500 117 msgTx.data[3]=(unsigned char)POMPES_PWM;
antbig 12:14729d584500 118 msgTx.data[4]=(unsigned char)POMPES_PWM;
antbig 12:14729d584500 119 msgTx.data[5]=(unsigned char)POMPES_PWM;
antbig 12:14729d584500 120
antbig 12:14729d584500 121 can1.write(msgTx);
antbig 8:0edc7dfb7f7e 122 break;
antbig 12:14729d584500 123 case 121://Désactiver les pompes
antbig 12:14729d584500 124 msgTx.id=POMPE_PWM;
antbig 12:14729d584500 125 msgTx.format=CANStandard;
antbig 12:14729d584500 126 msgTx.type=CANData;
antbig 12:14729d584500 127 msgTx.len=6;
antbig 12:14729d584500 128
antbig 12:14729d584500 129 // x sur 2 octets
antbig 12:14729d584500 130 msgTx.data[0]=(unsigned char)0;
antbig 12:14729d584500 131 msgTx.data[1]=(unsigned char)0;
antbig 12:14729d584500 132 msgTx.data[2]=(unsigned char)0;
antbig 12:14729d584500 133 msgTx.data[3]=(unsigned char)0;
antbig 12:14729d584500 134 msgTx.data[4]=(unsigned char)0;
antbig 12:14729d584500 135 msgTx.data[5]=(unsigned char)0;
antbig 12:14729d584500 136
antbig 12:14729d584500 137 can1.write(msgTx);
antbig 12:14729d584500 138 break;
antbig 12:14729d584500 139
antbig 12:14729d584500 140 case 10://Désactiver le stop
antbig 12:14729d584500 141 isStopEnable = 0;
antbig 12:14729d584500 142 break;
antbig 12:14729d584500 143 case 11://Activer le stop
antbig 12:14729d584500 144 isStopEnable = 1;
antbig 12:14729d584500 145 break;
antbig 12:14729d584500 146 case 20://Désactiver l'asservissement
antbig 12:14729d584500 147 setAsservissementEtat(0);
antbig 12:14729d584500 148 break;
antbig 12:14729d584500 149 case 21://Activer l'asservissement
antbig 12:14729d584500 150 setAsservissementEtat(1);
antbig 12:14729d584500 151 break;
antbig 12:14729d584500 152
antbig 12:14729d584500 153 case 22://Changer la vitesse du robot
antbig 12:14729d584500 154 SendSpeed(speed,(unsigned short)angle);
antbig 12:14729d584500 155 break;
antbig 12:14729d584500 156
antbig 12:14729d584500 157 case 30://Action tempo
antbig 12:14729d584500 158 wait_ms(speed);
antbig 12:14729d584500 159 break;
antbig 12:14729d584500 160
antbig 12:14729d584500 161 case 130://Lancer mouvement de sortie de la zone de départ
antbig 12:14729d584500 162 SendRawId(ACTION_BIG_DEMARRAGE);
antbig 12:14729d584500 163 waitingAckID = ACTION_BIG_DEMARRAGE;
antbig 12:14729d584500 164 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 12:14729d584500 165 break;
antbig 12:14729d584500 166
antbig 0:ad97421fb1fb 167 default:
antbig 0:ad97421fb1fb 168 return 0;//L'action n'existe pas, il faut utiliser le CAN
antbig 0:ad97421fb1fb 169
antbig 0:ad97421fb1fb 170 }
antbig 0:ad97421fb1fb 171 return 1;//L'action est spécifique.
antbig 0:ad97421fb1fb 172
antbig 0:ad97421fb1fb 173 }
antbig 0:ad97421fb1fb 174
antbig 0:ad97421fb1fb 175 /****************************************************************************************/
antbig 0:ad97421fb1fb 176 /* FUNCTION NAME: initRobot */
antbig 0:ad97421fb1fb 177 /* DESCRIPTION : initialiser le robot */
antbig 0:ad97421fb1fb 178 /****************************************************************************************/
antbig 9:d0042422d95a 179 void initRobot(void)
antbig 9:d0042422d95a 180 {
antbig 9:d0042422d95a 181 //Enregistrement de tous les AX12 présent sur la carte
antbig 8:0edc7dfb7f7e 182 AX12_register(5, AX12_SERIAL2);
antbig 8:0edc7dfb7f7e 183 AX12_register(18, AX12_SERIAL2);
antbig 8:0edc7dfb7f7e 184 AX12_register(13, AX12_SERIAL2);
antbig 12:14729d584500 185 AX12_register(1, AX12_SERIAL1);
antbig 12:14729d584500 186 AX12_register(11, AX12_SERIAL1);
antbig 12:14729d584500 187 AX12_register(8, AX12_SERIAL1);
antbig 12:14729d584500 188 AX12_register(7, AX12_SERIAL2);
antbig 9:d0042422d95a 189
antbig 12:14729d584500 190 //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
antbig 12:14729d584500 191 //AX12_processChange();
antbig 11:ed13a480ddca 192 //runRobotTest();
antbig 9:d0042422d95a 193 }
antbig 9:d0042422d95a 194
antbig 9:d0042422d95a 195 /****************************************************************************************/
antbig 12:14729d584500 196 /* FUNCTION NAME: initRobotActionneur */
antbig 12:14729d584500 197 /* DESCRIPTION : Initialiser la position des actionneurs du robot */
antbig 12:14729d584500 198 /****************************************************************************************/
antbig 12:14729d584500 199 void initRobotActionneur(void)
antbig 12:14729d584500 200 {
antbig 12:14729d584500 201 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 202 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 203 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 204 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 205 doAction(102,0,0);//Remonter le peigne
antbig 12:14729d584500 206 doAction(106,0,0);//Remonter le support du cone arriere
antbig 12:14729d584500 207 }
antbig 12:14729d584500 208
antbig 12:14729d584500 209 /****************************************************************************************/
antbig 9:d0042422d95a 210 /* FUNCTION NAME: runTest */
antbig 9:d0042422d95a 211 /* DESCRIPTION : tester l'ensemble des actionneurs du robot */
antbig 9:d0042422d95a 212 /****************************************************************************************/
antbig 9:d0042422d95a 213 void runRobotTest(void)
antbig 9:d0042422d95a 214 {
antbig 12:14729d584500 215 int waitTime = 500;
antbig 12:14729d584500 216
antbig 9:d0042422d95a 217 //Test des AX12 dans l'ordre
antbig 9:d0042422d95a 218 doAction(111,0,0);//Fermeture pince arrière haute
antbig 12:14729d584500 219 wait_ms(waitTime);
antbig 9:d0042422d95a 220 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 221 wait_ms(waitTime);
antbig 9:d0042422d95a 222 doAction(113,0,0);//Fermeture pince arrière basse
antbig 12:14729d584500 223 wait_ms(waitTime);
antbig 9:d0042422d95a 224 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 225 wait_ms(waitTime);
antbig 9:d0042422d95a 226 doAction(115,0,0);//Fermeture porte arrière
antbig 12:14729d584500 227 wait_ms(waitTime);
antbig 9:d0042422d95a 228 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 229 wait_ms(waitTime);
antbig 12:14729d584500 230 doAction(101,0,0);//Fermer les portes avant
antbig 12:14729d584500 231 wait_ms(waitTime);
antbig 12:14729d584500 232 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 233 wait_ms(waitTime);
antbig 12:14729d584500 234 doAction(103,0,0);//Descendre le peigne
antbig 12:14729d584500 235 wait_ms(waitTime);
antbig 12:14729d584500 236 doAction(102,0,0);//Remonter le peigne
antbig 0:ad97421fb1fb 237 }
antbig 3:19f2285a4757 238
antbig 4:88431b537477 239 /****************************************************************************************/
antbig 4:88431b537477 240 /* FUNCTION NAME: SelectStrategy */
antbig 4:88431b537477 241 /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
antbig 4:88431b537477 242 /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
antbig 4:88431b537477 243 /****************************************************************************************/
antbig 4:88431b537477 244 int SelectStrategy(unsigned char id)
antbig 4:88431b537477 245 {
antbig 4:88431b537477 246 switch(id)
antbig 4:88431b537477 247 {
antbig 4:88431b537477 248 case 1:
antbig 11:ed13a480ddca 249 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 11:ed13a480ddca 250 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 251 case 2:
antbig 11:ed13a480ddca 252 strcpy(cheminFileStart,"/local/strat2.txt");
antbig 11:ed13a480ddca 253 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 254 case 3:
antbig 11:ed13a480ddca 255 strcpy(cheminFileStart,"/local/strat3.txt");
antbig 11:ed13a480ddca 256 return FileExists(cheminFileStart);
antbig 12:14729d584500 257 case 4:
antbig 12:14729d584500 258 strcpy(cheminFileStart,"/local/strat4.txt");
antbig 12:14729d584500 259 return FileExists(cheminFileStart);
antbig 12:14729d584500 260 case 5:
antbig 12:14729d584500 261 strcpy(cheminFileStart,"/local/strat5.txt");
antbig 12:14729d584500 262 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 263 default:
antbig 12:14729d584500 264 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 11:ed13a480ddca 265 return 0;
antbig 4:88431b537477 266 }
antbig 4:88431b537477 267 }
antbig 4:88431b537477 268
antbig 12:14729d584500 269 /****************************************************************************************/
antbig 12:14729d584500 270 /* FUNCTION NAME: needToStop */
antbig 12:14729d584500 271 /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
antbig 12:14729d584500 272 /****************************************************************************************/
antbig 12:14729d584500 273 unsigned char needToStop(void)
antbig 12:14729d584500 274 {
antbig 12:14729d584500 275 return isStopEnable;
antbig 12:14729d584500 276 }
antbig 12:14729d584500 277
antbig 12:14729d584500 278 /****************************************************************************************/
antbig 12:14729d584500 279 /* FUNCTION NAME: doBeforeEndAction */
antbig 12:14729d584500 280 /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
antbig 12:14729d584500 281 /****************************************************************************************/
antbig 12:14729d584500 282 void doBeforeEndAction(void)
antbig 12:14729d584500 283 {
antbig 12:14729d584500 284 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 285 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 286 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 287 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 288 doAction(102,0,0);//Remonter le peigne
antbig 12:14729d584500 289 }
antbig 12:14729d584500 290
antbig 3:19f2285a4757 291 #endif