strat des robots
Fork of CRAC-Strat_2017 by
Robots/Strategie_small.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:
- 15:c2fc239e85df
1Version utilis? lors du match 5
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) { |
antbig | 10:a788d9cf60f2 | 39 | case 101://Descendre le bras pour pecher les poissons |
antbig | 11:ed13a480ddca | 40 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 41 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 42 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 11:ed13a480ddca | 43 | AX12_ANGLE_BRAS_BASE_DROIT_OUVERT, |
antbig | 11:ed13a480ddca | 44 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 45 | ); |
antbig | 12:14729d584500 | 46 | AX12_setGoal( |
antbig | 12:14729d584500 | 47 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 48 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER, |
antbig | 12:14729d584500 | 49 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 50 | ); |
antbig | 3:19f2285a4757 | 51 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 52 | waitingAckID = AX12_ID_BRAS_BASE_DROIT; |
antbig | 11:ed13a480ddca | 53 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 54 | } else { |
antbig | 11:ed13a480ddca | 55 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 56 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 11:ed13a480ddca | 57 | AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT, |
antbig | 11:ed13a480ddca | 58 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 59 | ); |
antbig | 12:14729d584500 | 60 | AX12_setGoal( |
antbig | 12:14729d584500 | 61 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 62 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 63 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 64 | ); |
antbig | 5:dcd817534b57 | 65 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 66 | waitingAckID = AX12_ID_BRAS_BASE_GAUCHE; |
antbig | 11:ed13a480ddca | 67 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 68 | } |
antbig | 3:19f2285a4757 | 69 | break; |
antbig | 10:a788d9cf60f2 | 70 | case 102://Remonter le bras avec les poissons dessus |
antbig | 11:ed13a480ddca | 71 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 72 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 73 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 11:ed13a480ddca | 74 | AX12_ANGLE_BRAS_BASE_DROIT_MOITER, |
antbig | 11:ed13a480ddca | 75 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 76 | ); |
antbig | 3:19f2285a4757 | 77 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 78 | waitingAckID = AX12_ID_BRAS_BASE_DROIT; |
antbig | 11:ed13a480ddca | 79 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 80 | } else { |
antbig | 11:ed13a480ddca | 81 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 82 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 11:ed13a480ddca | 83 | AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER, |
antbig | 11:ed13a480ddca | 84 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 85 | ); |
antbig | 5:dcd817534b57 | 86 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 87 | waitingAckID = AX12_ID_BRAS_BASE_GAUCHE; |
antbig | 11:ed13a480ddca | 88 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 89 | } |
antbig | 3:19f2285a4757 | 90 | break; |
antbig | 10:a788d9cf60f2 | 91 | case 103://Decendre à 30° |
antbig | 11:ed13a480ddca | 92 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 93 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 94 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 11:ed13a480ddca | 95 | AX12_ANGLE_BRAS_BASE_DROIT_RELACHER, |
antbig | 11:ed13a480ddca | 96 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 97 | ); |
antbig | 3:19f2285a4757 | 98 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 99 | waitingAckID = AX12_ID_BRAS_BASE_DROIT; |
antbig | 11:ed13a480ddca | 100 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 101 | } else { |
antbig | 11:ed13a480ddca | 102 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 103 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 11:ed13a480ddca | 104 | AX12_ANGLE_BRAS_BASE_GAUCHE_RELACHER, |
antbig | 11:ed13a480ddca | 105 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 106 | ); |
antbig | 5:dcd817534b57 | 107 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 108 | waitingAckID = AX12_ID_BRAS_BASE_GAUCHE; |
antbig | 11:ed13a480ddca | 109 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 110 | } |
antbig | 3:19f2285a4757 | 111 | break; |
antbig | 10:a788d9cf60f2 | 112 | case 104://ouvrir séparateur |
antbig | 11:ed13a480ddca | 113 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 114 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 115 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 11:ed13a480ddca | 116 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT, |
antbig | 11:ed13a480ddca | 117 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 11:ed13a480ddca | 118 | ); |
antbig | 3:19f2285a4757 | 119 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 120 | waitingAckID = AX12_ID_BRAS_RELACHEUR_DROIT; |
antbig | 11:ed13a480ddca | 121 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 122 | } else { |
antbig | 11:ed13a480ddca | 123 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 124 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 11:ed13a480ddca | 125 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT, |
antbig | 11:ed13a480ddca | 126 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 11:ed13a480ddca | 127 | ); |
antbig | 5:dcd817534b57 | 128 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 129 | waitingAckID = AX12_ID_BRAS_RELACHEUR_GAUCHE; |
antbig | 11:ed13a480ddca | 130 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 3:19f2285a4757 | 131 | } |
antbig | 3:19f2285a4757 | 132 | break; |
antbig | 10:a788d9cf60f2 | 133 | case 105://Rentrer le bras dans le robot, fermer le séparateur |
antbig | 11:ed13a480ddca | 134 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 135 | AX12_setGoal( |
antbig | 12:14729d584500 | 136 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 137 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER, |
antbig | 12:14729d584500 | 138 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 139 | ); |
antbig | 12:14729d584500 | 140 | AX12_processChange(); |
antbig | 12:14729d584500 | 141 | wait_ms(100); |
antbig | 12:14729d584500 | 142 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 143 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 11:ed13a480ddca | 144 | AX12_ANGLE_BRAS_BASE_DROIT_REPLIER, |
antbig | 11:ed13a480ddca | 145 | AX12_SPEED_BRAS_BASE |
antbig | 11:ed13a480ddca | 146 | ); |
antbig | 11:ed13a480ddca | 147 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 148 | waitingAckID = AX12_ID_BRAS_BASE_DROIT; |
antbig | 11:ed13a480ddca | 149 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 150 | } else { |
antbig | 11:ed13a480ddca | 151 | AX12_setGoal( |
antbig | 12:14729d584500 | 152 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 153 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER, |
antbig | 11:ed13a480ddca | 154 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 11:ed13a480ddca | 155 | ); |
antbig | 12:14729d584500 | 156 | AX12_processChange(); |
antbig | 12:14729d584500 | 157 | wait_ms(100); |
antbig | 11:ed13a480ddca | 158 | AX12_setGoal( |
antbig | 12:14729d584500 | 159 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 160 | AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER, |
antbig | 11:ed13a480ddca | 161 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 11:ed13a480ddca | 162 | ); |
antbig | 11:ed13a480ddca | 163 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 164 | waitingAckID = AX12_ID_BRAS_BASE_GAUCHE; |
antbig | 11:ed13a480ddca | 165 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 166 | } |
antbig | 10:a788d9cf60f2 | 167 | break; |
antbig | 11:ed13a480ddca | 168 | case 106://descendre l'attrape coquillages au niveau des rochets gauche si non inversé, droit si inversé |
antbig | 11:ed13a480ddca | 169 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 170 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 171 | AX12_ID_PALET_DROIT, |
antbig | 11:ed13a480ddca | 172 | AX12_ANGLE_PALET_DROIT_ROCHET, |
antbig | 11:ed13a480ddca | 173 | AX12_SPEED_PALET |
antbig | 11:ed13a480ddca | 174 | ); |
antbig | 11:ed13a480ddca | 175 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 176 | waitingAckID = AX12_ID_PALET_DROIT; |
antbig | 11:ed13a480ddca | 177 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 178 | } else { |
antbig | 11:ed13a480ddca | 179 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 180 | AX12_ID_PALET_GAUCHE, |
antbig | 11:ed13a480ddca | 181 | AX12_ANGLE_PALET_GAUCHE_ROCHET, |
antbig | 11:ed13a480ddca | 182 | AX12_SPEED_PALET |
antbig | 11:ed13a480ddca | 183 | ); |
antbig | 11:ed13a480ddca | 184 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 185 | waitingAckID = AX12_ID_PALET_GAUCHE; |
antbig | 11:ed13a480ddca | 186 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 187 | } |
antbig | 11:ed13a480ddca | 188 | break; |
antbig | 11:ed13a480ddca | 189 | case 107://descendre l'attrape coquillages au niveau de la table gauche si non inversé, droit si inversé |
antbig | 11:ed13a480ddca | 190 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 191 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 192 | AX12_ID_PALET_DROIT, |
antbig | 11:ed13a480ddca | 193 | AX12_ANGLE_PALET_DROIT_OUVERT, |
antbig | 11:ed13a480ddca | 194 | AX12_SPEED_PALET |
antbig | 11:ed13a480ddca | 195 | ); |
antbig | 11:ed13a480ddca | 196 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 197 | waitingAckID = AX12_ID_PALET_DROIT; |
antbig | 11:ed13a480ddca | 198 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 199 | } else { |
antbig | 11:ed13a480ddca | 200 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 201 | AX12_ID_PALET_GAUCHE, |
antbig | 11:ed13a480ddca | 202 | AX12_ANGLE_PALET_GAUCHE_OUVERT, |
antbig | 11:ed13a480ddca | 203 | AX12_SPEED_PALET |
antbig | 11:ed13a480ddca | 204 | ); |
antbig | 11:ed13a480ddca | 205 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 206 | waitingAckID = AX12_ID_PALET_GAUCHE; |
antbig | 11:ed13a480ddca | 207 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 208 | } |
antbig | 11:ed13a480ddca | 209 | break; |
antbig | 11:ed13a480ddca | 210 | case 108://Remonter l'attrape coquillages gauche si non inversé, droit si inversé |
antbig | 11:ed13a480ddca | 211 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit |
antbig | 11:ed13a480ddca | 212 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 213 | AX12_ID_PALET_DROIT, |
antbig | 11:ed13a480ddca | 214 | AX12_ANGLE_PALET_DROIT_FERMER, |
antbig | 11:ed13a480ddca | 215 | AX12_SPEED_PALET |
antbig | 11:ed13a480ddca | 216 | ); |
antbig | 11:ed13a480ddca | 217 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 218 | waitingAckID = AX12_ID_PALET_DROIT; |
antbig | 11:ed13a480ddca | 219 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 220 | } else { |
antbig | 11:ed13a480ddca | 221 | AX12_setGoal( |
antbig | 11:ed13a480ddca | 222 | AX12_ID_PALET_GAUCHE, |
antbig | 11:ed13a480ddca | 223 | AX12_ANGLE_PALET_GAUCHE_FERMER, |
antbig | 11:ed13a480ddca | 224 | AX12_SPEED_PALET |
antbig | 11:ed13a480ddca | 225 | ); |
antbig | 11:ed13a480ddca | 226 | AX12_processChange(); |
antbig | 11:ed13a480ddca | 227 | waitingAckID = AX12_ID_PALET_GAUCHE; |
antbig | 11:ed13a480ddca | 228 | waitingAckFrom = SERVO_AX12_DONE; |
antbig | 11:ed13a480ddca | 229 | } |
antbig | 5:dcd817534b57 | 230 | break; |
antbig | 10:a788d9cf60f2 | 231 | |
antbig | 11:ed13a480ddca | 232 | |
antbig | 10:a788d9cf60f2 | 233 | case 200://ouvir la pince gauche |
antbig | 11:ed13a480ddca | 234 | if(InversStrat == 1) { |
antbig | 12:14729d584500 | 235 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE); |
antbig | 11:ed13a480ddca | 236 | } else { |
antbig | 12:14729d584500 | 237 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE); |
antbig | 11:ed13a480ddca | 238 | } |
antbig | 10:a788d9cf60f2 | 239 | break; |
antbig | 10:a788d9cf60f2 | 240 | case 205://Sécuriser le palet gauche |
antbig | 11:ed13a480ddca | 241 | if(InversStrat == 1) { |
antbig | 11:ed13a480ddca | 242 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE); |
antbig | 11:ed13a480ddca | 243 | } else { |
antbig | 11:ed13a480ddca | 244 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE); |
antbig | 11:ed13a480ddca | 245 | } |
antbig | 10:a788d9cf60f2 | 246 | break; |
antbig | 10:a788d9cf60f2 | 247 | case 202://fermer pince gauche |
antbig | 11:ed13a480ddca | 248 | if(InversStrat == 1) { |
antbig | 11:ed13a480ddca | 249 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER); |
antbig | 11:ed13a480ddca | 250 | } else { |
antbig | 11:ed13a480ddca | 251 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER); |
antbig | 11:ed13a480ddca | 252 | } |
antbig | 10:a788d9cf60f2 | 253 | break; |
antbig | 10:a788d9cf60f2 | 254 | |
antbig | 10:a788d9cf60f2 | 255 | case 201://ouvir la pince droite |
antbig | 11:ed13a480ddca | 256 | if(InversStrat == 1) { |
antbig | 11:ed13a480ddca | 257 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE); |
antbig | 11:ed13a480ddca | 258 | } else { |
antbig | 11:ed13a480ddca | 259 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE); |
antbig | 11:ed13a480ddca | 260 | } |
antbig | 10:a788d9cf60f2 | 261 | break; |
antbig | 10:a788d9cf60f2 | 262 | case 204://Sécuriser le palet droit |
antbig | 11:ed13a480ddca | 263 | if(InversStrat == 1) { |
antbig | 11:ed13a480ddca | 264 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE); |
antbig | 11:ed13a480ddca | 265 | } else { |
antbig | 11:ed13a480ddca | 266 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE); |
antbig | 11:ed13a480ddca | 267 | } |
antbig | 10:a788d9cf60f2 | 268 | break; |
antbig | 10:a788d9cf60f2 | 269 | case 203://fermer pince droite |
antbig | 11:ed13a480ddca | 270 | if(InversStrat == 1) { |
antbig | 11:ed13a480ddca | 271 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER); |
antbig | 11:ed13a480ddca | 272 | } else { |
antbig | 11:ed13a480ddca | 273 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER); |
antbig | 11:ed13a480ddca | 274 | } |
antbig | 10:a788d9cf60f2 | 275 | break; |
antbig | 12:14729d584500 | 276 | case 10://Désactiver le stop |
antbig | 12:14729d584500 | 277 | isStopEnable = 0; |
antbig | 12:14729d584500 | 278 | break; |
antbig | 12:14729d584500 | 279 | case 11://Activer le stop |
antbig | 12:14729d584500 | 280 | isStopEnable = 1; |
antbig | 12:14729d584500 | 281 | break; |
antbig | 12:14729d584500 | 282 | case 20://Désactiver l'asservissement |
antbig | 12:14729d584500 | 283 | setAsservissementEtat(0); |
antbig | 12:14729d584500 | 284 | break; |
antbig | 12:14729d584500 | 285 | case 21://Activer l'asservissement |
antbig | 12:14729d584500 | 286 | setAsservissementEtat(1); |
antbig | 12:14729d584500 | 287 | break; |
antbig | 10:a788d9cf60f2 | 288 | |
antbig | 3:19f2285a4757 | 289 | default: |
antbig | 10:a788d9cf60f2 | 290 | return 0;//L'action n'existe pas, il faut utiliser le CAN |
antbig | 3:19f2285a4757 | 291 | |
antbig | 3:19f2285a4757 | 292 | } |
antbig | 11:ed13a480ddca | 293 | return 1;//L'action est spécifique. |
antbig | 3:19f2285a4757 | 294 | |
antbig | 3:19f2285a4757 | 295 | } |
antbig | 3:19f2285a4757 | 296 | |
antbig | 3:19f2285a4757 | 297 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 298 | /* FUNCTION NAME: initRobot */ |
antbig | 3:19f2285a4757 | 299 | /* DESCRIPTION : initialiser le robot */ |
antbig | 3:19f2285a4757 | 300 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 301 | void initRobot(void) { |
antbig | 3:19f2285a4757 | 302 | /** |
antbig | 3:19f2285a4757 | 303 | On enregistre les id des AX12 présent sur la carte |
antbig | 3:19f2285a4757 | 304 | **/ |
antbig | 5:dcd817534b57 | 305 | AX12_register(1,AX12_SERIAL1,0x0FF); |
antbig | 5:dcd817534b57 | 306 | AX12_register(2,AX12_SERIAL1); |
antbig | 5:dcd817534b57 | 307 | AX12_register(18,AX12_SERIAL1); |
antbig | 5:dcd817534b57 | 308 | AX12_register(4,AX12_SERIAL2); |
antbig | 5:dcd817534b57 | 309 | AX12_register(16,AX12_SERIAL2); |
antbig | 5:dcd817534b57 | 310 | AX12_register(17,AX12_SERIAL2,0x0FF); |
antbig | 12:14729d584500 | 311 | |
antbig | 12:14729d584500 | 312 | //runRobotTest(); |
antbig | 12:14729d584500 | 313 | |
antbig | 5:dcd817534b57 | 314 | /* |
antbig | 5:dcd817534b57 | 315 | AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF); |
antbig | 5:dcd817534b57 | 316 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras |
antbig | 5:dcd817534b57 | 317 | AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF); |
antbig | 5:dcd817534b57 | 318 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras |
antbig | 5:dcd817534b57 | 319 | AX12_processChange();*/ |
antbig | 3:19f2285a4757 | 320 | } |
antbig | 3:19f2285a4757 | 321 | |
antbig | 4:88431b537477 | 322 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 323 | /* FUNCTION NAME: initRobotActionneur */ |
antbig | 12:14729d584500 | 324 | /* DESCRIPTION : Initialiser la position des actionneurs du robot */ |
antbig | 12:14729d584500 | 325 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 326 | void initRobotActionneur(void) |
antbig | 12:14729d584500 | 327 | { |
antbig | 12:14729d584500 | 328 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER); |
antbig | 12:14729d584500 | 329 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER); |
antbig | 12:14729d584500 | 330 | |
antbig | 12:14729d584500 | 331 | AX12_setGoal( |
antbig | 12:14729d584500 | 332 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 333 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER, |
antbig | 12:14729d584500 | 334 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 335 | ); |
antbig | 12:14729d584500 | 336 | AX12_setGoal( |
antbig | 12:14729d584500 | 337 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 338 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 339 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 340 | ); |
antbig | 12:14729d584500 | 341 | AX12_setGoal( |
antbig | 12:14729d584500 | 342 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 12:14729d584500 | 343 | AX12_ANGLE_BRAS_BASE_DROIT_REPLIER, |
antbig | 12:14729d584500 | 344 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 345 | ); |
antbig | 12:14729d584500 | 346 | AX12_setGoal( |
antbig | 12:14729d584500 | 347 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 348 | AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 349 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 350 | ); |
antbig | 12:14729d584500 | 351 | AX12_setGoal( |
antbig | 12:14729d584500 | 352 | AX12_ID_PALET_DROIT, |
antbig | 12:14729d584500 | 353 | AX12_ANGLE_PALET_DROIT_FERMER, |
antbig | 12:14729d584500 | 354 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 355 | ); |
antbig | 12:14729d584500 | 356 | AX12_setGoal( |
antbig | 12:14729d584500 | 357 | AX12_ID_PALET_GAUCHE, |
antbig | 12:14729d584500 | 358 | AX12_ANGLE_PALET_GAUCHE_FERMER, |
antbig | 12:14729d584500 | 359 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 360 | ); |
antbig | 12:14729d584500 | 361 | AX12_processChange(); |
antbig | 12:14729d584500 | 362 | } |
antbig | 12:14729d584500 | 363 | |
antbig | 12:14729d584500 | 364 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 365 | /* FUNCTION NAME: runTest */ |
antbig | 9:d0042422d95a | 366 | /* DESCRIPTION : tester l'ensemble des actionneurs du robot */ |
antbig | 9:d0042422d95a | 367 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 368 | void runRobotTest(void) |
antbig | 9:d0042422d95a | 369 | { |
antbig | 12:14729d584500 | 370 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE); |
antbig | 12:14729d584500 | 371 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE); |
antbig | 9:d0042422d95a | 372 | |
antbig | 12:14729d584500 | 373 | wait_ms(200); |
antbig | 12:14729d584500 | 374 | |
antbig | 12:14729d584500 | 375 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER); |
antbig | 12:14729d584500 | 376 | XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER); |
antbig | 12:14729d584500 | 377 | |
antbig | 12:14729d584500 | 378 | wait_ms(200); |
antbig | 12:14729d584500 | 379 | |
antbig | 12:14729d584500 | 380 | AX12_setGoal( |
antbig | 12:14729d584500 | 381 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 12:14729d584500 | 382 | AX12_ANGLE_BRAS_BASE_DROIT_MOITER, |
antbig | 12:14729d584500 | 383 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 384 | ); |
antbig | 12:14729d584500 | 385 | AX12_setGoal( |
antbig | 12:14729d584500 | 386 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 387 | AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER, |
antbig | 12:14729d584500 | 388 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 389 | ); |
antbig | 12:14729d584500 | 390 | AX12_processChange(); |
antbig | 12:14729d584500 | 391 | |
antbig | 12:14729d584500 | 392 | wait_ms(500); |
antbig | 12:14729d584500 | 393 | |
antbig | 12:14729d584500 | 394 | AX12_setGoal( |
antbig | 12:14729d584500 | 395 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 396 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT, |
antbig | 12:14729d584500 | 397 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 398 | ); |
antbig | 12:14729d584500 | 399 | AX12_setGoal( |
antbig | 12:14729d584500 | 400 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 401 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT, |
antbig | 12:14729d584500 | 402 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 403 | ); |
antbig | 12:14729d584500 | 404 | AX12_processChange(); |
antbig | 12:14729d584500 | 405 | |
antbig | 12:14729d584500 | 406 | wait_ms(600); |
antbig | 12:14729d584500 | 407 | |
antbig | 12:14729d584500 | 408 | AX12_setGoal( |
antbig | 12:14729d584500 | 409 | AX12_ID_BRAS_RELACHEUR_DROIT, |
antbig | 12:14729d584500 | 410 | AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER, |
antbig | 12:14729d584500 | 411 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 412 | ); |
antbig | 12:14729d584500 | 413 | AX12_setGoal( |
antbig | 12:14729d584500 | 414 | AX12_ID_BRAS_RELACHEUR_GAUCHE, |
antbig | 12:14729d584500 | 415 | AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 416 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 417 | ); |
antbig | 12:14729d584500 | 418 | AX12_processChange(); |
antbig | 12:14729d584500 | 419 | |
antbig | 12:14729d584500 | 420 | wait_ms(500); |
antbig | 12:14729d584500 | 421 | |
antbig | 12:14729d584500 | 422 | AX12_setGoal( |
antbig | 12:14729d584500 | 423 | AX12_ID_BRAS_BASE_DROIT, |
antbig | 12:14729d584500 | 424 | AX12_ANGLE_BRAS_BASE_DROIT_REPLIER, |
antbig | 12:14729d584500 | 425 | AX12_SPEED_BRAS_BASE |
antbig | 12:14729d584500 | 426 | ); |
antbig | 12:14729d584500 | 427 | AX12_setGoal( |
antbig | 12:14729d584500 | 428 | AX12_ID_BRAS_BASE_GAUCHE, |
antbig | 12:14729d584500 | 429 | AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER, |
antbig | 12:14729d584500 | 430 | AX12_SPEED_BRAS_RELACHEUR |
antbig | 12:14729d584500 | 431 | ); |
antbig | 12:14729d584500 | 432 | AX12_processChange(); |
antbig | 12:14729d584500 | 433 | |
antbig | 12:14729d584500 | 434 | wait_ms(600); |
antbig | 12:14729d584500 | 435 | AX12_setGoal( |
antbig | 12:14729d584500 | 436 | AX12_ID_PALET_DROIT, |
antbig | 12:14729d584500 | 437 | AX12_ANGLE_PALET_DROIT_ROCHET, |
antbig | 12:14729d584500 | 438 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 439 | ); |
antbig | 12:14729d584500 | 440 | AX12_setGoal( |
antbig | 12:14729d584500 | 441 | AX12_ID_PALET_GAUCHE, |
antbig | 12:14729d584500 | 442 | AX12_ANGLE_PALET_GAUCHE_ROCHET, |
antbig | 12:14729d584500 | 443 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 444 | ); |
antbig | 12:14729d584500 | 445 | AX12_processChange(); |
antbig | 12:14729d584500 | 446 | |
antbig | 12:14729d584500 | 447 | wait_ms(500); |
antbig | 12:14729d584500 | 448 | |
antbig | 12:14729d584500 | 449 | AX12_setGoal( |
antbig | 12:14729d584500 | 450 | AX12_ID_PALET_DROIT, |
antbig | 12:14729d584500 | 451 | AX12_ANGLE_PALET_DROIT_FERMER, |
antbig | 12:14729d584500 | 452 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 453 | ); |
antbig | 12:14729d584500 | 454 | AX12_setGoal( |
antbig | 12:14729d584500 | 455 | AX12_ID_PALET_GAUCHE, |
antbig | 12:14729d584500 | 456 | AX12_ANGLE_PALET_GAUCHE_FERMER, |
antbig | 12:14729d584500 | 457 | AX12_SPEED_PALET |
antbig | 12:14729d584500 | 458 | ); |
antbig | 12:14729d584500 | 459 | AX12_processChange(); |
antbig | 9:d0042422d95a | 460 | } |
antbig | 9:d0042422d95a | 461 | |
antbig | 9:d0042422d95a | 462 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 463 | /* FUNCTION NAME: SelectStrategy */ |
antbig | 4:88431b537477 | 464 | /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */ |
antbig | 4:88431b537477 | 465 | /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */ |
antbig | 4:88431b537477 | 466 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 467 | int SelectStrategy(unsigned char id) |
antbig | 4:88431b537477 | 468 | { |
antbig | 12:14729d584500 | 469 | |
antbig | 12:14729d584500 | 470 | |
antbig | 4:88431b537477 | 471 | switch(id) |
antbig | 4:88431b537477 | 472 | { |
antbig | 4:88431b537477 | 473 | case 1: |
antbig | 4:88431b537477 | 474 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 4:88431b537477 | 475 | return FileExists(cheminFileStart); |
antbig | 4:88431b537477 | 476 | case 2: |
antbig | 4:88431b537477 | 477 | strcpy(cheminFileStart,"/local/strat2.txt"); |
antbig | 4:88431b537477 | 478 | return FileExists(cheminFileStart); |
antbig | 7:dcce34c7e06e | 479 | case 3: |
antbig | 7:dcce34c7e06e | 480 | strcpy(cheminFileStart,"/local/strat3.txt"); |
antbig | 7:dcce34c7e06e | 481 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 482 | case 4: |
antbig | 11:ed13a480ddca | 483 | strcpy(cheminFileStart,"/local/strat4.txt"); |
antbig | 11:ed13a480ddca | 484 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 485 | case 5: |
antbig | 11:ed13a480ddca | 486 | strcpy(cheminFileStart,"/local/strat5.txt"); |
antbig | 11:ed13a480ddca | 487 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 488 | case 6: |
antbig | 12:14729d584500 | 489 | strcpy(cheminFileStart,"/local/strat6.txt"); |
antbig | 12:14729d584500 | 490 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 491 | case 10: |
antbig | 12:14729d584500 | 492 | strcpy(cheminFileStart,"/local/grand_8.txt"); |
antbig | 12:14729d584500 | 493 | return FileExists(cheminFileStart); |
antbig | 4:88431b537477 | 494 | default: |
antbig | 12:14729d584500 | 495 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 4:88431b537477 | 496 | return 0; |
antbig | 4:88431b537477 | 497 | } |
antbig | 4:88431b537477 | 498 | } |
antbig | 4:88431b537477 | 499 | |
antbig | 12:14729d584500 | 500 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 501 | /* FUNCTION NAME: needToStop */ |
antbig | 12:14729d584500 | 502 | /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */ |
antbig | 12:14729d584500 | 503 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 504 | unsigned char needToStop(void) |
antbig | 12:14729d584500 | 505 | { |
antbig | 12:14729d584500 | 506 | return isStopEnable; |
antbig | 12:14729d584500 | 507 | } |
antbig | 12:14729d584500 | 508 | |
antbig | 12:14729d584500 | 509 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 510 | /* FUNCTION NAME: doBeforeEndAction */ |
antbig | 12:14729d584500 | 511 | /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */ |
antbig | 12:14729d584500 | 512 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 513 | void doBeforeEndAction(void) |
antbig | 12:14729d584500 | 514 | { |
antbig | 12:14729d584500 | 515 | |
antbig | 12:14729d584500 | 516 | } |
antbig | 12:14729d584500 | 517 | |
antbig | 3:19f2285a4757 | 518 | #endif |