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_small.cpp@34:6aa4b46b102e, 2018-04-26 (annotated)

Committer:
Sitkah
Date:
Thu Apr 26 20:14:18 2018 +0000
Revision:
34:6aa4b46b102e
Parent:
30:a1e37af4bbde
Child:
36:6dd30780bd8e
Ne pas tenir compte si besoin du code 2017

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sitkah 29:41e02746041d 1 #include "global.h"
antbig 3:19f2285a4757 2 #ifdef ROBOT_SMALL
Sitkah 29:41e02746041d 3
antbig 3:19f2285a4757 4
antbig 12:14729d584500 5 unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
ClementBreteau 21:590cdacb6a35 6 //unsigned short telemetreDistance;
ClementBreteau 18:cc5fec34ed9c 7
antbig 12:14729d584500 8
antbig 3:19f2285a4757 9 /****************************************************************************************/
antbig 3:19f2285a4757 10 /* FUNCTION NAME: doFunnyAction */
antbig 3:19f2285a4757 11 /* DESCRIPTION : Permet de faire la funny action en fin de partie */
antbig 3:19f2285a4757 12 /****************************************************************************************/
antbig 3:19f2285a4757 13 void doFunnyAction(void) {
antbig 3:19f2285a4757 14
antbig 3:19f2285a4757 15
antbig 3:19f2285a4757 16 }
antbig 10:a788d9cf60f2 17 //L'angle est entre 0 et 1023
antbig 10:a788d9cf60f2 18 void XL320_setGoal(unsigned char id, unsigned short angle);
antbig 10:a788d9cf60f2 19
antbig 10:a788d9cf60f2 20 void XL320_setGoal(unsigned char id, unsigned short angle)
antbig 10:a788d9cf60f2 21 {
antbig 10:a788d9cf60f2 22 CANMessage msgTx=CANMessage();
antbig 10:a788d9cf60f2 23 msgTx.id=SERVO_XL320;
antbig 10:a788d9cf60f2 24 msgTx.len=3;
antbig 10:a788d9cf60f2 25 msgTx.format=CANStandard;
antbig 10:a788d9cf60f2 26 msgTx.type=CANData;
antbig 10:a788d9cf60f2 27 msgTx.data[0]=(unsigned char)id;
antbig 10:a788d9cf60f2 28 // from sur 2 octets
antbig 10:a788d9cf60f2 29 msgTx.data[1]=(unsigned char)angle;
antbig 10:a788d9cf60f2 30 msgTx.data[2]=(unsigned char)(angle>>8);
antbig 10:a788d9cf60f2 31
antbig 10:a788d9cf60f2 32 can1.write(msgTx);
antbig 10:a788d9cf60f2 33 }
antbig 3:19f2285a4757 34
ClementBreteau 18:cc5fec34ed9c 35
antbig 3:19f2285a4757 36 /****************************************************************************************/
antbig 3:19f2285a4757 37 /* FUNCTION NAME: doAction */
antbig 3:19f2285a4757 38 /* DESCRIPTION : Effectuer une action specifique */
antbig 3:19f2285a4757 39 /****************************************************************************************/
Sitkah 34:6aa4b46b102e 40 unsigned char doAction(unsigned char id, unsigned short arg1, short arg2) {
ClementBreteau 18:cc5fec34ed9c 41 int retour = 1;
Sitkah 34:6aa4b46b102e 42 CANMessage msgTx=CANMessage();
Sitkah 34:6aa4b46b102e 43 msgTx.format=CANStandard;
Sitkah 34:6aa4b46b102e 44 msgTx.type=CANData;
antbig 3:19f2285a4757 45 switch(id) {
Sitkah 30:a1e37af4bbde 46 case 101: //baisser attrape bloc avant
Sitkah 30:a1e37af4bbde 47 SendRawId(BAISSER_ATTRAPE_BLOC);
Sitkah 30:a1e37af4bbde 48 break;
Sitkah 30:a1e37af4bbde 49 case 102: //Relever attrape bloc avant
Sitkah 30:a1e37af4bbde 50 SendRawId(RELEVER_ATTRAPE_BLOC);
Sitkah 30:a1e37af4bbde 51 break;
Sitkah 30:a1e37af4bbde 52 case 115: //allumer panneau
Sitkah 30:a1e37af4bbde 53 SendRawId(ALLUMER_PANNEAU_UP);
Sitkah 30:a1e37af4bbde 54 break;
Sitkah 30:a1e37af4bbde 55 case 116: //range le bras qui allume le panneau
Sitkah 30:a1e37af4bbde 56 SendRawId(ALLUMER_PANNEAU_DOWN);
Sitkah 30:a1e37af4bbde 57 break;
Sitkah 30:a1e37af4bbde 58 case 117: //lève le bras pour pousser l'abeille
Sitkah 30:a1e37af4bbde 59 SendRawId(BRAS_ABEILLE_UP);
Sitkah 30:a1e37af4bbde 60 break;
Sitkah 30:a1e37af4bbde 61 case 118: //abaisse le bras qui pousse l'abeille
Sitkah 30:a1e37af4bbde 62 SendRawId(BRAS_ABEILLE_DOWN);
Sitkah 30:a1e37af4bbde 63 break;
Sitkah 30:a1e37af4bbde 64 case 130: //Inclinaison pour viser le chateau d'eau
Sitkah 30:a1e37af4bbde 65 SendRawId(INCLINAISON_CHATEAU);
Sitkah 30:a1e37af4bbde 66 break;
Sitkah 30:a1e37af4bbde 67 case 131: //inclinaison pour deverser les balles sales
Sitkah 30:a1e37af4bbde 68 SendRawId(INCLINAISON_EPURATION);
Sitkah 30:a1e37af4bbde 69 break;
Sitkah 30:a1e37af4bbde 70 case 132: //blocage des balles
Sitkah 30:a1e37af4bbde 71 SendRawId(BLOCAGE_BALLE);
Sitkah 30:a1e37af4bbde 72 break;
Sitkah 30:a1e37af4bbde 73 case 133: //lance le pwm de tir
Sitkah 34:6aa4b46b102e 74 msgTx.id=LANCEMENT_MOTEUR_TIR_ON;
Sitkah 34:6aa4b46b102e 75
Sitkah 34:6aa4b46b102e 76 msgTx.len=1;
Sitkah 34:6aa4b46b102e 77 msgTx.data[0]=arg1;
Sitkah 34:6aa4b46b102e 78 can2.write(msgTx);
Sitkah 30:a1e37af4bbde 79 break;
Sitkah 30:a1e37af4bbde 80 case 134: ///Arrete le pwm de tir
Sitkah 30:a1e37af4bbde 81 SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
Sitkah 30:a1e37af4bbde 82 break;
Sitkah 30:a1e37af4bbde 83 case 135://position aiguilleur au centre
Sitkah 30:a1e37af4bbde 84 SendRawId(AIGUILLEUR_CENTRE);
Sitkah 30:a1e37af4bbde 85 break;
Sitkah 30:a1e37af4bbde 86 case 136://position aiguilleur à gauche
Sitkah 30:a1e37af4bbde 87 SendRawId(AIGUILLEUR_GAUCHE);
Sitkah 30:a1e37af4bbde 88 break;
Sitkah 30:a1e37af4bbde 89 case 137://position aiguilleur à droite
Sitkah 30:a1e37af4bbde 90 SendRawId(AIGUILLEUR_DROITE);
Sitkah 30:a1e37af4bbde 91 break;
Sitkah 30:a1e37af4bbde 92 case 138://position aiguilleur au centre
Sitkah 30:a1e37af4bbde 93 SendRawId(TRI_BALLE);
Sitkah 30:a1e37af4bbde 94 break;
Sitkah 34:6aa4b46b102e 95 case 139:
Sitkah 34:6aa4b46b102e 96 SendRawId(VIBRO);
Sitkah 34:6aa4b46b102e 97 break;
Sitkah 34:6aa4b46b102e 98
ClementBreteau 18:cc5fec34ed9c 99
ClementBreteau 16:7321fb3bb396 100
ClementBreteau 18:cc5fec34ed9c 101 case 200 :
Sitkah 30:a1e37af4bbde 102 SendRawId(DATA_TELEMETRE);
Sitkah 30:a1e37af4bbde 103 /*telemetreDistance = dataTelemetre();
ClementBreteau 18:cc5fec34ed9c 104 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 105 telemetreDistance = dataTelemetre();
Sitkah 30:a1e37af4bbde 106 telemetreDistance = telemetreDistance - 170;*/
ClementBreteau 18:cc5fec34ed9c 107 break;
ClementBreteau 18:cc5fec34ed9c 108
ClementBreteau 18:cc5fec34ed9c 109 case 201 :
Sitkah 30:a1e37af4bbde 110 SendRawId(0x99);
ClementBreteau 18:cc5fec34ed9c 111 retour = 2;
antbig 10:a788d9cf60f2 112 break;
ClementBreteau 15:c2fc239e85df 113
antbig 12:14729d584500 114 case 10://Désactiver le stop
antbig 12:14729d584500 115 isStopEnable = 0;
antbig 12:14729d584500 116 break;
antbig 12:14729d584500 117 case 11://Activer le stop
antbig 12:14729d584500 118 isStopEnable = 1;
antbig 12:14729d584500 119 break;
antbig 12:14729d584500 120 case 20://Désactiver l'asservissement
antbig 12:14729d584500 121 setAsservissementEtat(0);
antbig 12:14729d584500 122 break;
antbig 12:14729d584500 123 case 21://Activer l'asservissement
antbig 12:14729d584500 124 setAsservissementEtat(1);
ClementBreteau 16:7321fb3bb396 125 break;
ClementBreteau 16:7321fb3bb396 126
Sitkah 34:6aa4b46b102e 127 case 22://Changer la vitesse du robot
Sitkah 34:6aa4b46b102e 128 SendSpeed(arg1,(unsigned short)arg2);
ClementBreteau 18:cc5fec34ed9c 129 wait_us(200);
antbig 28:acd18776ed2d 130 waitingAckFrom = 0;
antbig 28:acd18776ed2d 131 waitingAckID = 0;
antbig 28:acd18776ed2d 132 break;
antbig 28:acd18776ed2d 133
antbig 28:acd18776ed2d 134 case 19: // CHANGER LA VITESSE + DECELERATION
Sitkah 34:6aa4b46b102e 135 SendSpeedDecel(arg1,(unsigned short) arg2);
antbig 28:acd18776ed2d 136 wait_us(200);
antbig 28:acd18776ed2d 137 waitingAckFrom = 0;
antbig 28:acd18776ed2d 138 waitingAckID =0;
ClementBreteau 16:7321fb3bb396 139 break;
ClementBreteau 16:7321fb3bb396 140
ClementBreteau 16:7321fb3bb396 141 case 30://Action tempo
Sitkah 34:6aa4b46b102e 142 wait_ms(arg1);
antbig 12:14729d584500 143 break;
Sitkah 34:6aa4b46b102e 144
antbig 3:19f2285a4757 145 default:
ClementBreteau 18:cc5fec34ed9c 146 retour = 0;//L'action n'existe pas, il faut utiliser le CAN
antbig 3:19f2285a4757 147
antbig 3:19f2285a4757 148 }
ClementBreteau 18:cc5fec34ed9c 149 return retour;//L'action est spécifique.
antbig 3:19f2285a4757 150
antbig 3:19f2285a4757 151 }
antbig 3:19f2285a4757 152
antbig 3:19f2285a4757 153 /****************************************************************************************/
antbig 3:19f2285a4757 154 /* FUNCTION NAME: initRobot */
antbig 3:19f2285a4757 155 /* DESCRIPTION : initialiser le robot */
antbig 3:19f2285a4757 156 /****************************************************************************************/
Sitkah 30:a1e37af4bbde 157 //void initRobot(void) {
antbig 3:19f2285a4757 158 /**
antbig 3:19f2285a4757 159 On enregistre les id des AX12 présent sur la carte
antbig 3:19f2285a4757 160 **/
ClementBreteau 15:c2fc239e85df 161 /*AX12_register(1,AX12_SERIAL1,0x0FF);
antbig 5:dcd817534b57 162 AX12_register(2,AX12_SERIAL1);
antbig 5:dcd817534b57 163 AX12_register(18,AX12_SERIAL1);
antbig 5:dcd817534b57 164 AX12_register(4,AX12_SERIAL2);
antbig 5:dcd817534b57 165 AX12_register(16,AX12_SERIAL2);
ClementBreteau 15:c2fc239e85df 166 AX12_register(17,AX12_SERIAL2,0x0FF);*/
antbig 12:14729d584500 167
antbig 12:14729d584500 168 //runRobotTest();
antbig 12:14729d584500 169
antbig 5:dcd817534b57 170 /*
antbig 5:dcd817534b57 171 AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
antbig 5:dcd817534b57 172 AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
antbig 5:dcd817534b57 173 AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF);
antbig 5:dcd817534b57 174 AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
antbig 5:dcd817534b57 175 AX12_processChange();*/
ClementBreteau 16:7321fb3bb396 176
Sitkah 30:a1e37af4bbde 177 //initialisation_AX12();
ClementBreteau 18:cc5fec34ed9c 178
Sitkah 30:a1e37af4bbde 179 //}
antbig 3:19f2285a4757 180
antbig 4:88431b537477 181 /****************************************************************************************/
antbig 12:14729d584500 182 /* FUNCTION NAME: initRobotActionneur */
antbig 12:14729d584500 183 /* DESCRIPTION : Initialiser la position des actionneurs du robot */
antbig 12:14729d584500 184 /****************************************************************************************/
Sitkah 30:a1e37af4bbde 185 /*void initRobotActionneur(void)
antbig 12:14729d584500 186 {
ClementBreteau 18:cc5fec34ed9c 187 moteurGauchePWM(0);
ClementBreteau 18:cc5fec34ed9c 188 moteurDroitPWM(0);
ClementBreteau 18:cc5fec34ed9c 189 AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
ClementBreteau 18:cc5fec34ed9c 190 AX12_automate(AX12_GAUCHE_CROC_INITIALE);
ClementBreteau 18:cc5fec34ed9c 191 AX12_automate(AX12_DROIT_CROC_INITIALE);
ClementBreteau 18:cc5fec34ed9c 192 AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
ClementBreteau 18:cc5fec34ed9c 193 AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
antbig 12:14729d584500 194 }
Sitkah 30:a1e37af4bbde 195 */
antbig 12:14729d584500 196 /****************************************************************************************/
antbig 9:d0042422d95a 197 /* FUNCTION NAME: runTest */
antbig 9:d0042422d95a 198 /* DESCRIPTION : tester l'ensemble des actionneurs du robot */
antbig 9:d0042422d95a 199 /****************************************************************************************/
antbig 9:d0042422d95a 200 void runRobotTest(void)
antbig 9:d0042422d95a 201 {
ClementBreteau 18:cc5fec34ed9c 202
antbig 9:d0042422d95a 203 }
antbig 9:d0042422d95a 204
antbig 9:d0042422d95a 205 /****************************************************************************************/
antbig 4:88431b537477 206 /* FUNCTION NAME: SelectStrategy */
antbig 4:88431b537477 207 /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
antbig 4:88431b537477 208 /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
antbig 4:88431b537477 209 /****************************************************************************************/
antbig 4:88431b537477 210 int SelectStrategy(unsigned char id)
antbig 4:88431b537477 211 {
antbig 12:14729d584500 212
antbig 4:88431b537477 213 switch(id)
antbig 4:88431b537477 214 {
antbig 4:88431b537477 215 case 1:
Sitkah 29:41e02746041d 216 strcpy(cheminFileStart,"/sd/strat1.txt");
antbig 4:88431b537477 217 return FileExists(cheminFileStart);
antbig 4:88431b537477 218 case 2:
Sitkah 29:41e02746041d 219 strcpy(cheminFileStart,"/sd/strat2.txt");
antbig 4:88431b537477 220 return FileExists(cheminFileStart);
antbig 7:dcce34c7e06e 221 case 3:
Sitkah 29:41e02746041d 222 strcpy(cheminFileStart,"/sd/strat3.txt");
antbig 7:dcce34c7e06e 223 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 224 case 4:
Sitkah 29:41e02746041d 225 strcpy(cheminFileStart,"/sd/strat4.txt");
antbig 11:ed13a480ddca 226 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 227 case 5:
Sitkah 29:41e02746041d 228 strcpy(cheminFileStart,"/sd/strat5.txt");
antbig 11:ed13a480ddca 229 return FileExists(cheminFileStart);
antbig 12:14729d584500 230 case 6:
Sitkah 29:41e02746041d 231 strcpy(cheminFileStart,"/sd/strat6.txt");
antbig 12:14729d584500 232 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 233 case 7:
Sitkah 29:41e02746041d 234 strcpy(cheminFileStart,"/sd/strat7.txt");
ClementBreteau 16:7321fb3bb396 235 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 236 case 8:
Sitkah 29:41e02746041d 237 strcpy(cheminFileStart,"/sd/strat8.txt");
ClementBreteau 16:7321fb3bb396 238 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 239 case 9:
Sitkah 29:41e02746041d 240 strcpy(cheminFileStart,"/sd/strat9.txt");
ClementBreteau 16:7321fb3bb396 241 return FileExists(cheminFileStart);
antbig 12:14729d584500 242 case 10:
Sitkah 29:41e02746041d 243 strcpy(cheminFileStart,"/sd/strat10.txt");
ClementBreteau 16:7321fb3bb396 244 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 245 case 11:
Sitkah 29:41e02746041d 246 strcpy(cheminFileStart,"/sd/grand_8.txt");
antbig 12:14729d584500 247 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 248
ClementBreteau 16:7321fb3bb396 249 case 0x10:
Sitkah 29:41e02746041d 250 strcpy(cheminFileStart,"/sd/demoBras.txt");
ClementBreteau 16:7321fb3bb396 251 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 252
antbig 4:88431b537477 253 default:
Sitkah 29:41e02746041d 254 strcpy(cheminFileStart,"/sd/strat1.txt");
ClementBreteau 16:7321fb3bb396 255 SendRawId(0x258);
antbig 4:88431b537477 256 return 0;
antbig 4:88431b537477 257 }
antbig 4:88431b537477 258 }
antbig 4:88431b537477 259
antbig 12:14729d584500 260 /****************************************************************************************/
antbig 12:14729d584500 261 /* FUNCTION NAME: needToStop */
antbig 12:14729d584500 262 /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
antbig 12:14729d584500 263 /****************************************************************************************/
antbig 12:14729d584500 264 unsigned char needToStop(void)
antbig 12:14729d584500 265 {
antbig 12:14729d584500 266 return isStopEnable;
antbig 12:14729d584500 267 }
antbig 12:14729d584500 268
antbig 12:14729d584500 269 /****************************************************************************************/
antbig 12:14729d584500 270 /* FUNCTION NAME: doBeforeEndAction */
antbig 12:14729d584500 271 /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
antbig 12:14729d584500 272 /****************************************************************************************/
antbig 12:14729d584500 273 void doBeforeEndAction(void)
antbig 12:14729d584500 274 {
antbig 12:14729d584500 275
antbig 12:14729d584500 276 }
antbig 12:14729d584500 277
antbig 3:19f2285a4757 278 #endif