![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Robots/Strategie_big.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 |
---|---|---|---|
ClementBreteau | 15:c2fc239e85df | 1 | |
antbig | 1:116040d14164 | 2 | #include "StrategieManager.h" |
antbig | 3:19f2285a4757 | 3 | #ifdef ROBOT_BIG |
antbig | 3:19f2285a4757 | 4 | #include "Config_big.h" |
antbig | 0:ad97421fb1fb | 5 | |
antbig | 12:14729d584500 | 6 | unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises |
antbig | 12:14729d584500 | 7 | |
antbig | 0:ad97421fb1fb | 8 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 9 | /* FUNCTION NAME: doFunnyAction */ |
antbig | 0:ad97421fb1fb | 10 | /* DESCRIPTION : Permet de faire la funny action en fin de partie */ |
antbig | 0:ad97421fb1fb | 11 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 12 | void doFunnyAction(void) { |
ClementBreteau | 15:c2fc239e85df | 13 | /* AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION); |
ClementBreteau | 15:c2fc239e85df | 14 | AX12_processChange();*/ |
antbig | 0:ad97421fb1fb | 15 | |
antbig | 0:ad97421fb1fb | 16 | } |
antbig | 0:ad97421fb1fb | 17 | |
antbig | 0:ad97421fb1fb | 18 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 19 | /* FUNCTION NAME: doAction */ |
antbig | 0:ad97421fb1fb | 20 | /* DESCRIPTION : Effectuer une action specifique */ |
antbig | 0:ad97421fb1fb | 21 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 22 | unsigned char doAction(unsigned char id, unsigned short speed, short angle) { |
antbig | 12:14729d584500 | 23 | CANMessage msgTx=CANMessage(); |
antbig | 0:ad97421fb1fb | 24 | switch(id) { |
antbig | 3:19f2285a4757 | 25 | |
ClementBreteau | 15:c2fc239e85df | 26 | case 100://preparation prise |
ClementBreteau | 15:c2fc239e85df | 27 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 28 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 29 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 30 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 31 | |
ClementBreteau | 15:c2fc239e85df | 32 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 33 | msgTx.data[0]=1; |
ClementBreteau | 15:c2fc239e85df | 34 | if (InversStrat){ // si on est inversé, on echange les bras |
ClementBreteau | 15:c2fc239e85df | 35 | if (speed == BRAS_GAUCHE) speed = BRAS_DROIT; |
ClementBreteau | 15:c2fc239e85df | 36 | else speed = BRAS_GAUCHE; |
ClementBreteau | 15:c2fc239e85df | 37 | } |
ClementBreteau | 15:c2fc239e85df | 38 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 39 | can1.write(msgTx); |
antbig | 12:14729d584500 | 40 | break; |
ClementBreteau | 15:c2fc239e85df | 41 | case 101://stockage haut |
ClementBreteau | 15:c2fc239e85df | 42 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 43 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 44 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 45 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 46 | |
ClementBreteau | 15:c2fc239e85df | 47 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 48 | msgTx.data[0]=2; |
ClementBreteau | 15:c2fc239e85df | 49 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 50 | |
ClementBreteau | 15:c2fc239e85df | 51 | can1.write(msgTx); |
antbig | 12:14729d584500 | 52 | break; |
antbig | 12:14729d584500 | 53 | |
ClementBreteau | 15:c2fc239e85df | 54 | case 102://stockage bas |
ClementBreteau | 15:c2fc239e85df | 55 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 56 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 57 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 58 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 59 | |
ClementBreteau | 15:c2fc239e85df | 60 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 61 | msgTx.data[0]=3; |
ClementBreteau | 15:c2fc239e85df | 62 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 63 | |
ClementBreteau | 15:c2fc239e85df | 64 | can1.write(msgTx); |
antbig | 12:14729d584500 | 65 | break; |
ClementBreteau | 15:c2fc239e85df | 66 | case 103://deposer |
ClementBreteau | 15:c2fc239e85df | 67 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 68 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 69 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 70 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 71 | |
ClementBreteau | 15:c2fc239e85df | 72 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 73 | msgTx.data[0]=4; |
ClementBreteau | 15:c2fc239e85df | 74 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 75 | |
ClementBreteau | 15:c2fc239e85df | 76 | can1.write(msgTx); |
antbig | 12:14729d584500 | 77 | break; |
antbig | 12:14729d584500 | 78 | |
ClementBreteau | 15:c2fc239e85df | 79 | case 104://preparation depot bas |
ClementBreteau | 15:c2fc239e85df | 80 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 81 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 82 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 83 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 84 | |
ClementBreteau | 15:c2fc239e85df | 85 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 86 | msgTx.data[0]=5; |
ClementBreteau | 15:c2fc239e85df | 87 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 88 | |
ClementBreteau | 15:c2fc239e85df | 89 | can1.write(msgTx); |
antbig | 12:14729d584500 | 90 | break; |
ClementBreteau | 15:c2fc239e85df | 91 | case 105://preparation depot haut |
ClementBreteau | 15:c2fc239e85df | 92 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 93 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 94 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 95 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 96 | |
ClementBreteau | 15:c2fc239e85df | 97 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 98 | msgTx.data[0]=6; |
ClementBreteau | 15:c2fc239e85df | 99 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 100 | |
ClementBreteau | 15:c2fc239e85df | 101 | can1.write(msgTx); |
antbig | 12:14729d584500 | 102 | break; |
ClementBreteau | 15:c2fc239e85df | 103 | case 106://pousser module |
ClementBreteau | 15:c2fc239e85df | 104 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 105 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 106 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 107 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 108 | |
ClementBreteau | 15:c2fc239e85df | 109 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 110 | msgTx.data[0]=7; |
ClementBreteau | 15:c2fc239e85df | 111 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 112 | |
ClementBreteau | 15:c2fc239e85df | 113 | can1.write(msgTx); |
antbig | 12:14729d584500 | 114 | break; |
antbig | 12:14729d584500 | 115 | |
antbig | 8:0edc7dfb7f7e | 116 | case 110://Ouvrir la pince arrière haute |
ClementBreteau | 15:c2fc239e85df | 117 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 118 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 119 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 120 | msgTx.len=2; |
ClementBreteau | 15:c2fc239e85df | 121 | |
ClementBreteau | 15:c2fc239e85df | 122 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 123 | msgTx.data[0]=10; |
ClementBreteau | 15:c2fc239e85df | 124 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 125 | |
ClementBreteau | 15:c2fc239e85df | 126 | can1.write(msgTx); |
antbig | 8:0edc7dfb7f7e | 127 | break; |
antbig | 8:0edc7dfb7f7e | 128 | case 111://Fermer la pince arrière haute |
ClementBreteau | 15:c2fc239e85df | 129 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 130 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 131 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 132 | msgTx.len=3; |
ClementBreteau | 15:c2fc239e85df | 133 | |
ClementBreteau | 15:c2fc239e85df | 134 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 135 | msgTx.data[0]=11; |
ClementBreteau | 15:c2fc239e85df | 136 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 137 | msgTx.data[2]=angle; |
ClementBreteau | 15:c2fc239e85df | 138 | |
ClementBreteau | 15:c2fc239e85df | 139 | can1.write(msgTx); |
antbig | 8:0edc7dfb7f7e | 140 | break; |
antbig | 9:d0042422d95a | 141 | |
antbig | 8:0edc7dfb7f7e | 142 | case 112://Ouvrir la pince arrière basse |
ClementBreteau | 15:c2fc239e85df | 143 | msgTx.id=SERVO_AX12_ACTION; |
ClementBreteau | 15:c2fc239e85df | 144 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 145 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 146 | msgTx.len=2; |
antbig | 8:0edc7dfb7f7e | 147 | |
ClementBreteau | 15:c2fc239e85df | 148 | // action et le cote selectionné |
ClementBreteau | 15:c2fc239e85df | 149 | msgTx.data[0]=12; |
ClementBreteau | 15:c2fc239e85df | 150 | msgTx.data[1]=speed; |
ClementBreteau | 15:c2fc239e85df | 151 | |
ClementBreteau | 15:c2fc239e85df | 152 | can1.write(msgTx); |
antbig | 8:0edc7dfb7f7e | 153 | break; |
ClementBreteau | 15:c2fc239e85df | 154 | |
antbig | 12:14729d584500 | 155 | case 120://Activer les pompes |
ClementBreteau | 15:c2fc239e85df | 156 | /*AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE); |
ClementBreteau | 15:c2fc239e85df | 157 | AX12_processChange();*/ |
antbig | 12:14729d584500 | 158 | |
antbig | 12:14729d584500 | 159 | |
antbig | 12:14729d584500 | 160 | msgTx.id=POMPE_PWM; |
antbig | 12:14729d584500 | 161 | msgTx.format=CANStandard; |
antbig | 12:14729d584500 | 162 | msgTx.type=CANData; |
antbig | 12:14729d584500 | 163 | msgTx.len=6; |
antbig | 12:14729d584500 | 164 | |
antbig | 12:14729d584500 | 165 | // x sur 2 octets |
antbig | 12:14729d584500 | 166 | msgTx.data[0]=(unsigned char)POMPES_PWM; |
antbig | 12:14729d584500 | 167 | msgTx.data[1]=(unsigned char)POMPES_PWM; |
antbig | 12:14729d584500 | 168 | msgTx.data[2]=(unsigned char)POMPES_PWM; |
antbig | 12:14729d584500 | 169 | msgTx.data[3]=(unsigned char)POMPES_PWM; |
antbig | 12:14729d584500 | 170 | msgTx.data[4]=(unsigned char)POMPES_PWM; |
antbig | 12:14729d584500 | 171 | msgTx.data[5]=(unsigned char)POMPES_PWM; |
antbig | 12:14729d584500 | 172 | |
antbig | 12:14729d584500 | 173 | can1.write(msgTx); |
antbig | 8:0edc7dfb7f7e | 174 | break; |
antbig | 12:14729d584500 | 175 | case 121://Désactiver les pompes |
antbig | 12:14729d584500 | 176 | msgTx.id=POMPE_PWM; |
antbig | 12:14729d584500 | 177 | msgTx.format=CANStandard; |
antbig | 12:14729d584500 | 178 | msgTx.type=CANData; |
antbig | 12:14729d584500 | 179 | msgTx.len=6; |
antbig | 12:14729d584500 | 180 | |
antbig | 12:14729d584500 | 181 | // x sur 2 octets |
antbig | 12:14729d584500 | 182 | msgTx.data[0]=(unsigned char)0; |
antbig | 12:14729d584500 | 183 | msgTx.data[1]=(unsigned char)0; |
antbig | 12:14729d584500 | 184 | msgTx.data[2]=(unsigned char)0; |
antbig | 12:14729d584500 | 185 | msgTx.data[3]=(unsigned char)0; |
antbig | 12:14729d584500 | 186 | msgTx.data[4]=(unsigned char)0; |
antbig | 12:14729d584500 | 187 | msgTx.data[5]=(unsigned char)0; |
antbig | 12:14729d584500 | 188 | |
antbig | 12:14729d584500 | 189 | can1.write(msgTx); |
antbig | 12:14729d584500 | 190 | break; |
antbig | 12:14729d584500 | 191 | |
antbig | 12:14729d584500 | 192 | case 10://Désactiver le stop |
antbig | 12:14729d584500 | 193 | isStopEnable = 0; |
antbig | 12:14729d584500 | 194 | break; |
antbig | 12:14729d584500 | 195 | case 11://Activer le stop |
antbig | 12:14729d584500 | 196 | isStopEnable = 1; |
antbig | 12:14729d584500 | 197 | break; |
antbig | 12:14729d584500 | 198 | case 20://Désactiver l'asservissement |
antbig | 12:14729d584500 | 199 | setAsservissementEtat(0); |
antbig | 12:14729d584500 | 200 | break; |
antbig | 12:14729d584500 | 201 | case 21://Activer l'asservissement |
antbig | 12:14729d584500 | 202 | setAsservissementEtat(1); |
antbig | 12:14729d584500 | 203 | break; |
antbig | 12:14729d584500 | 204 | |
antbig | 12:14729d584500 | 205 | case 22://Changer la vitesse du robot |
antbig | 12:14729d584500 | 206 | SendSpeed(speed,(unsigned short)angle); |
antbig | 12:14729d584500 | 207 | break; |
antbig | 12:14729d584500 | 208 | |
antbig | 12:14729d584500 | 209 | case 30://Action tempo |
antbig | 12:14729d584500 | 210 | wait_ms(speed); |
antbig | 12:14729d584500 | 211 | break; |
antbig | 12:14729d584500 | 212 | |
ClementBreteau | 14:c8fc06c4887f | 213 | case 40: // demande au telemetre la position d'un objet |
ClementBreteau | 14:c8fc06c4887f | 214 | //SendRawId(TELEMETRE_RECHERCHE_OBJET); |
ClementBreteau | 15:c2fc239e85df | 215 | |
ClementBreteau | 15:c2fc239e85df | 216 | modeTelemetre = 1; |
ClementBreteau | 15:c2fc239e85df | 217 | |
ClementBreteau | 15:c2fc239e85df | 218 | //angle = angle /10; |
ClementBreteau | 15:c2fc239e85df | 219 | |
ClementBreteau | 15:c2fc239e85df | 220 | msgTx.id=TELEMETRE_OBJET; |
ClementBreteau | 15:c2fc239e85df | 221 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 222 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 223 | msgTx.len=1; |
ClementBreteau | 15:c2fc239e85df | 224 | // indice du module sur le terrain |
ClementBreteau | 15:c2fc239e85df | 225 | msgTx.data[0] = (unsigned char)speed; |
ClementBreteau | 15:c2fc239e85df | 226 | |
ClementBreteau | 15:c2fc239e85df | 227 | /* |
ClementBreteau | 15:c2fc239e85df | 228 | // x sur 2 octets |
ClementBreteau | 15:c2fc239e85df | 229 | msgTx.data[0]=(unsigned char)speed; |
ClementBreteau | 15:c2fc239e85df | 230 | msgTx.data[1]=(unsigned char)(speed>>8); |
ClementBreteau | 15:c2fc239e85df | 231 | // y sur 2 octets |
ClementBreteau | 15:c2fc239e85df | 232 | msgTx.data[2]=(unsigned char)angle; |
ClementBreteau | 15:c2fc239e85df | 233 | msgTx.data[3]=(unsigned char)(angle>>8); |
ClementBreteau | 15:c2fc239e85df | 234 | // theta signé sur 2 octets |
ClementBreteau | 15:c2fc239e85df | 235 | //msgTx.data[4]=(unsigned char)theta; |
ClementBreteau | 15:c2fc239e85df | 236 | //msgTx.data[5]=(unsigned char)(theta>>8); |
ClementBreteau | 15:c2fc239e85df | 237 | msgTx.data[4]=0; |
ClementBreteau | 15:c2fc239e85df | 238 | msgTx.data[5]=0;*/ |
ClementBreteau | 15:c2fc239e85df | 239 | |
ClementBreteau | 15:c2fc239e85df | 240 | can1.write(msgTx); |
ClementBreteau | 15:c2fc239e85df | 241 | |
ClementBreteau | 15:c2fc239e85df | 242 | break; |
ClementBreteau | 14:c8fc06c4887f | 243 | |
antbig | 12:14729d584500 | 244 | case 130://Lancer mouvement de sortie de la zone de départ |
ClementBreteau | 15:c2fc239e85df | 245 | msgTx.id=ACTION_BIG_DEMARRAGE; |
ClementBreteau | 15:c2fc239e85df | 246 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 247 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 248 | msgTx.len=1; |
ClementBreteau | 15:c2fc239e85df | 249 | msgTx.data[0] = (unsigned char)speed; |
ClementBreteau | 15:c2fc239e85df | 250 | can1.write(msgTx); |
antbig | 12:14729d584500 | 251 | break; |
antbig | 12:14729d584500 | 252 | |
antbig | 0:ad97421fb1fb | 253 | default: |
antbig | 0:ad97421fb1fb | 254 | return 0;//L'action n'existe pas, il faut utiliser le CAN |
antbig | 0:ad97421fb1fb | 255 | |
antbig | 0:ad97421fb1fb | 256 | } |
antbig | 0:ad97421fb1fb | 257 | return 1;//L'action est spécifique. |
antbig | 0:ad97421fb1fb | 258 | |
antbig | 0:ad97421fb1fb | 259 | } |
antbig | 0:ad97421fb1fb | 260 | |
antbig | 0:ad97421fb1fb | 261 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 262 | /* FUNCTION NAME: initRobot */ |
antbig | 0:ad97421fb1fb | 263 | /* DESCRIPTION : initialiser le robot */ |
antbig | 0:ad97421fb1fb | 264 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 265 | void initRobot(void) |
antbig | 9:d0042422d95a | 266 | { |
antbig | 9:d0042422d95a | 267 | //Enregistrement de tous les AX12 présent sur la carte |
ClementBreteau | 15:c2fc239e85df | 268 | /*AX12_register(5, AX12_SERIAL2); |
antbig | 8:0edc7dfb7f7e | 269 | AX12_register(18, AX12_SERIAL2); |
antbig | 8:0edc7dfb7f7e | 270 | AX12_register(13, AX12_SERIAL2); |
antbig | 12:14729d584500 | 271 | AX12_register(1, AX12_SERIAL1); |
antbig | 12:14729d584500 | 272 | AX12_register(11, AX12_SERIAL1); |
antbig | 12:14729d584500 | 273 | AX12_register(8, AX12_SERIAL1); |
ClementBreteau | 15:c2fc239e85df | 274 | AX12_register(7, AX12_SERIAL2);*/ |
antbig | 9:d0042422d95a | 275 | |
antbig | 12:14729d584500 | 276 | //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION); |
antbig | 12:14729d584500 | 277 | //AX12_processChange(); |
antbig | 11:ed13a480ddca | 278 | //runRobotTest(); |
ClementBreteau | 16:7321fb3bb396 | 279 | |
antbig | 9:d0042422d95a | 280 | } |
antbig | 9:d0042422d95a | 281 | |
antbig | 9:d0042422d95a | 282 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 283 | /* FUNCTION NAME: initRobotActionneur */ |
antbig | 12:14729d584500 | 284 | /* DESCRIPTION : Initialiser la position des actionneurs du robot */ |
antbig | 12:14729d584500 | 285 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 286 | void initRobotActionneur(void) |
antbig | 12:14729d584500 | 287 | { |
antbig | 12:14729d584500 | 288 | doAction(110,0,0);//Ouverture pince arrière haute |
antbig | 12:14729d584500 | 289 | doAction(112,0,0);//Ouverture pince arrière basse |
antbig | 12:14729d584500 | 290 | doAction(114,0,0);//Ouverture porte arrière |
antbig | 12:14729d584500 | 291 | doAction(100,0,0);//Ouvrir les portes avant |
antbig | 12:14729d584500 | 292 | doAction(102,0,0);//Remonter le peigne |
antbig | 12:14729d584500 | 293 | doAction(106,0,0);//Remonter le support du cone arriere |
antbig | 12:14729d584500 | 294 | } |
antbig | 12:14729d584500 | 295 | |
antbig | 12:14729d584500 | 296 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 297 | /* FUNCTION NAME: runTest */ |
antbig | 9:d0042422d95a | 298 | /* DESCRIPTION : tester l'ensemble des actionneurs du robot */ |
antbig | 9:d0042422d95a | 299 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 300 | void runRobotTest(void) |
antbig | 9:d0042422d95a | 301 | { |
antbig | 12:14729d584500 | 302 | int waitTime = 500; |
antbig | 12:14729d584500 | 303 | |
antbig | 9:d0042422d95a | 304 | //Test des AX12 dans l'ordre |
antbig | 9:d0042422d95a | 305 | doAction(111,0,0);//Fermeture pince arrière haute |
antbig | 12:14729d584500 | 306 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 307 | doAction(110,0,0);//Ouverture pince arrière haute |
antbig | 12:14729d584500 | 308 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 309 | doAction(113,0,0);//Fermeture pince arrière basse |
antbig | 12:14729d584500 | 310 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 311 | doAction(112,0,0);//Ouverture pince arrière basse |
antbig | 12:14729d584500 | 312 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 313 | doAction(115,0,0);//Fermeture porte arrière |
antbig | 12:14729d584500 | 314 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 315 | doAction(114,0,0);//Ouverture porte arrière |
antbig | 12:14729d584500 | 316 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 317 | doAction(101,0,0);//Fermer les portes avant |
antbig | 12:14729d584500 | 318 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 319 | doAction(100,0,0);//Ouvrir les portes avant |
antbig | 12:14729d584500 | 320 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 321 | doAction(103,0,0);//Descendre le peigne |
antbig | 12:14729d584500 | 322 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 323 | doAction(102,0,0);//Remonter le peigne |
antbig | 0:ad97421fb1fb | 324 | } |
antbig | 3:19f2285a4757 | 325 | |
antbig | 4:88431b537477 | 326 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 327 | /* FUNCTION NAME: SelectStrategy */ |
antbig | 4:88431b537477 | 328 | /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */ |
antbig | 4:88431b537477 | 329 | /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */ |
antbig | 4:88431b537477 | 330 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 331 | int SelectStrategy(unsigned char id) |
antbig | 4:88431b537477 | 332 | { |
antbig | 4:88431b537477 | 333 | switch(id) |
antbig | 4:88431b537477 | 334 | { |
ClementBreteau | 15:c2fc239e85df | 335 | // strat de match |
antbig | 4:88431b537477 | 336 | case 1: |
antbig | 11:ed13a480ddca | 337 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 11:ed13a480ddca | 338 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 339 | case 2: |
antbig | 11:ed13a480ddca | 340 | strcpy(cheminFileStart,"/local/strat2.txt"); |
antbig | 11:ed13a480ddca | 341 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 342 | case 3: |
antbig | 11:ed13a480ddca | 343 | strcpy(cheminFileStart,"/local/strat3.txt"); |
antbig | 11:ed13a480ddca | 344 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 345 | case 4: |
antbig | 12:14729d584500 | 346 | strcpy(cheminFileStart,"/local/strat4.txt"); |
antbig | 12:14729d584500 | 347 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 348 | case 5: |
antbig | 12:14729d584500 | 349 | strcpy(cheminFileStart,"/local/strat5.txt"); |
antbig | 12:14729d584500 | 350 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 351 | case 6: |
ClementBreteau | 14:c8fc06c4887f | 352 | strcpy(cheminFileStart,"/local/strat6.txt"); |
ClementBreteau | 14:c8fc06c4887f | 353 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 354 | case 7: |
ClementBreteau | 14:c8fc06c4887f | 355 | strcpy(cheminFileStart,"/local/strat7.txt"); |
ClementBreteau | 14:c8fc06c4887f | 356 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 357 | case 8: |
ClementBreteau | 14:c8fc06c4887f | 358 | strcpy(cheminFileStart,"/local/strat8.txt"); |
ClementBreteau | 14:c8fc06c4887f | 359 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 360 | case 9: |
ClementBreteau | 14:c8fc06c4887f | 361 | strcpy(cheminFileStart,"/local/strat9.txt"); |
ClementBreteau | 14:c8fc06c4887f | 362 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 363 | case 10: |
ClementBreteau | 14:c8fc06c4887f | 364 | strcpy(cheminFileStart,"/local/strat10.txt"); |
ClementBreteau | 14:c8fc06c4887f | 365 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 366 | |
ClementBreteau | 15:c2fc239e85df | 367 | // strat de demo |
ClementBreteau | 14:c8fc06c4887f | 368 | case 0x10: |
ClementBreteau | 14:c8fc06c4887f | 369 | strcpy(cheminFileStart,"/local/moteur.txt"); |
ClementBreteau | 14:c8fc06c4887f | 370 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 371 | case 0x11: |
ClementBreteau | 15:c2fc239e85df | 372 | #ifdef ROBOT_BIG |
ClementBreteau | 15:c2fc239e85df | 373 | strcpy(cheminFileStart,"/local/bras.txt"); |
ClementBreteau | 15:c2fc239e85df | 374 | #else |
ClementBreteau | 15:c2fc239e85df | 375 | strcpy(cheminFileStart,"/local/porteAvant.txt"); |
ClementBreteau | 15:c2fc239e85df | 376 | #endif |
ClementBreteau | 14:c8fc06c4887f | 377 | return FileExists(cheminFileStart); |
ClementBreteau | 15:c2fc239e85df | 378 | case 0x12: |
ClementBreteau | 15:c2fc239e85df | 379 | #ifdef ROBOT_BIG |
ClementBreteau | 15:c2fc239e85df | 380 | strcpy(cheminFileStart,"/local/balancier.txt"); |
ClementBreteau | 15:c2fc239e85df | 381 | #else |
ClementBreteau | 15:c2fc239e85df | 382 | strcpy(cheminFileStart,"/local/mainTourneuse.txt"); |
ClementBreteau | 15:c2fc239e85df | 383 | #endif |
ClementBreteau | 15:c2fc239e85df | 384 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 385 | default: |
antbig | 12:14729d584500 | 386 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 11:ed13a480ddca | 387 | return 0; |
antbig | 4:88431b537477 | 388 | } |
antbig | 4:88431b537477 | 389 | } |
antbig | 4:88431b537477 | 390 | |
antbig | 12:14729d584500 | 391 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 392 | /* FUNCTION NAME: needToStop */ |
antbig | 12:14729d584500 | 393 | /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */ |
antbig | 12:14729d584500 | 394 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 395 | unsigned char needToStop(void) |
antbig | 12:14729d584500 | 396 | { |
antbig | 12:14729d584500 | 397 | return isStopEnable; |
antbig | 12:14729d584500 | 398 | } |
antbig | 12:14729d584500 | 399 | |
antbig | 12:14729d584500 | 400 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 401 | /* FUNCTION NAME: doBeforeEndAction */ |
antbig | 12:14729d584500 | 402 | /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */ |
antbig | 12:14729d584500 | 403 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 404 | void doBeforeEndAction(void) |
antbig | 12:14729d584500 | 405 | { |
antbig | 12:14729d584500 | 406 | doAction(110,0,0);//Ouverture pince arrière haute |
antbig | 12:14729d584500 | 407 | doAction(112,0,0);//Ouverture pince arrière basse |
antbig | 12:14729d584500 | 408 | doAction(114,0,0);//Ouverture porte arrière |
antbig | 12:14729d584500 | 409 | doAction(100,0,0);//Ouvrir les portes avant |
antbig | 12:14729d584500 | 410 | doAction(102,0,0);//Remonter le peigne |
antbig | 12:14729d584500 | 411 | } |
antbig | 12:14729d584500 | 412 | |
antbig | 3:19f2285a4757 | 413 | #endif |