CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

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