CRAC Team / CRAC-Strat_2017_homologation_gros_rob_testAck

homologation gros robot et test avec les ack de la carte a tout faire

Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by CRAC Team

Robots/Strategie_small.cpp@21:7c60d6dfcab5, 2017-05-25 (annotated)

Committer:
ClementBreteau
Date:
Thu May 25 06:36:48 2017 +0000
Revision:
21:7c60d6dfcab5
Parent:
18:cc5fec34ed9c 
homologation gros robot avec test au niveau des ack

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
ClementBreteau 21:7c60d6dfcab5 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 /****************************************************************************************/
antbig 3:19f2285a4757 40 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
ClementBreteau 18:cc5fec34ed9c 41 int retour = 1;
antbig 3:19f2285a4757 42 switch(id) {
ClementBreteau 15:c2fc239e85df 43 case 100: // preparation prise bras central
ClementBreteau 18:cc5fec34ed9c 44 AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_PRISE);
ClementBreteau 15:c2fc239e85df 45 break;
ClementBreteau 15:c2fc239e85df 46 case 101:// stockage haut bras central
ClementBreteau 18:cc5fec34ed9c 47 AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_HAUT);
ClementBreteau 15:c2fc239e85df 48 break;
ClementBreteau 15:c2fc239e85df 49 case 102:// stockage bas bras central
ClementBreteau 18:cc5fec34ed9c 50 AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_BAS);
ClementBreteau 15:c2fc239e85df 51 break;
ClementBreteau 15:c2fc239e85df 52 case 103:// ouvrir la main du bras cental
ClementBreteau 18:cc5fec34ed9c 53 AX12_automate(AX12_PINCE_CENTRALE_DEPOSER);
ClementBreteau 15:c2fc239e85df 54 break;
ClementBreteau 15:c2fc239e85df 55 case 104:// preparation de depot du module bas du bras central
ClementBreteau 18:cc5fec34ed9c 56 AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
ClementBreteau 15:c2fc239e85df 57 break;
ClementBreteau 15:c2fc239e85df 58 case 105:// preparation de depot du module haut du bras central
ClementBreteau 18:cc5fec34ed9c 59 AX12_automate(AX12_PINCE_CENTRALE_DEPOT_HAUT);
ClementBreteau 18:cc5fec34ed9c 60 AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
ClementBreteau 15:c2fc239e85df 61 break;
ClementBreteau 18:cc5fec34ed9c 62 case 106:// on rentre la pince dans le robot
ClementBreteau 18:cc5fec34ed9c 63 AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
ClementBreteau 15:c2fc239e85df 64 break;
ClementBreteau 18:cc5fec34ed9c 65 case 107: // on ferme la pince du robot
ClementBreteau 18:cc5fec34ed9c 66 AX12_automate(AX12_PINCE_CENTRALE_PRISE_MODULE);
ClementBreteau 18:cc5fec34ed9c 67 break;
ClementBreteau 18:cc5fec34ed9c 68
ClementBreteau 15:c2fc239e85df 69 case 110:// ouvrir une porte avant
antbig 11:ed13a480ddca 70 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
ClementBreteau 15:c2fc239e85df 71 if(speed == 1) speed = 0;
ClementBreteau 15:c2fc239e85df 72 else speed = 1;
ClementBreteau 18:cc5fec34ed9c 73 }
ClementBreteau 18:cc5fec34ed9c 74
ClementBreteau 18:cc5fec34ed9c 75 if (speed == 1){
ClementBreteau 18:cc5fec34ed9c 76 AX12_automate(AX12_DROIT_CROC_OUVERT);
ClementBreteau 18:cc5fec34ed9c 77 }else{
ClementBreteau 18:cc5fec34ed9c 78 AX12_automate(AX12_GAUCHE_CROC_OUVERT);
antbig 3:19f2285a4757 79 }
antbig 3:19f2285a4757 80 break;
ClementBreteau 15:c2fc239e85df 81 case 111:// securiser un module avec une porte avant
antbig 11:ed13a480ddca 82 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
ClementBreteau 15:c2fc239e85df 83 if(speed == 1) speed = 0;
ClementBreteau 15:c2fc239e85df 84 else speed = 1;
ClementBreteau 18:cc5fec34ed9c 85 }
ClementBreteau 18:cc5fec34ed9c 86
ClementBreteau 18:cc5fec34ed9c 87 if (speed == 1){
ClementBreteau 18:cc5fec34ed9c 88 AX12_automate(AX12_DROIT_CROC_FERME);
ClementBreteau 18:cc5fec34ed9c 89 }else{
ClementBreteau 18:cc5fec34ed9c 90 AX12_automate(AX12_GAUCHE_CROC_FERME);
antbig 11:ed13a480ddca 91 }
antbig 10:a788d9cf60f2 92 break;
ClementBreteau 15:c2fc239e85df 93 case 112:// ranger le porte avant dans le robot
antbig 11:ed13a480ddca 94 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
ClementBreteau 15:c2fc239e85df 95 if(speed == 1) speed = 0;
ClementBreteau 15:c2fc239e85df 96 else speed = 1;
ClementBreteau 18:cc5fec34ed9c 97 }
ClementBreteau 18:cc5fec34ed9c 98
ClementBreteau 18:cc5fec34ed9c 99 if (speed == 1){
ClementBreteau 18:cc5fec34ed9c 100 AX12_automate(AX12_DROIT_CROC_INITIALE);
ClementBreteau 18:cc5fec34ed9c 101 }else{
ClementBreteau 18:cc5fec34ed9c 102 AX12_automate(AX12_GAUCHE_CROC_INITIALE);
antbig 11:ed13a480ddca 103 }
antbig 11:ed13a480ddca 104 break;
ClementBreteau 15:c2fc239e85df 105 case 120:// poser une main tourneuse
antbig 11:ed13a480ddca 106 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
ClementBreteau 16:7321fb3bb396 107 if(speed == 2) speed = 1;
ClementBreteau 16:7321fb3bb396 108 else speed = 2;
ClementBreteau 16:7321fb3bb396 109 if(angle == 1) angle = 0;
ClementBreteau 16:7321fb3bb396 110 else angle = 1;
antbig 11:ed13a480ddca 111 }
ClementBreteau 16:7321fb3bb396 112
ClementBreteau 16:7321fb3bb396 113 if (speed == 1){
ClementBreteau 18:cc5fec34ed9c 114 AX12_automate(AX12_TOURNANTE_DROIT_PREPARATION);
ClementBreteau 16:7321fb3bb396 115 }else{
ClementBreteau 18:cc5fec34ed9c 116 AX12_automate(AX12_TOURNANTE_GAUCHE_PREPARATION);
ClementBreteau 16:7321fb3bb396 117 }
antbig 5:dcd817534b57 118 break;
antbig 10:a788d9cf60f2 119
ClementBreteau 15:c2fc239e85df 120 case 121:// relever une main tourneuse
ClementBreteau 15:c2fc239e85df 121 if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
ClementBreteau 16:7321fb3bb396 122 if(speed == 2) speed = 1;
ClementBreteau 16:7321fb3bb396 123 else speed = 2;
ClementBreteau 16:7321fb3bb396 124 if(angle == 1) angle = 0;
ClementBreteau 16:7321fb3bb396 125 else angle = 1;
antbig 11:ed13a480ddca 126 }
ClementBreteau 16:7321fb3bb396 127
ClementBreteau 16:7321fb3bb396 128 if (speed == 1){
ClementBreteau 18:cc5fec34ed9c 129 AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
ClementBreteau 16:7321fb3bb396 130 }else{
ClementBreteau 18:cc5fec34ed9c 131 AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
ClementBreteau 16:7321fb3bb396 132 }
ClementBreteau 16:7321fb3bb396 133
antbig 10:a788d9cf60f2 134 break;
antbig 10:a788d9cf60f2 135
ClementBreteau 15:c2fc239e85df 136 case 122:// tourner un module
ClementBreteau 18:cc5fec34ed9c 137 if(InversStrat == 1) {//Si c'est inversé
ClementBreteau 16:7321fb3bb396 138 if(speed == 2) speed = 1;
ClementBreteau 16:7321fb3bb396 139 else speed = 2;
ClementBreteau 16:7321fb3bb396 140 if(angle == 1) angle = 0;
ClementBreteau 16:7321fb3bb396 141 else angle = 1;
antbig 11:ed13a480ddca 142 }
ClementBreteau 16:7321fb3bb396 143
ClementBreteau 18:cc5fec34ed9c 144 if (speed == 1){
ClementBreteau 18:cc5fec34ed9c 145 AX12_automate(AX12_TOURNANTE_DROIT_MODULE);
ClementBreteau 18:cc5fec34ed9c 146 Tourner_module_droit();
ClementBreteau 16:7321fb3bb396 147 }else{
ClementBreteau 18:cc5fec34ed9c 148 AX12_automate(AX12_TOURNANTE_GAUCHE_MODULE);
ClementBreteau 18:cc5fec34ed9c 149 Tourner_module_gauche();
ClementBreteau 18:cc5fec34ed9c 150 }
ClementBreteau 18:cc5fec34ed9c 151 break;
ClementBreteau 18:cc5fec34ed9c 152
ClementBreteau 18:cc5fec34ed9c 153 case 200 :
ClementBreteau 18:cc5fec34ed9c 154 telemetreDistance = dataTelemetre();
ClementBreteau 18:cc5fec34ed9c 155 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 156 telemetreDistance = dataTelemetre();
ClementBreteau 18:cc5fec34ed9c 157 telemetreDistance = telemetreDistance - 170;
ClementBreteau 18:cc5fec34ed9c 158 break;
ClementBreteau 18:cc5fec34ed9c 159
ClementBreteau 18:cc5fec34ed9c 160 case 201 :
ClementBreteau 18:cc5fec34ed9c 161 SendRawId(0x96);
ClementBreteau 18:cc5fec34ed9c 162 retour = 2;
antbig 10:a788d9cf60f2 163 break;
ClementBreteau 15:c2fc239e85df 164
antbig 12:14729d584500 165 case 10://Désactiver le stop
antbig 12:14729d584500 166 isStopEnable = 0;
antbig 12:14729d584500 167 break;
antbig 12:14729d584500 168 case 11://Activer le stop
antbig 12:14729d584500 169 isStopEnable = 1;
antbig 12:14729d584500 170 break;
antbig 12:14729d584500 171 case 20://Désactiver l'asservissement
antbig 12:14729d584500 172 setAsservissementEtat(0);
antbig 12:14729d584500 173 break;
antbig 12:14729d584500 174 case 21://Activer l'asservissement
antbig 12:14729d584500 175 setAsservissementEtat(1);
ClementBreteau 16:7321fb3bb396 176 break;
ClementBreteau 16:7321fb3bb396 177
ClementBreteau 16:7321fb3bb396 178 case 22://Changer la vitesse du robot
ClementBreteau 16:7321fb3bb396 179 SendSpeed(speed,(unsigned short)angle);
ClementBreteau 18:cc5fec34ed9c 180 wait_us(200);
ClementBreteau 16:7321fb3bb396 181 break;
ClementBreteau 16:7321fb3bb396 182
ClementBreteau 16:7321fb3bb396 183 case 30://Action tempo
ClementBreteau 16:7321fb3bb396 184 wait_ms(speed);
antbig 12:14729d584500 185 break;
antbig 10:a788d9cf60f2 186
antbig 3:19f2285a4757 187 default:
ClementBreteau 18:cc5fec34ed9c 188 retour = 0;//L'action n'existe pas, il faut utiliser le CAN
antbig 3:19f2285a4757 189
antbig 3:19f2285a4757 190 }
ClementBreteau 18:cc5fec34ed9c 191 return retour;//L'action est spécifique.
antbig 3:19f2285a4757 192
antbig 3:19f2285a4757 193 }
antbig 3:19f2285a4757 194
antbig 3:19f2285a4757 195 /****************************************************************************************/
antbig 3:19f2285a4757 196 /* FUNCTION NAME: initRobot */
antbig 3:19f2285a4757 197 /* DESCRIPTION : initialiser le robot */
antbig 3:19f2285a4757 198 /****************************************************************************************/
antbig 3:19f2285a4757 199 void initRobot(void) {
antbig 3:19f2285a4757 200 /**
antbig 3:19f2285a4757 201 On enregistre les id des AX12 présent sur la carte
antbig 3:19f2285a4757 202 **/
ClementBreteau 15:c2fc239e85df 203 /*AX12_register(1,AX12_SERIAL1,0x0FF);
antbig 5:dcd817534b57 204 AX12_register(2,AX12_SERIAL1);
antbig 5:dcd817534b57 205 AX12_register(18,AX12_SERIAL1);
antbig 5:dcd817534b57 206 AX12_register(4,AX12_SERIAL2);
antbig 5:dcd817534b57 207 AX12_register(16,AX12_SERIAL2);
ClementBreteau 15:c2fc239e85df 208 AX12_register(17,AX12_SERIAL2,0x0FF);*/
antbig 12:14729d584500 209
antbig 12:14729d584500 210 //runRobotTest();
antbig 12:14729d584500 211
antbig 5:dcd817534b57 212 /*
antbig 5:dcd817534b57 213 AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
antbig 5:dcd817534b57 214 AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
antbig 5:dcd817534b57 215 AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF);
antbig 5:dcd817534b57 216 AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
antbig 5:dcd817534b57 217 AX12_processChange();*/
ClementBreteau 16:7321fb3bb396 218
ClementBreteau 18:cc5fec34ed9c 219 initialisation_AX12();
ClementBreteau 18:cc5fec34ed9c 220
antbig 3:19f2285a4757 221 }
antbig 3:19f2285a4757 222
antbig 4:88431b537477 223 /****************************************************************************************/
antbig 12:14729d584500 224 /* FUNCTION NAME: initRobotActionneur */
antbig 12:14729d584500 225 /* DESCRIPTION : Initialiser la position des actionneurs du robot */
antbig 12:14729d584500 226 /****************************************************************************************/
antbig 12:14729d584500 227 void initRobotActionneur(void)
antbig 12:14729d584500 228 {
ClementBreteau 18:cc5fec34ed9c 229 moteurGauchePWM(0);
ClementBreteau 18:cc5fec34ed9c 230 moteurDroitPWM(0);
ClementBreteau 18:cc5fec34ed9c 231 AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
ClementBreteau 18:cc5fec34ed9c 232 AX12_automate(AX12_GAUCHE_CROC_INITIALE);
ClementBreteau 18:cc5fec34ed9c 233 AX12_automate(AX12_DROIT_CROC_INITIALE);
ClementBreteau 18:cc5fec34ed9c 234 AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
ClementBreteau 18:cc5fec34ed9c 235 AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
antbig 12:14729d584500 236 }
antbig 12:14729d584500 237
antbig 12:14729d584500 238 /****************************************************************************************/
antbig 9:d0042422d95a 239 /* FUNCTION NAME: runTest */
antbig 9:d0042422d95a 240 /* DESCRIPTION : tester l'ensemble des actionneurs du robot */
antbig 9:d0042422d95a 241 /****************************************************************************************/
antbig 9:d0042422d95a 242 void runRobotTest(void)
antbig 9:d0042422d95a 243 {
ClementBreteau 18:cc5fec34ed9c 244
antbig 9:d0042422d95a 245 }
antbig 9:d0042422d95a 246
antbig 9:d0042422d95a 247 /****************************************************************************************/
antbig 4:88431b537477 248 /* FUNCTION NAME: SelectStrategy */
antbig 4:88431b537477 249 /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
antbig 4:88431b537477 250 /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
antbig 4:88431b537477 251 /****************************************************************************************/
antbig 4:88431b537477 252 int SelectStrategy(unsigned char id)
antbig 4:88431b537477 253 {
antbig 12:14729d584500 254
antbig 4:88431b537477 255 switch(id)
antbig 4:88431b537477 256 {
antbig 4:88431b537477 257 case 1:
antbig 4:88431b537477 258 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 4:88431b537477 259 return FileExists(cheminFileStart);
antbig 4:88431b537477 260 case 2:
antbig 4:88431b537477 261 strcpy(cheminFileStart,"/local/strat2.txt");
antbig 4:88431b537477 262 return FileExists(cheminFileStart);
antbig 7:dcce34c7e06e 263 case 3:
antbig 7:dcce34c7e06e 264 strcpy(cheminFileStart,"/local/strat3.txt");
antbig 7:dcce34c7e06e 265 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 266 case 4:
antbig 11:ed13a480ddca 267 strcpy(cheminFileStart,"/local/strat4.txt");
antbig 11:ed13a480ddca 268 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 269 case 5:
antbig 11:ed13a480ddca 270 strcpy(cheminFileStart,"/local/strat5.txt");
antbig 11:ed13a480ddca 271 return FileExists(cheminFileStart);
antbig 12:14729d584500 272 case 6:
antbig 12:14729d584500 273 strcpy(cheminFileStart,"/local/strat6.txt");
antbig 12:14729d584500 274 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 275 case 7:
ClementBreteau 16:7321fb3bb396 276 strcpy(cheminFileStart,"/local/strat7.txt");
ClementBreteau 16:7321fb3bb396 277 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 278 case 8:
ClementBreteau 16:7321fb3bb396 279 strcpy(cheminFileStart,"/local/strat8.txt");
ClementBreteau 16:7321fb3bb396 280 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 281 case 9:
ClementBreteau 16:7321fb3bb396 282 strcpy(cheminFileStart,"/local/strat9.txt");
ClementBreteau 16:7321fb3bb396 283 return FileExists(cheminFileStart);
antbig 12:14729d584500 284 case 10:
ClementBreteau 16:7321fb3bb396 285 strcpy(cheminFileStart,"/local/strat10.txt");
ClementBreteau 16:7321fb3bb396 286 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 287 case 11:
antbig 12:14729d584500 288 strcpy(cheminFileStart,"/local/grand_8.txt");
antbig 12:14729d584500 289 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 290
ClementBreteau 16:7321fb3bb396 291 case 0x10:
ClementBreteau 16:7321fb3bb396 292 strcpy(cheminFileStart,"/local/demoBras.txt");
ClementBreteau 16:7321fb3bb396 293 return FileExists(cheminFileStart);
ClementBreteau 16:7321fb3bb396 294
antbig 4:88431b537477 295 default:
antbig 12:14729d584500 296 strcpy(cheminFileStart,"/local/strat1.txt");
ClementBreteau 16:7321fb3bb396 297 SendRawId(0x258);
antbig 4:88431b537477 298 return 0;
antbig 4:88431b537477 299 }
antbig 4:88431b537477 300 }
antbig 4:88431b537477 301
antbig 12:14729d584500 302 /****************************************************************************************/
antbig 12:14729d584500 303 /* FUNCTION NAME: needToStop */
antbig 12:14729d584500 304 /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
antbig 12:14729d584500 305 /****************************************************************************************/
antbig 12:14729d584500 306 unsigned char needToStop(void)
antbig 12:14729d584500 307 {
antbig 12:14729d584500 308 return isStopEnable;
antbig 12:14729d584500 309 }
antbig 12:14729d584500 310
antbig 12:14729d584500 311 /****************************************************************************************/
antbig 12:14729d584500 312 /* FUNCTION NAME: doBeforeEndAction */
antbig 12:14729d584500 313 /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
antbig 12:14729d584500 314 /****************************************************************************************/
antbig 12:14729d584500 315 void doBeforeEndAction(void)
antbig 12:14729d584500 316 {
antbig 12:14729d584500 317
antbig 12:14729d584500 318 }
antbig 12:14729d584500 319
antbig 3:19f2285a4757 320 #endif