Strategie_13h30
Fork of CRAC-Strat_2017_homologation_gros_rob by
Robots/Strategie_small.cpp@16:7321fb3bb396, 2017-05-19 (annotated)
- Committer:
- ClementBreteau
- Date:
- Fri May 19 17:13:46 2017 +0000
- Revision:
- 16:7321fb3bb396
- Parent:
- 15:c2fc239e85df
- Child:
- 18:cc5fec34ed9c
strat du robot, ann?e 2017, le 19 mai ? 19h
Who changed what in which revision?
User | Revision | Line number | New 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) { |
ClementBreteau | 15:c2fc239e85df | 39 | case 100: // preparation prise bras central |
ClementBreteau | 15:c2fc239e85df | 40 | Preparation_prise(); |
ClementBreteau | 15:c2fc239e85df | 41 | break; |
ClementBreteau | 15:c2fc239e85df | 42 | case 101:// stockage haut bras central |
ClementBreteau | 15:c2fc239e85df | 43 | Stockage_haut(); |
ClementBreteau | 15:c2fc239e85df | 44 | break; |
ClementBreteau | 15:c2fc239e85df | 45 | case 102:// stockage bas bras central |
ClementBreteau | 15:c2fc239e85df | 46 | Stockage_bas(); |
ClementBreteau | 15:c2fc239e85df | 47 | break; |
ClementBreteau | 15:c2fc239e85df | 48 | case 103:// ouvrir la main du bras cental |
ClementBreteau | 15:c2fc239e85df | 49 | Deposer(); |
ClementBreteau | 15:c2fc239e85df | 50 | break; |
ClementBreteau | 15:c2fc239e85df | 51 | case 104:// preparation de depot du module bas du bras central |
ClementBreteau | 15:c2fc239e85df | 52 | Preparation_depot_bas(); |
ClementBreteau | 15:c2fc239e85df | 53 | break; |
ClementBreteau | 15:c2fc239e85df | 54 | case 105:// preparation de depot du module haut du bras central |
ClementBreteau | 15:c2fc239e85df | 55 | Preparation_depot_haut(); |
ClementBreteau | 15:c2fc239e85df | 56 | break; |
ClementBreteau | 15:c2fc239e85df | 57 | case 106:// positionner le bras afin de pousser un module debout |
ClementBreteau | 15:c2fc239e85df | 58 | Pousser_module(); |
ClementBreteau | 15:c2fc239e85df | 59 | break; |
ClementBreteau | 15:c2fc239e85df | 60 | case 110:// ouvrir une porte avant |
antbig | 11:ed13a480ddca | 61 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
ClementBreteau | 15:c2fc239e85df | 62 | if(speed == 1) speed = 0; |
ClementBreteau | 15:c2fc239e85df | 63 | else speed = 1; |
antbig | 3:19f2285a4757 | 64 | } else { |
ClementBreteau | 15:c2fc239e85df | 65 | |
antbig | 3:19f2285a4757 | 66 | } |
antbig | 3:19f2285a4757 | 67 | break; |
ClementBreteau | 15:c2fc239e85df | 68 | case 111:// securiser un module avec une porte avant |
antbig | 11:ed13a480ddca | 69 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
ClementBreteau | 15:c2fc239e85df | 70 | if(speed == 1) speed = 0; |
ClementBreteau | 15:c2fc239e85df | 71 | else speed = 1; |
ClementBreteau | 15:c2fc239e85df | 72 | } else { |
ClementBreteau | 15:c2fc239e85df | 73 | |
antbig | 11:ed13a480ddca | 74 | } |
antbig | 10:a788d9cf60f2 | 75 | break; |
ClementBreteau | 15:c2fc239e85df | 76 | case 112:// ranger le porte avant dans le robot |
antbig | 11:ed13a480ddca | 77 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
ClementBreteau | 15:c2fc239e85df | 78 | if(speed == 1) speed = 0; |
ClementBreteau | 15:c2fc239e85df | 79 | else speed = 1; |
antbig | 11:ed13a480ddca | 80 | } else { |
ClementBreteau | 15:c2fc239e85df | 81 | |
antbig | 11:ed13a480ddca | 82 | } |
antbig | 11:ed13a480ddca | 83 | break; |
ClementBreteau | 15:c2fc239e85df | 84 | case 120:// poser une main tourneuse |
antbig | 11:ed13a480ddca | 85 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
ClementBreteau | 16:7321fb3bb396 | 86 | if(speed == 2) speed = 1; |
ClementBreteau | 16:7321fb3bb396 | 87 | else speed = 2; |
ClementBreteau | 16:7321fb3bb396 | 88 | if(angle == 1) angle = 0; |
ClementBreteau | 16:7321fb3bb396 | 89 | else angle = 1; |
antbig | 11:ed13a480ddca | 90 | } |
ClementBreteau | 16:7321fb3bb396 | 91 | |
ClementBreteau | 16:7321fb3bb396 | 92 | if (speed == 1){ |
ClementBreteau | 16:7321fb3bb396 | 93 | //Preparation_module_droit(); |
ClementBreteau | 16:7321fb3bb396 | 94 | }else{ |
ClementBreteau | 16:7321fb3bb396 | 95 | //Preparation_module_gauche(); |
ClementBreteau | 16:7321fb3bb396 | 96 | } |
ClementBreteau | 16:7321fb3bb396 | 97 | Preparation_module_gauche(); |
antbig | 5:dcd817534b57 | 98 | break; |
antbig | 10:a788d9cf60f2 | 99 | |
ClementBreteau | 15:c2fc239e85df | 100 | case 121:// relever une main tourneuse |
ClementBreteau | 15:c2fc239e85df | 101 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
ClementBreteau | 16:7321fb3bb396 | 102 | if(speed == 2) speed = 1; |
ClementBreteau | 16:7321fb3bb396 | 103 | else speed = 2; |
ClementBreteau | 16:7321fb3bb396 | 104 | if(angle == 1) angle = 0; |
ClementBreteau | 16:7321fb3bb396 | 105 | else angle = 1; |
antbig | 11:ed13a480ddca | 106 | } |
ClementBreteau | 16:7321fb3bb396 | 107 | |
ClementBreteau | 16:7321fb3bb396 | 108 | if (speed == 1){ |
ClementBreteau | 16:7321fb3bb396 | 109 | //RangerBrasDroit(); |
ClementBreteau | 16:7321fb3bb396 | 110 | }else{ |
ClementBreteau | 16:7321fb3bb396 | 111 | RangerBrasGauche(); |
ClementBreteau | 16:7321fb3bb396 | 112 | } |
ClementBreteau | 16:7321fb3bb396 | 113 | |
antbig | 10:a788d9cf60f2 | 114 | break; |
antbig | 10:a788d9cf60f2 | 115 | |
ClementBreteau | 15:c2fc239e85df | 116 | case 122:// tourner un module |
ClementBreteau | 15:c2fc239e85df | 117 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
ClementBreteau | 16:7321fb3bb396 | 118 | if(speed == 2) speed = 1; |
ClementBreteau | 16:7321fb3bb396 | 119 | else speed = 2; |
ClementBreteau | 16:7321fb3bb396 | 120 | if(angle == 1) angle = 0; |
ClementBreteau | 16:7321fb3bb396 | 121 | else angle = 1; |
antbig | 11:ed13a480ddca | 122 | } |
ClementBreteau | 16:7321fb3bb396 | 123 | |
ClementBreteau | 16:7321fb3bb396 | 124 | /*if (speed == 1){ |
ClementBreteau | 16:7321fb3bb396 | 125 | //Tourner_module_droite(); |
ClementBreteau | 16:7321fb3bb396 | 126 | }else{ |
ClementBreteau | 16:7321fb3bb396 | 127 | Tourner_module_gauche(); |
ClementBreteau | 16:7321fb3bb396 | 128 | }*/ |
ClementBreteau | 16:7321fb3bb396 | 129 | Tourner_module_gauche(); |
ClementBreteau | 16:7321fb3bb396 | 130 | |
ClementBreteau | 16:7321fb3bb396 | 131 | |
antbig | 10:a788d9cf60f2 | 132 | break; |
ClementBreteau | 15:c2fc239e85df | 133 | |
antbig | 12:14729d584500 | 134 | case 10://Désactiver le stop |
antbig | 12:14729d584500 | 135 | isStopEnable = 0; |
antbig | 12:14729d584500 | 136 | break; |
antbig | 12:14729d584500 | 137 | case 11://Activer le stop |
antbig | 12:14729d584500 | 138 | isStopEnable = 1; |
antbig | 12:14729d584500 | 139 | break; |
antbig | 12:14729d584500 | 140 | case 20://Désactiver l'asservissement |
antbig | 12:14729d584500 | 141 | setAsservissementEtat(0); |
antbig | 12:14729d584500 | 142 | break; |
antbig | 12:14729d584500 | 143 | case 21://Activer l'asservissement |
antbig | 12:14729d584500 | 144 | setAsservissementEtat(1); |
ClementBreteau | 16:7321fb3bb396 | 145 | break; |
ClementBreteau | 16:7321fb3bb396 | 146 | |
ClementBreteau | 16:7321fb3bb396 | 147 | case 22://Changer la vitesse du robot |
ClementBreteau | 16:7321fb3bb396 | 148 | SendSpeed(speed,(unsigned short)angle); |
ClementBreteau | 16:7321fb3bb396 | 149 | break; |
ClementBreteau | 16:7321fb3bb396 | 150 | |
ClementBreteau | 16:7321fb3bb396 | 151 | case 30://Action tempo |
ClementBreteau | 16:7321fb3bb396 | 152 | wait_ms(speed); |
antbig | 12:14729d584500 | 153 | break; |
antbig | 10:a788d9cf60f2 | 154 | |
antbig | 3:19f2285a4757 | 155 | default: |
antbig | 10:a788d9cf60f2 | 156 | return 0;//L'action n'existe pas, il faut utiliser le CAN |
antbig | 3:19f2285a4757 | 157 | |
antbig | 3:19f2285a4757 | 158 | } |
antbig | 11:ed13a480ddca | 159 | return 1;//L'action est spécifique. |
antbig | 3:19f2285a4757 | 160 | |
antbig | 3:19f2285a4757 | 161 | } |
antbig | 3:19f2285a4757 | 162 | |
antbig | 3:19f2285a4757 | 163 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 164 | /* FUNCTION NAME: initRobot */ |
antbig | 3:19f2285a4757 | 165 | /* DESCRIPTION : initialiser le robot */ |
antbig | 3:19f2285a4757 | 166 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 167 | void initRobot(void) { |
antbig | 3:19f2285a4757 | 168 | /** |
antbig | 3:19f2285a4757 | 169 | On enregistre les id des AX12 présent sur la carte |
antbig | 3:19f2285a4757 | 170 | **/ |
ClementBreteau | 15:c2fc239e85df | 171 | /*AX12_register(1,AX12_SERIAL1,0x0FF); |
antbig | 5:dcd817534b57 | 172 | AX12_register(2,AX12_SERIAL1); |
antbig | 5:dcd817534b57 | 173 | AX12_register(18,AX12_SERIAL1); |
antbig | 5:dcd817534b57 | 174 | AX12_register(4,AX12_SERIAL2); |
antbig | 5:dcd817534b57 | 175 | AX12_register(16,AX12_SERIAL2); |
ClementBreteau | 15:c2fc239e85df | 176 | AX12_register(17,AX12_SERIAL2,0x0FF);*/ |
antbig | 12:14729d584500 | 177 | |
antbig | 12:14729d584500 | 178 | //runRobotTest(); |
antbig | 12:14729d584500 | 179 | |
antbig | 5:dcd817534b57 | 180 | /* |
antbig | 5:dcd817534b57 | 181 | AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF); |
antbig | 5:dcd817534b57 | 182 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras |
antbig | 5:dcd817534b57 | 183 | AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF); |
antbig | 5:dcd817534b57 | 184 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras |
antbig | 5:dcd817534b57 | 185 | AX12_processChange();*/ |
ClementBreteau | 16:7321fb3bb396 | 186 | |
ClementBreteau | 16:7321fb3bb396 | 187 | initAX12(); |
ClementBreteau | 16:7321fb3bb396 | 188 | moteurGauchePWM(0); |
ClementBreteau | 16:7321fb3bb396 | 189 | moteurDroitPWM(0); |
antbig | 3:19f2285a4757 | 190 | } |
antbig | 3:19f2285a4757 | 191 | |
antbig | 4:88431b537477 | 192 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 193 | /* FUNCTION NAME: initRobotActionneur */ |
antbig | 12:14729d584500 | 194 | /* DESCRIPTION : Initialiser la position des actionneurs du robot */ |
antbig | 12:14729d584500 | 195 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 196 | void initRobotActionneur(void) |
antbig | 12:14729d584500 | 197 | { |
ClementBreteau | 15:c2fc239e85df | 198 | /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER); |
antbig | 12:14729d584500 | 199 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER); |
antbig | 12:14729d584500 | 200 | |
antbig | 12:14729d584500 | 201 | AX12_setGoal( |
antbig | 12:14729d584500 | 202 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 203 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER, |
antbig | 12:14729d584500 | 204 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 205 | ); |
antbig | 12:14729d584500 | 206 | AX12_setGoal( |
antbig | 12:14729d584500 | 207 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 208 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 209 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 210 | ); |
antbig | 12:14729d584500 | 211 | AX12_setGoal( |
antbig | 12:14729d584500 | 212 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 12:14729d584500 | 213 | AX12_ANGLE_BRAS_BASE_DROIT_REPLIER, |
antbig | 12:14729d584500 | 214 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 215 | ); |
antbig | 12:14729d584500 | 216 | AX12_setGoal( |
antbig | 12:14729d584500 | 217 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 218 | AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 219 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 220 | ); |
antbig | 12:14729d584500 | 221 | AX12_setGoal( |
antbig | 12:14729d584500 | 222 | AX12_ID_PALET_DROIT, |
antbig | 12:14729d584500 | 223 | AX12_ANGLE_PALET_DROIT_FERMER, |
antbig | 12:14729d584500 | 224 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 225 | ); |
antbig | 12:14729d584500 | 226 | AX12_setGoal( |
antbig | 12:14729d584500 | 227 | AX12_ID_PALET_GAUCHE, |
antbig | 12:14729d584500 | 228 | AX12_ANGLE_PALET_GAUCHE_FERMER, |
antbig | 12:14729d584500 | 229 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 230 | ); |
ClementBreteau | 15:c2fc239e85df | 231 | AX12_processChange(); */ |
antbig | 12:14729d584500 | 232 | } |
antbig | 12:14729d584500 | 233 | |
antbig | 12:14729d584500 | 234 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 235 | /* FUNCTION NAME: runTest */ |
antbig | 9:d0042422d95a | 236 | /* DESCRIPTION : tester l'ensemble des actionneurs du robot */ |
antbig | 9:d0042422d95a | 237 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 238 | void runRobotTest(void) |
antbig | 9:d0042422d95a | 239 | { |
ClementBreteau | 15:c2fc239e85df | 240 | /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE); |
antbig | 12:14729d584500 | 241 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE); |
antbig | 9:d0042422d95a | 242 | |
antbig | 12:14729d584500 | 243 | wait_ms(200); |
antbig | 12:14729d584500 | 244 | |
antbig | 12:14729d584500 | 245 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER); |
antbig | 12:14729d584500 | 246 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER); |
antbig | 12:14729d584500 | 247 | |
antbig | 12:14729d584500 | 248 | wait_ms(200); |
antbig | 12:14729d584500 | 249 | |
antbig | 12:14729d584500 | 250 | AX12_setGoal( |
antbig | 12:14729d584500 | 251 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 12:14729d584500 | 252 | AX12_ANGLE_BRAS_BASE_DROIT_MOITER, |
antbig | 12:14729d584500 | 253 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 254 | ); |
antbig | 12:14729d584500 | 255 | AX12_setGoal( |
antbig | 12:14729d584500 | 256 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 257 | AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER, |
antbig | 12:14729d584500 | 258 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 259 | ); |
antbig | 12:14729d584500 | 260 | AX12_processChange(); |
antbig | 12:14729d584500 | 261 | |
antbig | 12:14729d584500 | 262 | wait_ms(500); |
antbig | 12:14729d584500 | 263 | |
antbig | 12:14729d584500 | 264 | AX12_setGoal( |
antbig | 12:14729d584500 | 265 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 266 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT, |
antbig | 12:14729d584500 | 267 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 268 | ); |
antbig | 12:14729d584500 | 269 | AX12_setGoal( |
antbig | 12:14729d584500 | 270 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 271 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT, |
antbig | 12:14729d584500 | 272 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 273 | ); |
antbig | 12:14729d584500 | 274 | AX12_processChange(); |
antbig | 12:14729d584500 | 275 | |
antbig | 12:14729d584500 | 276 | wait_ms(600); |
antbig | 12:14729d584500 | 277 | |
antbig | 12:14729d584500 | 278 | AX12_setGoal( |
antbig | 12:14729d584500 | 279 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 280 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER, |
antbig | 12:14729d584500 | 281 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 282 | ); |
antbig | 12:14729d584500 | 283 | AX12_setGoal( |
antbig | 12:14729d584500 | 284 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 285 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 286 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 287 | ); |
antbig | 12:14729d584500 | 288 | AX12_processChange(); |
antbig | 12:14729d584500 | 289 | |
antbig | 12:14729d584500 | 290 | wait_ms(500); |
antbig | 12:14729d584500 | 291 | |
antbig | 12:14729d584500 | 292 | AX12_setGoal( |
antbig | 12:14729d584500 | 293 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 12:14729d584500 | 294 | AX12_ANGLE_BRAS_BASE_DROIT_REPLIER, |
antbig | 12:14729d584500 | 295 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 296 | ); |
antbig | 12:14729d584500 | 297 | AX12_setGoal( |
antbig | 12:14729d584500 | 298 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 299 | AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 300 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 301 | ); |
antbig | 12:14729d584500 | 302 | AX12_processChange(); |
antbig | 12:14729d584500 | 303 | |
antbig | 12:14729d584500 | 304 | wait_ms(600); |
antbig | 12:14729d584500 | 305 | AX12_setGoal( |
antbig | 12:14729d584500 | 306 | AX12_ID_PALET_DROIT, |
antbig | 12:14729d584500 | 307 | AX12_ANGLE_PALET_DROIT_ROCHET, |
antbig | 12:14729d584500 | 308 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 309 | ); |
antbig | 12:14729d584500 | 310 | AX12_setGoal( |
antbig | 12:14729d584500 | 311 | AX12_ID_PALET_GAUCHE, |
antbig | 12:14729d584500 | 312 | AX12_ANGLE_PALET_GAUCHE_ROCHET, |
antbig | 12:14729d584500 | 313 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 314 | ); |
antbig | 12:14729d584500 | 315 | AX12_processChange(); |
antbig | 12:14729d584500 | 316 | |
antbig | 12:14729d584500 | 317 | wait_ms(500); |
antbig | 12:14729d584500 | 318 | |
antbig | 12:14729d584500 | 319 | AX12_setGoal( |
antbig | 12:14729d584500 | 320 | AX12_ID_PALET_DROIT, |
antbig | 12:14729d584500 | 321 | AX12_ANGLE_PALET_DROIT_FERMER, |
antbig | 12:14729d584500 | 322 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 323 | ); |
antbig | 12:14729d584500 | 324 | AX12_setGoal( |
antbig | 12:14729d584500 | 325 | AX12_ID_PALET_GAUCHE, |
antbig | 12:14729d584500 | 326 | AX12_ANGLE_PALET_GAUCHE_FERMER, |
antbig | 12:14729d584500 | 327 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 328 | ); |
ClementBreteau | 15:c2fc239e85df | 329 | AX12_processChange();*/ |
antbig | 9:d0042422d95a | 330 | } |
antbig | 9:d0042422d95a | 331 | |
antbig | 9:d0042422d95a | 332 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 333 | /* FUNCTION NAME: SelectStrategy */ |
antbig | 4:88431b537477 | 334 | /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */ |
antbig | 4:88431b537477 | 335 | /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */ |
antbig | 4:88431b537477 | 336 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 337 | int SelectStrategy(unsigned char id) |
antbig | 4:88431b537477 | 338 | { |
antbig | 12:14729d584500 | 339 | |
antbig | 4:88431b537477 | 340 | switch(id) |
antbig | 4:88431b537477 | 341 | { |
antbig | 4:88431b537477 | 342 | case 1: |
antbig | 4:88431b537477 | 343 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 4:88431b537477 | 344 | return FileExists(cheminFileStart); |
antbig | 4:88431b537477 | 345 | case 2: |
antbig | 4:88431b537477 | 346 | strcpy(cheminFileStart,"/local/strat2.txt"); |
antbig | 4:88431b537477 | 347 | return FileExists(cheminFileStart); |
antbig | 7:dcce34c7e06e | 348 | case 3: |
antbig | 7:dcce34c7e06e | 349 | strcpy(cheminFileStart,"/local/strat3.txt"); |
antbig | 7:dcce34c7e06e | 350 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 351 | case 4: |
antbig | 11:ed13a480ddca | 352 | strcpy(cheminFileStart,"/local/strat4.txt"); |
antbig | 11:ed13a480ddca | 353 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 354 | case 5: |
antbig | 11:ed13a480ddca | 355 | strcpy(cheminFileStart,"/local/strat5.txt"); |
antbig | 11:ed13a480ddca | 356 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 357 | case 6: |
antbig | 12:14729d584500 | 358 | strcpy(cheminFileStart,"/local/strat6.txt"); |
antbig | 12:14729d584500 | 359 | return FileExists(cheminFileStart); |
ClementBreteau | 16:7321fb3bb396 | 360 | case 7: |
ClementBreteau | 16:7321fb3bb396 | 361 | strcpy(cheminFileStart,"/local/strat7.txt"); |
ClementBreteau | 16:7321fb3bb396 | 362 | return FileExists(cheminFileStart); |
ClementBreteau | 16:7321fb3bb396 | 363 | case 8: |
ClementBreteau | 16:7321fb3bb396 | 364 | strcpy(cheminFileStart,"/local/strat8.txt"); |
ClementBreteau | 16:7321fb3bb396 | 365 | return FileExists(cheminFileStart); |
ClementBreteau | 16:7321fb3bb396 | 366 | case 9: |
ClementBreteau | 16:7321fb3bb396 | 367 | strcpy(cheminFileStart,"/local/strat9.txt"); |
ClementBreteau | 16:7321fb3bb396 | 368 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 369 | case 10: |
ClementBreteau | 16:7321fb3bb396 | 370 | strcpy(cheminFileStart,"/local/strat10.txt"); |
ClementBreteau | 16:7321fb3bb396 | 371 | return FileExists(cheminFileStart); |
ClementBreteau | 16:7321fb3bb396 | 372 | case 11: |
antbig | 12:14729d584500 | 373 | strcpy(cheminFileStart,"/local/grand_8.txt"); |
antbig | 12:14729d584500 | 374 | return FileExists(cheminFileStart); |
ClementBreteau | 16:7321fb3bb396 | 375 | |
ClementBreteau | 16:7321fb3bb396 | 376 | case 0x10: |
ClementBreteau | 16:7321fb3bb396 | 377 | strcpy(cheminFileStart,"/local/demoBras.txt"); |
ClementBreteau | 16:7321fb3bb396 | 378 | return FileExists(cheminFileStart); |
ClementBreteau | 16:7321fb3bb396 | 379 | |
antbig | 4:88431b537477 | 380 | default: |
antbig | 12:14729d584500 | 381 | strcpy(cheminFileStart,"/local/strat1.txt"); |
ClementBreteau | 16:7321fb3bb396 | 382 | SendRawId(0x258); |
antbig | 4:88431b537477 | 383 | return 0; |
antbig | 4:88431b537477 | 384 | } |
antbig | 4:88431b537477 | 385 | } |
antbig | 4:88431b537477 | 386 | |
antbig | 12:14729d584500 | 387 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 388 | /* FUNCTION NAME: needToStop */ |
antbig | 12:14729d584500 | 389 | /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */ |
antbig | 12:14729d584500 | 390 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 391 | unsigned char needToStop(void) |
antbig | 12:14729d584500 | 392 | { |
antbig | 12:14729d584500 | 393 | return isStopEnable; |
antbig | 12:14729d584500 | 394 | } |
antbig | 12:14729d584500 | 395 | |
antbig | 12:14729d584500 | 396 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 397 | /* FUNCTION NAME: doBeforeEndAction */ |
antbig | 12:14729d584500 | 398 | /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */ |
antbig | 12:14729d584500 | 399 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 400 | void doBeforeEndAction(void) |
antbig | 12:14729d584500 | 401 | { |
antbig | 12:14729d584500 | 402 | |
antbig | 12:14729d584500 | 403 | } |
antbig | 12:14729d584500 | 404 | |
antbig | 3:19f2285a4757 | 405 | #endif |