Strategie_13h30

Fork of CRAC-Strat_2017_homologation_gros_rob by CRAC Team

Committer:
matthieuvignon
Date:
Thu May 25 11:30:31 2017 +0000
Revision:
22:8dec8824a6f7
Parent:
21:65ba6e4d53fa
13h30_Strategie

Who changed what in which revision?

UserRevisionLine numberNew 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
ClementBreteau 21:65ba6e4d53fa 7 unsigned short telemetreDistance;
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 18:cc5fec34ed9c 13 //envoie de la funny action
ClementBreteau 18:cc5fec34ed9c 14 // 0x007, 01, 01
ClementBreteau 18:cc5fec34ed9c 15 CANMessage msgTx=CANMessage();
ClementBreteau 18:cc5fec34ed9c 16 msgTx.id=GLOBAL_FUNNY_ACTION;
ClementBreteau 18:cc5fec34ed9c 17 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 18 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 19 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 20 msgTx.data[0]=0x01;
ClementBreteau 18:cc5fec34ed9c 21 msgTx.data[1]=0x01;
ClementBreteau 18:cc5fec34ed9c 22 can1.write(msgTx);
antbig 0:ad97421fb1fb 23 }
antbig 0:ad97421fb1fb 24
antbig 0:ad97421fb1fb 25 /****************************************************************************************/
antbig 0:ad97421fb1fb 26 /* FUNCTION NAME: doAction */
antbig 0:ad97421fb1fb 27 /* DESCRIPTION : Effectuer une action specifique */
antbig 0:ad97421fb1fb 28 /****************************************************************************************/
antbig 0:ad97421fb1fb 29 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
antbig 12:14729d584500 30 CANMessage msgTx=CANMessage();
ClementBreteau 18:cc5fec34ed9c 31 int localData = 0, localData2;
antbig 0:ad97421fb1fb 32 switch(id) {
ClementBreteau 18:cc5fec34ed9c 33 case 100: // position initiale
ClementBreteau 18:cc5fec34ed9c 34 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 35 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 36 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 37 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 38
ClementBreteau 18:cc5fec34ed9c 39 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 40 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 41 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 42 }
ClementBreteau 18:cc5fec34ed9c 43
ClementBreteau 18:cc5fec34ed9c 44 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 45 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 46 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 47 break;
antbig 3:19f2285a4757 48
ClementBreteau 18:cc5fec34ed9c 49 case 101: /// preparation prise
ClementBreteau 18:cc5fec34ed9c 50 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 51 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 52 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 53 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 54
ClementBreteau 18:cc5fec34ed9c 55 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 56 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 57 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 58 }
ClementBreteau 18:cc5fec34ed9c 59
ClementBreteau 18:cc5fec34ed9c 60 msgTx.data[0]=2;
ClementBreteau 18:cc5fec34ed9c 61 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 62 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 63 break;
ClementBreteau 18:cc5fec34ed9c 64
ClementBreteau 18:cc5fec34ed9c 65 case 102: // stockage haut
ClementBreteau 15:c2fc239e85df 66 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 67 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 68 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 69 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 70
ClementBreteau 15:c2fc239e85df 71 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 72 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 73 else speed = BRAS_AVANT;
ClementBreteau 15:c2fc239e85df 74 }
ClementBreteau 18:cc5fec34ed9c 75
ClementBreteau 18:cc5fec34ed9c 76 msgTx.data[0]=3;
ClementBreteau 15:c2fc239e85df 77 msgTx.data[1]=speed;
ClementBreteau 15:c2fc239e85df 78 can1.write(msgTx);
antbig 12:14729d584500 79 break;
ClementBreteau 18:cc5fec34ed9c 80
ClementBreteau 18:cc5fec34ed9c 81 case 103: // stockage bas
ClementBreteau 18:cc5fec34ed9c 82 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 83 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 84 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 85 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 86
ClementBreteau 18:cc5fec34ed9c 87 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 88 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 89 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 90 }
ClementBreteau 18:cc5fec34ed9c 91
ClementBreteau 18:cc5fec34ed9c 92 msgTx.data[0]=4;
ClementBreteau 18:cc5fec34ed9c 93 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 94 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 95 break;
ClementBreteau 18:cc5fec34ed9c 96
ClementBreteau 18:cc5fec34ed9c 97 case 104: // pousser module
ClementBreteau 18:cc5fec34ed9c 98 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 99 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 100 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 101 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 102
ClementBreteau 18:cc5fec34ed9c 103 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 104 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 105 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 106 }
ClementBreteau 18:cc5fec34ed9c 107
ClementBreteau 18:cc5fec34ed9c 108 msgTx.data[0]=7;
ClementBreteau 18:cc5fec34ed9c 109 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 110 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 111 break;
ClementBreteau 18:cc5fec34ed9c 112
ClementBreteau 18:cc5fec34ed9c 113 case 105: // preparation prise
ClementBreteau 15:c2fc239e85df 114 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 115 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 116 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 117 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 118
ClementBreteau 18:cc5fec34ed9c 119 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 120 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 121 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 122 }
ClementBreteau 18:cc5fec34ed9c 123
ClementBreteau 18:cc5fec34ed9c 124 msgTx.data[0]=6;
ClementBreteau 15:c2fc239e85df 125 msgTx.data[1]=speed;
ClementBreteau 15:c2fc239e85df 126 can1.write(msgTx);
antbig 12:14729d584500 127 break;
antbig 12:14729d584500 128
ClementBreteau 18:cc5fec34ed9c 129 case 106: //deposer module
ClementBreteau 18:cc5fec34ed9c 130 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 131 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 132 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 133 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 134
ClementBreteau 18:cc5fec34ed9c 135 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 136 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 137 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 138 }
ClementBreteau 18:cc5fec34ed9c 139
ClementBreteau 18:cc5fec34ed9c 140 msgTx.data[0]=5;
ClementBreteau 18:cc5fec34ed9c 141 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 142 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 143 break;
ClementBreteau 18:cc5fec34ed9c 144
ClementBreteau 18:cc5fec34ed9c 145 case 110: // repos bras avant
ClementBreteau 18:cc5fec34ed9c 146 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 147 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 148 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 149 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 150 speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 151 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 152 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 153 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 154 }
ClementBreteau 18:cc5fec34ed9c 155 if (speed == BRAS_AVANT)localData = 0x10;
ClementBreteau 18:cc5fec34ed9c 156 else localData = 0x24;
ClementBreteau 18:cc5fec34ed9c 157
ClementBreteau 18:cc5fec34ed9c 158 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 159 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 160 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 161 break;
ClementBreteau 18:cc5fec34ed9c 162
ClementBreteau 18:cc5fec34ed9c 163 case 111: // init bras module
ClementBreteau 15:c2fc239e85df 164 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 165 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 166 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 167 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 168 speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 169 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 170 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 171 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 172 }
ClementBreteau 18:cc5fec34ed9c 173 if (speed == BRAS_AVANT)localData = 0x0A;
ClementBreteau 18:cc5fec34ed9c 174 else localData = 0x1E;
ClementBreteau 18:cc5fec34ed9c 175
ClementBreteau 18:cc5fec34ed9c 176 msgTx.data[0]=localData;
ClementBreteau 15:c2fc239e85df 177 msgTx.data[1]=speed;
ClementBreteau 15:c2fc239e85df 178 can1.write(msgTx);
antbig 12:14729d584500 179 break;
ClementBreteau 18:cc5fec34ed9c 180
ClementBreteau 18:cc5fec34ed9c 181 case 112: // bouger module avant la gouttière
ClementBreteau 15:c2fc239e85df 182 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 183 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 184 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 185 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 186 speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 187 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 188 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 189 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 190 }
ClementBreteau 18:cc5fec34ed9c 191 if (speed == BRAS_AVANT)localData = 0x0B;
ClementBreteau 18:cc5fec34ed9c 192 else localData = 0x1F;
ClementBreteau 18:cc5fec34ed9c 193
ClementBreteau 18:cc5fec34ed9c 194 msgTx.data[0]=localData;
ClementBreteau 15:c2fc239e85df 195 msgTx.data[1]=speed;
ClementBreteau 15:c2fc239e85df 196 can1.write(msgTx);
antbig 12:14729d584500 197 break;
antbig 12:14729d584500 198
ClementBreteau 18:cc5fec34ed9c 199 case 113: // deposer module
ClementBreteau 15:c2fc239e85df 200 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 201 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 202 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 203 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 204 localData2 = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 205
ClementBreteau 18:cc5fec34ed9c 206 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 207 if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 208 else localData2 = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 209 }
ClementBreteau 18:cc5fec34ed9c 210
ClementBreteau 18:cc5fec34ed9c 211 if(localData2 == BRAS_AVANT){
ClementBreteau 18:cc5fec34ed9c 212 if(speed == INV){
ClementBreteau 18:cc5fec34ed9c 213 localData = 0x0E;
ClementBreteau 18:cc5fec34ed9c 214 }else{
ClementBreteau 18:cc5fec34ed9c 215 localData = 0x0C;
ClementBreteau 18:cc5fec34ed9c 216 }
ClementBreteau 18:cc5fec34ed9c 217 }else{
ClementBreteau 18:cc5fec34ed9c 218 if(speed == INV){
ClementBreteau 18:cc5fec34ed9c 219 localData = 0x22;
ClementBreteau 18:cc5fec34ed9c 220 }else{
ClementBreteau 18:cc5fec34ed9c 221 localData = 0x20;
ClementBreteau 18:cc5fec34ed9c 222 }
ClementBreteau 18:cc5fec34ed9c 223 }
ClementBreteau 18:cc5fec34ed9c 224
ClementBreteau 18:cc5fec34ed9c 225 msgTx.data[0]=localData;
SquirrelGod 19:b4b91258c275 226 msgTx.data[1]=localData2;
ClementBreteau 15:c2fc239e85df 227 can1.write(msgTx);
antbig 12:14729d584500 228 break;
ClementBreteau 18:cc5fec34ed9c 229
ClementBreteau 18:cc5fec34ed9c 230 case 114: // repositionner bras avant
ClementBreteau 15:c2fc239e85df 231 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 232 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 233 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 234 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 235 speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 236 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 237 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 238 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 239 }
ClementBreteau 18:cc5fec34ed9c 240 if (speed == BRAS_AVANT)localData = 0x0F;
ClementBreteau 18:cc5fec34ed9c 241 else localData = 0x23;
ClementBreteau 18:cc5fec34ed9c 242
ClementBreteau 18:cc5fec34ed9c 243 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 244 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 245 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 246 break;
ClementBreteau 15:c2fc239e85df 247
ClementBreteau 18:cc5fec34ed9c 248 case 115: // pompe avant ON
ClementBreteau 18:cc5fec34ed9c 249 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 250 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 251 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 252 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 253 localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 254 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 255 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 256 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 257 }
ClementBreteau 18:cc5fec34ed9c 258
ClementBreteau 18:cc5fec34ed9c 259 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 260 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 261
ClementBreteau 18:cc5fec34ed9c 262 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 263 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 264 can1.write(msgTx);
matthieuvignon 22:8dec8824a6f7 265 waitingAckID= 0;
matthieuvignon 22:8dec8824a6f7 266 waitingAckFrom = 0;
ClementBreteau 18:cc5fec34ed9c 267 break;
ClementBreteau 15:c2fc239e85df 268
ClementBreteau 18:cc5fec34ed9c 269 case 116: // pompe avant OFF
ClementBreteau 18:cc5fec34ed9c 270 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 271 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 272 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 273 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 274 localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 275 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 276 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 277 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 278 }
ClementBreteau 18:cc5fec34ed9c 279
ClementBreteau 18:cc5fec34ed9c 280 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 281 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 282
ClementBreteau 18:cc5fec34ed9c 283 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 284 wait_ms(1);
ClementBreteau 15:c2fc239e85df 285 can1.write(msgTx);
matthieuvignon 22:8dec8824a6f7 286 waitingAckID= 0;
matthieuvignon 22:8dec8824a6f7 287 waitingAckFrom = 0;
antbig 12:14729d584500 288 break;
ClementBreteau 18:cc5fec34ed9c 289
ClementBreteau 18:cc5fec34ed9c 290 case 120: // repos bras arrière
ClementBreteau 18:cc5fec34ed9c 291 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 292 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 293 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 294 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 295 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 296 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 297 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 298 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 299 }
ClementBreteau 18:cc5fec34ed9c 300 if (speed == BRAS_AVANT)localData = 0x10;
ClementBreteau 18:cc5fec34ed9c 301 else localData = 0x24;
ClementBreteau 18:cc5fec34ed9c 302
ClementBreteau 18:cc5fec34ed9c 303 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 304 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 305 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 306 break;
ClementBreteau 18:cc5fec34ed9c 307
ClementBreteau 18:cc5fec34ed9c 308 case 121: // position d'init avant de prendre un module bras arriere
ClementBreteau 18:cc5fec34ed9c 309 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 310 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 311 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 312 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 313 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 314 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 315 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 316 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 317 }
ClementBreteau 18:cc5fec34ed9c 318 if (speed == BRAS_AVANT)localData = 0xA0;
ClementBreteau 18:cc5fec34ed9c 319 else localData = 0x1E;
ClementBreteau 18:cc5fec34ed9c 320
ClementBreteau 18:cc5fec34ed9c 321 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 322 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 323 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 324 break;
ClementBreteau 18:cc5fec34ed9c 325
ClementBreteau 18:cc5fec34ed9c 326 case 122: // bouger le bras arriere avant la gouttiere
ClementBreteau 18:cc5fec34ed9c 327 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 328 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 329 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 330 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 331 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 332 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 333 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 334 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 335 }
ClementBreteau 18:cc5fec34ed9c 336 if (speed == BRAS_AVANT)localData = 0x0B;
ClementBreteau 18:cc5fec34ed9c 337 else localData = 0x1F;
ClementBreteau 18:cc5fec34ed9c 338
ClementBreteau 18:cc5fec34ed9c 339 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 340 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 341 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 342 break;
ClementBreteau 18:cc5fec34ed9c 343
ClementBreteau 18:cc5fec34ed9c 344 case 123: // deposer module
ClementBreteau 15:c2fc239e85df 345 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 346 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 347 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 348 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 349 localData2 = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 350
ClementBreteau 18:cc5fec34ed9c 351 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 352 if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 353 else localData2 = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 354 }
ClementBreteau 18:cc5fec34ed9c 355
ClementBreteau 18:cc5fec34ed9c 356 if(localData2 == BRAS_AVANT){
ClementBreteau 18:cc5fec34ed9c 357 if(speed == INV){
ClementBreteau 18:cc5fec34ed9c 358 localData = 0x0E;
ClementBreteau 18:cc5fec34ed9c 359 }else{
ClementBreteau 18:cc5fec34ed9c 360 localData = 0x0C;
ClementBreteau 18:cc5fec34ed9c 361 }
ClementBreteau 18:cc5fec34ed9c 362 }else{
ClementBreteau 18:cc5fec34ed9c 363 if(speed == INV){
ClementBreteau 18:cc5fec34ed9c 364 localData = 0x22;
ClementBreteau 18:cc5fec34ed9c 365 }else{
ClementBreteau 18:cc5fec34ed9c 366 localData = 0x20;
ClementBreteau 18:cc5fec34ed9c 367 }
ClementBreteau 18:cc5fec34ed9c 368 }
ClementBreteau 18:cc5fec34ed9c 369
ClementBreteau 18:cc5fec34ed9c 370 msgTx.data[0]=localData;
matthieuvignon 22:8dec8824a6f7 371 msgTx.data[1]=localData2;
ClementBreteau 15:c2fc239e85df 372 can1.write(msgTx);
antbig 12:14729d584500 373 break;
antbig 12:14729d584500 374
ClementBreteau 18:cc5fec34ed9c 375 case 124: // remettre le bras arriere en posiion initiale
ClementBreteau 15:c2fc239e85df 376 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 377 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 378 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 379 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 380 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 381 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 382 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 383 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 384 }
ClementBreteau 18:cc5fec34ed9c 385 if (speed == BRAS_AVANT)localData = 0x0F;
ClementBreteau 18:cc5fec34ed9c 386 else localData = 0x23;
ClementBreteau 18:cc5fec34ed9c 387
ClementBreteau 18:cc5fec34ed9c 388 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 389 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 390 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 391 break;
antbig 8:0edc7dfb7f7e 392
ClementBreteau 18:cc5fec34ed9c 393 case 125: // pompe pwm arriere ON
ClementBreteau 18:cc5fec34ed9c 394 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 395 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 396 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 397 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 398 localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 399 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 400 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 401 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 402 }
ClementBreteau 18:cc5fec34ed9c 403
ClementBreteau 18:cc5fec34ed9c 404 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 405 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 406
ClementBreteau 18:cc5fec34ed9c 407 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 408 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 409 can1.write(msgTx);
matthieuvignon 22:8dec8824a6f7 410
matthieuvignon 22:8dec8824a6f7 411 waitingAckID= 0;
matthieuvignon 22:8dec8824a6f7 412 waitingAckFrom = 0;
ClementBreteau 18:cc5fec34ed9c 413 break;
ClementBreteau 18:cc5fec34ed9c 414
ClementBreteau 18:cc5fec34ed9c 415 case 126: // pompe arriere pwm OFF
ClementBreteau 18:cc5fec34ed9c 416 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 417 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 418 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 419 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 420 localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 421 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 422 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 423 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 424 }
ClementBreteau 18:cc5fec34ed9c 425
ClementBreteau 18:cc5fec34ed9c 426 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 427 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 428
ClementBreteau 18:cc5fec34ed9c 429 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 430 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 431 can1.write(msgTx);
matthieuvignon 22:8dec8824a6f7 432 waitingAckID= 0;
matthieuvignon 22:8dec8824a6f7 433 waitingAckFrom = 0;
ClementBreteau 18:cc5fec34ed9c 434 break;
ClementBreteau 18:cc5fec34ed9c 435
ClementBreteau 18:cc5fec34ed9c 436 case 130: // baiser bras turbine
ClementBreteau 18:cc5fec34ed9c 437 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 438 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 439 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 440 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 441 msgTx.data[0]=0x14;
ClementBreteau 18:cc5fec34ed9c 442 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 443 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 444 break;
ClementBreteau 15:c2fc239e85df 445
ClementBreteau 18:cc5fec34ed9c 446 case 131: // lever bras turbine
ClementBreteau 18:cc5fec34ed9c 447 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 448 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 449 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 450 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 451 msgTx.data[0]=0x15;
ClementBreteau 18:cc5fec34ed9c 452 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 453 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 454 break;
ClementBreteau 18:cc5fec34ed9c 455
ClementBreteau 18:cc5fec34ed9c 456 case 132: // allumer turbine
ClementBreteau 18:cc5fec34ed9c 457 msgTx.id=0x403;
ClementBreteau 18:cc5fec34ed9c 458 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 459 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 460 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 461 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 462 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 463 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 464 break;
ClementBreteau 18:cc5fec34ed9c 465
ClementBreteau 18:cc5fec34ed9c 466 case 133: // stop turbine
ClementBreteau 18:cc5fec34ed9c 467 msgTx.id=0x403;
ClementBreteau 18:cc5fec34ed9c 468 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 469 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 470 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 471 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 472 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 473 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 474 break;
ClementBreteau 18:cc5fec34ed9c 475
ClementBreteau 18:cc5fec34ed9c 476 case 140:// lanceur ON
ClementBreteau 18:cc5fec34ed9c 477 msgTx.id=0x402;
ClementBreteau 18:cc5fec34ed9c 478 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 479 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 480 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 481
ClementBreteau 18:cc5fec34ed9c 482 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 483 msgTx.data[1]=0x02;
ClementBreteau 18:cc5fec34ed9c 484 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 485 break;
ClementBreteau 18:cc5fec34ed9c 486
ClementBreteau 18:cc5fec34ed9c 487 case 141: // lanceur OFF
ClementBreteau 18:cc5fec34ed9c 488 msgTx.id=0x402;
ClementBreteau 18:cc5fec34ed9c 489 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 490 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 491 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 492
ClementBreteau 18:cc5fec34ed9c 493 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 494 msgTx.data[1]=0x02;
ClementBreteau 15:c2fc239e85df 495 can1.write(msgTx);
antbig 8:0edc7dfb7f7e 496 break;
ClementBreteau 15:c2fc239e85df 497
ClementBreteau 18:cc5fec34ed9c 498 case 150:
ClementBreteau 18:cc5fec34ed9c 499 msgTx.id=0x404;
antbig 12:14729d584500 500 msgTx.format=CANStandard;
antbig 12:14729d584500 501 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 502 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 503
ClementBreteau 18:cc5fec34ed9c 504 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 505 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 506
antbig 12:14729d584500 507 can1.write(msgTx);
antbig 8:0edc7dfb7f7e 508 break;
ClementBreteau 18:cc5fec34ed9c 509
ClementBreteau 18:cc5fec34ed9c 510 case 151:
ClementBreteau 18:cc5fec34ed9c 511 msgTx.id=0x404;
antbig 12:14729d584500 512 msgTx.format=CANStandard;
antbig 12:14729d584500 513 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 514 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 515
ClementBreteau 18:cc5fec34ed9c 516 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 517 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 518
antbig 12:14729d584500 519 can1.write(msgTx);
antbig 12:14729d584500 520 break;
antbig 12:14729d584500 521
antbig 12:14729d584500 522 case 10://Désactiver le stop
antbig 12:14729d584500 523 isStopEnable = 0;
antbig 12:14729d584500 524 break;
antbig 12:14729d584500 525 case 11://Activer le stop
antbig 12:14729d584500 526 isStopEnable = 1;
antbig 12:14729d584500 527 break;
antbig 12:14729d584500 528 case 20://Désactiver l'asservissement
antbig 12:14729d584500 529 setAsservissementEtat(0);
antbig 12:14729d584500 530 break;
antbig 12:14729d584500 531 case 21://Activer l'asservissement
antbig 12:14729d584500 532 setAsservissementEtat(1);
antbig 12:14729d584500 533 break;
antbig 12:14729d584500 534
antbig 12:14729d584500 535 case 22://Changer la vitesse du robot
antbig 12:14729d584500 536 SendSpeed(speed,(unsigned short)angle);
antbig 12:14729d584500 537 break;
antbig 12:14729d584500 538
antbig 12:14729d584500 539 case 30://Action tempo
antbig 12:14729d584500 540 wait_ms(speed);
antbig 12:14729d584500 541 break;
antbig 12:14729d584500 542
ClementBreteau 18:cc5fec34ed9c 543 /*case 40: // demande au telemetre la position d'un objet
ClementBreteau 14:c8fc06c4887f 544 //SendRawId(TELEMETRE_RECHERCHE_OBJET);
ClementBreteau 15:c2fc239e85df 545
ClementBreteau 15:c2fc239e85df 546 modeTelemetre = 1;
ClementBreteau 15:c2fc239e85df 547
ClementBreteau 15:c2fc239e85df 548 //angle = angle /10;
ClementBreteau 15:c2fc239e85df 549
ClementBreteau 15:c2fc239e85df 550 msgTx.id=TELEMETRE_OBJET;
ClementBreteau 15:c2fc239e85df 551 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 552 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 553 msgTx.len=1;
ClementBreteau 15:c2fc239e85df 554 // indice du module sur le terrain
ClementBreteau 15:c2fc239e85df 555 msgTx.data[0] = (unsigned char)speed;
ClementBreteau 15:c2fc239e85df 556
ClementBreteau 18:cc5fec34ed9c 557
ClementBreteau 15:c2fc239e85df 558 // x sur 2 octets
ClementBreteau 15:c2fc239e85df 559 msgTx.data[0]=(unsigned char)speed;
ClementBreteau 15:c2fc239e85df 560 msgTx.data[1]=(unsigned char)(speed>>8);
ClementBreteau 15:c2fc239e85df 561 // y sur 2 octets
ClementBreteau 15:c2fc239e85df 562 msgTx.data[2]=(unsigned char)angle;
ClementBreteau 15:c2fc239e85df 563 msgTx.data[3]=(unsigned char)(angle>>8);
ClementBreteau 15:c2fc239e85df 564 // theta signé sur 2 octets
ClementBreteau 15:c2fc239e85df 565 //msgTx.data[4]=(unsigned char)theta;
ClementBreteau 15:c2fc239e85df 566 //msgTx.data[5]=(unsigned char)(theta>>8);
ClementBreteau 15:c2fc239e85df 567 msgTx.data[4]=0;
ClementBreteau 18:cc5fec34ed9c 568 msgTx.data[5]=0;
ClementBreteau 15:c2fc239e85df 569
ClementBreteau 15:c2fc239e85df 570 can1.write(msgTx);
ClementBreteau 15:c2fc239e85df 571
ClementBreteau 18:cc5fec34ed9c 572 break;*/
ClementBreteau 14:c8fc06c4887f 573
ClementBreteau 18:cc5fec34ed9c 574 /* case 130://Lancer mouvement de sortie de la zone de départ
ClementBreteau 15:c2fc239e85df 575 msgTx.id=ACTION_BIG_DEMARRAGE;
ClementBreteau 15:c2fc239e85df 576 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 577 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 578 msgTx.len=1;
ClementBreteau 15:c2fc239e85df 579 msgTx.data[0] = (unsigned char)speed;
ClementBreteau 15:c2fc239e85df 580 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 581 break;*/
antbig 12:14729d584500 582
antbig 0:ad97421fb1fb 583 default:
antbig 0:ad97421fb1fb 584 return 0;//L'action n'existe pas, il faut utiliser le CAN
antbig 0:ad97421fb1fb 585
antbig 0:ad97421fb1fb 586 }
antbig 0:ad97421fb1fb 587 return 1;//L'action est spécifique.
antbig 0:ad97421fb1fb 588
antbig 0:ad97421fb1fb 589 }
antbig 0:ad97421fb1fb 590
antbig 0:ad97421fb1fb 591 /****************************************************************************************/
antbig 0:ad97421fb1fb 592 /* FUNCTION NAME: initRobot */
antbig 0:ad97421fb1fb 593 /* DESCRIPTION : initialiser le robot */
antbig 0:ad97421fb1fb 594 /****************************************************************************************/
antbig 9:d0042422d95a 595 void initRobot(void)
antbig 9:d0042422d95a 596 {
antbig 9:d0042422d95a 597 //Enregistrement de tous les AX12 présent sur la carte
ClementBreteau 15:c2fc239e85df 598 /*AX12_register(5, AX12_SERIAL2);
antbig 8:0edc7dfb7f7e 599 AX12_register(18, AX12_SERIAL2);
antbig 8:0edc7dfb7f7e 600 AX12_register(13, AX12_SERIAL2);
antbig 12:14729d584500 601 AX12_register(1, AX12_SERIAL1);
antbig 12:14729d584500 602 AX12_register(11, AX12_SERIAL1);
antbig 12:14729d584500 603 AX12_register(8, AX12_SERIAL1);
ClementBreteau 15:c2fc239e85df 604 AX12_register(7, AX12_SERIAL2);*/
antbig 9:d0042422d95a 605
antbig 12:14729d584500 606 //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
antbig 12:14729d584500 607 //AX12_processChange();
antbig 11:ed13a480ddca 608 //runRobotTest();
ClementBreteau 16:7321fb3bb396 609
antbig 9:d0042422d95a 610 }
antbig 9:d0042422d95a 611
antbig 9:d0042422d95a 612 /****************************************************************************************/
antbig 12:14729d584500 613 /* FUNCTION NAME: initRobotActionneur */
antbig 12:14729d584500 614 /* DESCRIPTION : Initialiser la position des actionneurs du robot */
antbig 12:14729d584500 615 /****************************************************************************************/
antbig 12:14729d584500 616 void initRobotActionneur(void)
antbig 12:14729d584500 617 {
ClementBreteau 21:65ba6e4d53fa 618
antbig 12:14729d584500 619 }
antbig 12:14729d584500 620
antbig 12:14729d584500 621 /****************************************************************************************/
antbig 9:d0042422d95a 622 /* FUNCTION NAME: runTest */
antbig 9:d0042422d95a 623 /* DESCRIPTION : tester l'ensemble des actionneurs du robot */
antbig 9:d0042422d95a 624 /****************************************************************************************/
antbig 9:d0042422d95a 625 void runRobotTest(void)
antbig 9:d0042422d95a 626 {
ClementBreteau 18:cc5fec34ed9c 627 /*
antbig 12:14729d584500 628 int waitTime = 500;
antbig 12:14729d584500 629
antbig 9:d0042422d95a 630 //Test des AX12 dans l'ordre
antbig 9:d0042422d95a 631 doAction(111,0,0);//Fermeture pince arrière haute
antbig 12:14729d584500 632 wait_ms(waitTime);
antbig 9:d0042422d95a 633 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 634 wait_ms(waitTime);
antbig 9:d0042422d95a 635 doAction(113,0,0);//Fermeture pince arrière basse
antbig 12:14729d584500 636 wait_ms(waitTime);
antbig 9:d0042422d95a 637 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 638 wait_ms(waitTime);
antbig 9:d0042422d95a 639 doAction(115,0,0);//Fermeture porte arrière
antbig 12:14729d584500 640 wait_ms(waitTime);
antbig 9:d0042422d95a 641 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 642 wait_ms(waitTime);
antbig 12:14729d584500 643 doAction(101,0,0);//Fermer les portes avant
antbig 12:14729d584500 644 wait_ms(waitTime);
antbig 12:14729d584500 645 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 646 wait_ms(waitTime);
antbig 12:14729d584500 647 doAction(103,0,0);//Descendre le peigne
antbig 12:14729d584500 648 wait_ms(waitTime);
ClementBreteau 18:cc5fec34ed9c 649 doAction(102,0,0);//Remonter le peigne*/
antbig 0:ad97421fb1fb 650 }
antbig 3:19f2285a4757 651
antbig 4:88431b537477 652 /****************************************************************************************/
antbig 4:88431b537477 653 /* FUNCTION NAME: SelectStrategy */
antbig 4:88431b537477 654 /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
antbig 4:88431b537477 655 /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
antbig 4:88431b537477 656 /****************************************************************************************/
antbig 4:88431b537477 657 int SelectStrategy(unsigned char id)
antbig 4:88431b537477 658 {
antbig 4:88431b537477 659 switch(id)
antbig 4:88431b537477 660 {
ClementBreteau 15:c2fc239e85df 661 // strat de match
antbig 4:88431b537477 662 case 1:
antbig 11:ed13a480ddca 663 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 11:ed13a480ddca 664 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 665 case 2:
antbig 11:ed13a480ddca 666 strcpy(cheminFileStart,"/local/strat2.txt");
antbig 11:ed13a480ddca 667 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 668 case 3:
antbig 11:ed13a480ddca 669 strcpy(cheminFileStart,"/local/strat3.txt");
antbig 11:ed13a480ddca 670 return FileExists(cheminFileStart);
antbig 12:14729d584500 671 case 4:
antbig 12:14729d584500 672 strcpy(cheminFileStart,"/local/strat4.txt");
antbig 12:14729d584500 673 return FileExists(cheminFileStart);
antbig 12:14729d584500 674 case 5:
antbig 12:14729d584500 675 strcpy(cheminFileStart,"/local/strat5.txt");
antbig 12:14729d584500 676 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 677 case 6:
ClementBreteau 14:c8fc06c4887f 678 strcpy(cheminFileStart,"/local/strat6.txt");
ClementBreteau 14:c8fc06c4887f 679 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 680 case 7:
ClementBreteau 14:c8fc06c4887f 681 strcpy(cheminFileStart,"/local/strat7.txt");
ClementBreteau 14:c8fc06c4887f 682 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 683 case 8:
ClementBreteau 14:c8fc06c4887f 684 strcpy(cheminFileStart,"/local/strat8.txt");
ClementBreteau 14:c8fc06c4887f 685 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 686 case 9:
ClementBreteau 14:c8fc06c4887f 687 strcpy(cheminFileStart,"/local/strat9.txt");
ClementBreteau 14:c8fc06c4887f 688 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 689 case 10:
ClementBreteau 14:c8fc06c4887f 690 strcpy(cheminFileStart,"/local/strat10.txt");
ClementBreteau 14:c8fc06c4887f 691 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 692
ClementBreteau 15:c2fc239e85df 693 // strat de demo
ClementBreteau 14:c8fc06c4887f 694 case 0x10:
ClementBreteau 14:c8fc06c4887f 695 strcpy(cheminFileStart,"/local/moteur.txt");
ClementBreteau 14:c8fc06c4887f 696 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 697 case 0x11:
ClementBreteau 15:c2fc239e85df 698 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 699 strcpy(cheminFileStart,"/local/bras.txt");
ClementBreteau 15:c2fc239e85df 700 #else
ClementBreteau 15:c2fc239e85df 701 strcpy(cheminFileStart,"/local/porteAvant.txt");
ClementBreteau 15:c2fc239e85df 702 #endif
ClementBreteau 14:c8fc06c4887f 703 return FileExists(cheminFileStart);
ClementBreteau 15:c2fc239e85df 704 case 0x12:
ClementBreteau 15:c2fc239e85df 705 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 706 strcpy(cheminFileStart,"/local/balancier.txt");
ClementBreteau 15:c2fc239e85df 707 #else
ClementBreteau 15:c2fc239e85df 708 strcpy(cheminFileStart,"/local/mainTourneuse.txt");
ClementBreteau 15:c2fc239e85df 709 #endif
ClementBreteau 15:c2fc239e85df 710 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 711 default:
antbig 12:14729d584500 712 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 11:ed13a480ddca 713 return 0;
antbig 4:88431b537477 714 }
antbig 4:88431b537477 715 }
antbig 4:88431b537477 716
antbig 12:14729d584500 717 /****************************************************************************************/
antbig 12:14729d584500 718 /* FUNCTION NAME: needToStop */
antbig 12:14729d584500 719 /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
antbig 12:14729d584500 720 /****************************************************************************************/
antbig 12:14729d584500 721 unsigned char needToStop(void)
antbig 12:14729d584500 722 {
antbig 12:14729d584500 723 return isStopEnable;
antbig 12:14729d584500 724 }
antbig 12:14729d584500 725
antbig 12:14729d584500 726 /****************************************************************************************/
antbig 12:14729d584500 727 /* FUNCTION NAME: doBeforeEndAction */
antbig 12:14729d584500 728 /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
antbig 12:14729d584500 729 /****************************************************************************************/
antbig 12:14729d584500 730 void doBeforeEndAction(void)
antbig 12:14729d584500 731 {
antbig 12:14729d584500 732 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 733 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 734 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 735 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 736 doAction(102,0,0);//Remonter le peigne
antbig 12:14729d584500 737 }
antbig 12:14729d584500 738
antbig 3:19f2285a4757 739 #endif