CRAC Team / CRAC-Strat_2017_homologation_gros_rob_testAck

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

Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by CRAC Team

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

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

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:7c60d6dfcab5 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);
ClementBreteau 18:cc5fec34ed9c 265 break;
ClementBreteau 15:c2fc239e85df 266
ClementBreteau 18:cc5fec34ed9c 267 case 116: // pompe avant OFF
ClementBreteau 18:cc5fec34ed9c 268 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 269 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 270 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 271 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 272 localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 273 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 274 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 275 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 276 }
ClementBreteau 18:cc5fec34ed9c 277
ClementBreteau 18:cc5fec34ed9c 278 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 279 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 280
ClementBreteau 18:cc5fec34ed9c 281 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 282 wait_ms(1);
ClementBreteau 15:c2fc239e85df 283 can1.write(msgTx);
antbig 12:14729d584500 284 break;
ClementBreteau 18:cc5fec34ed9c 285
ClementBreteau 18:cc5fec34ed9c 286 case 120: // repos bras arrière
ClementBreteau 18:cc5fec34ed9c 287 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 288 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 289 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 290 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 291 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 292 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 293 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 294 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 295 }
ClementBreteau 18:cc5fec34ed9c 296 if (speed == BRAS_AVANT)localData = 0x10;
ClementBreteau 18:cc5fec34ed9c 297 else localData = 0x24;
ClementBreteau 18:cc5fec34ed9c 298
ClementBreteau 18:cc5fec34ed9c 299 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 300 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 301 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 302 break;
ClementBreteau 18:cc5fec34ed9c 303
ClementBreteau 18:cc5fec34ed9c 304 case 121: // position d'init avant de prendre un module bras arriere
ClementBreteau 18:cc5fec34ed9c 305 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 306 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 307 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 308 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 309 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 310 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 311 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 312 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 313 }
ClementBreteau 18:cc5fec34ed9c 314 if (speed == BRAS_AVANT)localData = 0xA0;
ClementBreteau 18:cc5fec34ed9c 315 else localData = 0x1E;
ClementBreteau 18:cc5fec34ed9c 316
ClementBreteau 18:cc5fec34ed9c 317 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 318 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 319 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 320 break;
ClementBreteau 18:cc5fec34ed9c 321
ClementBreteau 18:cc5fec34ed9c 322 case 122: // bouger le bras arriere avant la gouttiere
ClementBreteau 18:cc5fec34ed9c 323 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 324 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 325 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 326 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 327 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 328 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 329 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 330 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 331 }
ClementBreteau 18:cc5fec34ed9c 332 if (speed == BRAS_AVANT)localData = 0x0B;
ClementBreteau 18:cc5fec34ed9c 333 else localData = 0x1F;
ClementBreteau 18:cc5fec34ed9c 334
ClementBreteau 18:cc5fec34ed9c 335 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 336 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 337 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 338 break;
ClementBreteau 18:cc5fec34ed9c 339
ClementBreteau 18:cc5fec34ed9c 340 case 123: // deposer module
ClementBreteau 15:c2fc239e85df 341 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 342 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 343 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 344 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 345 localData2 = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 346
ClementBreteau 18:cc5fec34ed9c 347 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 348 if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 349 else localData2 = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 350 }
ClementBreteau 18:cc5fec34ed9c 351
ClementBreteau 18:cc5fec34ed9c 352 if(localData2 == BRAS_AVANT){
ClementBreteau 18:cc5fec34ed9c 353 if(speed == INV){
ClementBreteau 18:cc5fec34ed9c 354 localData = 0x0E;
ClementBreteau 18:cc5fec34ed9c 355 }else{
ClementBreteau 18:cc5fec34ed9c 356 localData = 0x0C;
ClementBreteau 18:cc5fec34ed9c 357 }
ClementBreteau 18:cc5fec34ed9c 358 }else{
ClementBreteau 18:cc5fec34ed9c 359 if(speed == INV){
ClementBreteau 18:cc5fec34ed9c 360 localData = 0x22;
ClementBreteau 18:cc5fec34ed9c 361 }else{
ClementBreteau 18:cc5fec34ed9c 362 localData = 0x20;
ClementBreteau 18:cc5fec34ed9c 363 }
ClementBreteau 18:cc5fec34ed9c 364 }
ClementBreteau 18:cc5fec34ed9c 365
ClementBreteau 18:cc5fec34ed9c 366 msgTx.data[0]=localData;
ClementBreteau 21:7c60d6dfcab5 367 msgTx.data[1]=localData2;
ClementBreteau 15:c2fc239e85df 368 can1.write(msgTx);
antbig 12:14729d584500 369 break;
antbig 12:14729d584500 370
ClementBreteau 18:cc5fec34ed9c 371 case 124: // remettre le bras arriere en posiion initiale
ClementBreteau 15:c2fc239e85df 372 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 373 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 374 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 375 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 376 speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 377 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 378 if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 379 else speed = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 380 }
ClementBreteau 18:cc5fec34ed9c 381 if (speed == BRAS_AVANT)localData = 0x0F;
ClementBreteau 18:cc5fec34ed9c 382 else localData = 0x23;
ClementBreteau 18:cc5fec34ed9c 383
ClementBreteau 18:cc5fec34ed9c 384 msgTx.data[0]=localData;
ClementBreteau 18:cc5fec34ed9c 385 msgTx.data[1]=speed;
ClementBreteau 18:cc5fec34ed9c 386 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 387 break;
antbig 8:0edc7dfb7f7e 388
ClementBreteau 18:cc5fec34ed9c 389 case 125: // pompe pwm arriere ON
ClementBreteau 18:cc5fec34ed9c 390 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 391 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 392 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 393 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 394 localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 395 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 396 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 397 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 398 }
ClementBreteau 18:cc5fec34ed9c 399
ClementBreteau 18:cc5fec34ed9c 400 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 401 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 402
ClementBreteau 18:cc5fec34ed9c 403 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 404 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 405 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 406 break;
ClementBreteau 18:cc5fec34ed9c 407
ClementBreteau 18:cc5fec34ed9c 408 case 126: // pompe arriere pwm OFF
ClementBreteau 18:cc5fec34ed9c 409 msgTx.id=0x400;
ClementBreteau 18:cc5fec34ed9c 410 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 411 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 412 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 413 localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 414 if (InversStrat){ // si on est inversé, on echange les bras
ClementBreteau 18:cc5fec34ed9c 415 if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
ClementBreteau 18:cc5fec34ed9c 416 else localData = BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 417 }
ClementBreteau 18:cc5fec34ed9c 418
ClementBreteau 18:cc5fec34ed9c 419 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 420 msgTx.data[1]=localData;
ClementBreteau 18:cc5fec34ed9c 421
ClementBreteau 18:cc5fec34ed9c 422 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 423 wait_ms(1);
ClementBreteau 18:cc5fec34ed9c 424 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 425 break;
ClementBreteau 18:cc5fec34ed9c 426
ClementBreteau 18:cc5fec34ed9c 427 case 130: // baiser bras turbine
ClementBreteau 18:cc5fec34ed9c 428 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 429 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 430 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 431 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 432 msgTx.data[0]=0x14;
ClementBreteau 18:cc5fec34ed9c 433 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 434 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 435 break;
ClementBreteau 15:c2fc239e85df 436
ClementBreteau 18:cc5fec34ed9c 437 case 131: // lever bras turbine
ClementBreteau 18:cc5fec34ed9c 438 msgTx.id=SERVO_AX12_ACTION;
ClementBreteau 18:cc5fec34ed9c 439 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 440 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 441 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 442 msgTx.data[0]=0x15;
ClementBreteau 18:cc5fec34ed9c 443 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 444 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 445 break;
ClementBreteau 18:cc5fec34ed9c 446
ClementBreteau 18:cc5fec34ed9c 447 case 132: // allumer turbine
ClementBreteau 18:cc5fec34ed9c 448 msgTx.id=0x403;
ClementBreteau 18:cc5fec34ed9c 449 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 450 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 451 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 452 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 453 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 454 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 455 break;
ClementBreteau 18:cc5fec34ed9c 456
ClementBreteau 18:cc5fec34ed9c 457 case 133: // stop turbine
ClementBreteau 18:cc5fec34ed9c 458 msgTx.id=0x403;
ClementBreteau 18:cc5fec34ed9c 459 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 460 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 461 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 462 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 463 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 464 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 465 break;
ClementBreteau 18:cc5fec34ed9c 466
ClementBreteau 18:cc5fec34ed9c 467 case 140:// lanceur ON
ClementBreteau 18:cc5fec34ed9c 468 msgTx.id=0x402;
ClementBreteau 18:cc5fec34ed9c 469 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 470 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 471 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 472
ClementBreteau 18:cc5fec34ed9c 473 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 474 msgTx.data[1]=0x02;
ClementBreteau 18:cc5fec34ed9c 475 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 476 break;
ClementBreteau 18:cc5fec34ed9c 477
ClementBreteau 18:cc5fec34ed9c 478 case 141: // lanceur OFF
ClementBreteau 18:cc5fec34ed9c 479 msgTx.id=0x402;
ClementBreteau 18:cc5fec34ed9c 480 msgTx.format=CANStandard;
ClementBreteau 18:cc5fec34ed9c 481 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 482 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 483
ClementBreteau 18:cc5fec34ed9c 484 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 485 msgTx.data[1]=0x02;
ClementBreteau 15:c2fc239e85df 486 can1.write(msgTx);
antbig 8:0edc7dfb7f7e 487 break;
ClementBreteau 15:c2fc239e85df 488
ClementBreteau 18:cc5fec34ed9c 489 case 150:
ClementBreteau 18:cc5fec34ed9c 490 msgTx.id=0x404;
antbig 12:14729d584500 491 msgTx.format=CANStandard;
antbig 12:14729d584500 492 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 493 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 494
ClementBreteau 18:cc5fec34ed9c 495 msgTx.data[0]=1;
ClementBreteau 18:cc5fec34ed9c 496 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 497
antbig 12:14729d584500 498 can1.write(msgTx);
antbig 8:0edc7dfb7f7e 499 break;
ClementBreteau 18:cc5fec34ed9c 500
ClementBreteau 18:cc5fec34ed9c 501 case 151:
ClementBreteau 18:cc5fec34ed9c 502 msgTx.id=0x404;
antbig 12:14729d584500 503 msgTx.format=CANStandard;
antbig 12:14729d584500 504 msgTx.type=CANData;
ClementBreteau 18:cc5fec34ed9c 505 msgTx.len=2;
ClementBreteau 18:cc5fec34ed9c 506
ClementBreteau 18:cc5fec34ed9c 507 msgTx.data[0]=0;
ClementBreteau 18:cc5fec34ed9c 508 msgTx.data[1]=BRAS_AVANT;
ClementBreteau 18:cc5fec34ed9c 509
antbig 12:14729d584500 510 can1.write(msgTx);
antbig 12:14729d584500 511 break;
antbig 12:14729d584500 512
antbig 12:14729d584500 513 case 10://Désactiver le stop
antbig 12:14729d584500 514 isStopEnable = 0;
antbig 12:14729d584500 515 break;
antbig 12:14729d584500 516 case 11://Activer le stop
antbig 12:14729d584500 517 isStopEnable = 1;
antbig 12:14729d584500 518 break;
antbig 12:14729d584500 519 case 20://Désactiver l'asservissement
antbig 12:14729d584500 520 setAsservissementEtat(0);
antbig 12:14729d584500 521 break;
antbig 12:14729d584500 522 case 21://Activer l'asservissement
antbig 12:14729d584500 523 setAsservissementEtat(1);
antbig 12:14729d584500 524 break;
antbig 12:14729d584500 525
antbig 12:14729d584500 526 case 22://Changer la vitesse du robot
antbig 12:14729d584500 527 SendSpeed(speed,(unsigned short)angle);
antbig 12:14729d584500 528 break;
antbig 12:14729d584500 529
antbig 12:14729d584500 530 case 30://Action tempo
antbig 12:14729d584500 531 wait_ms(speed);
antbig 12:14729d584500 532 break;
antbig 12:14729d584500 533
ClementBreteau 18:cc5fec34ed9c 534 /*case 40: // demande au telemetre la position d'un objet
ClementBreteau 14:c8fc06c4887f 535 //SendRawId(TELEMETRE_RECHERCHE_OBJET);
ClementBreteau 15:c2fc239e85df 536
ClementBreteau 15:c2fc239e85df 537 modeTelemetre = 1;
ClementBreteau 15:c2fc239e85df 538
ClementBreteau 15:c2fc239e85df 539 //angle = angle /10;
ClementBreteau 15:c2fc239e85df 540
ClementBreteau 15:c2fc239e85df 541 msgTx.id=TELEMETRE_OBJET;
ClementBreteau 15:c2fc239e85df 542 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 543 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 544 msgTx.len=1;
ClementBreteau 15:c2fc239e85df 545 // indice du module sur le terrain
ClementBreteau 15:c2fc239e85df 546 msgTx.data[0] = (unsigned char)speed;
ClementBreteau 15:c2fc239e85df 547
ClementBreteau 18:cc5fec34ed9c 548
ClementBreteau 15:c2fc239e85df 549 // x sur 2 octets
ClementBreteau 15:c2fc239e85df 550 msgTx.data[0]=(unsigned char)speed;
ClementBreteau 15:c2fc239e85df 551 msgTx.data[1]=(unsigned char)(speed>>8);
ClementBreteau 15:c2fc239e85df 552 // y sur 2 octets
ClementBreteau 15:c2fc239e85df 553 msgTx.data[2]=(unsigned char)angle;
ClementBreteau 15:c2fc239e85df 554 msgTx.data[3]=(unsigned char)(angle>>8);
ClementBreteau 15:c2fc239e85df 555 // theta signé sur 2 octets
ClementBreteau 15:c2fc239e85df 556 //msgTx.data[4]=(unsigned char)theta;
ClementBreteau 15:c2fc239e85df 557 //msgTx.data[5]=(unsigned char)(theta>>8);
ClementBreteau 15:c2fc239e85df 558 msgTx.data[4]=0;
ClementBreteau 18:cc5fec34ed9c 559 msgTx.data[5]=0;
ClementBreteau 15:c2fc239e85df 560
ClementBreteau 15:c2fc239e85df 561 can1.write(msgTx);
ClementBreteau 15:c2fc239e85df 562
ClementBreteau 18:cc5fec34ed9c 563 break;*/
ClementBreteau 14:c8fc06c4887f 564
ClementBreteau 18:cc5fec34ed9c 565 /* case 130://Lancer mouvement de sortie de la zone de départ
ClementBreteau 15:c2fc239e85df 566 msgTx.id=ACTION_BIG_DEMARRAGE;
ClementBreteau 15:c2fc239e85df 567 msgTx.format=CANStandard;
ClementBreteau 15:c2fc239e85df 568 msgTx.type=CANData;
ClementBreteau 15:c2fc239e85df 569 msgTx.len=1;
ClementBreteau 15:c2fc239e85df 570 msgTx.data[0] = (unsigned char)speed;
ClementBreteau 15:c2fc239e85df 571 can1.write(msgTx);
ClementBreteau 18:cc5fec34ed9c 572 break;*/
antbig 12:14729d584500 573
antbig 0:ad97421fb1fb 574 default:
antbig 0:ad97421fb1fb 575 return 0;//L'action n'existe pas, il faut utiliser le CAN
antbig 0:ad97421fb1fb 576
antbig 0:ad97421fb1fb 577 }
antbig 0:ad97421fb1fb 578 return 1;//L'action est spécifique.
antbig 0:ad97421fb1fb 579
antbig 0:ad97421fb1fb 580 }
antbig 0:ad97421fb1fb 581
antbig 0:ad97421fb1fb 582 /****************************************************************************************/
antbig 0:ad97421fb1fb 583 /* FUNCTION NAME: initRobot */
antbig 0:ad97421fb1fb 584 /* DESCRIPTION : initialiser le robot */
antbig 0:ad97421fb1fb 585 /****************************************************************************************/
antbig 9:d0042422d95a 586 void initRobot(void)
antbig 9:d0042422d95a 587 {
antbig 9:d0042422d95a 588 //Enregistrement de tous les AX12 présent sur la carte
ClementBreteau 15:c2fc239e85df 589 /*AX12_register(5, AX12_SERIAL2);
antbig 8:0edc7dfb7f7e 590 AX12_register(18, AX12_SERIAL2);
antbig 8:0edc7dfb7f7e 591 AX12_register(13, AX12_SERIAL2);
antbig 12:14729d584500 592 AX12_register(1, AX12_SERIAL1);
antbig 12:14729d584500 593 AX12_register(11, AX12_SERIAL1);
antbig 12:14729d584500 594 AX12_register(8, AX12_SERIAL1);
ClementBreteau 15:c2fc239e85df 595 AX12_register(7, AX12_SERIAL2);*/
antbig 9:d0042422d95a 596
antbig 12:14729d584500 597 //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
antbig 12:14729d584500 598 //AX12_processChange();
antbig 11:ed13a480ddca 599 //runRobotTest();
ClementBreteau 16:7321fb3bb396 600
antbig 9:d0042422d95a 601 }
antbig 9:d0042422d95a 602
antbig 9:d0042422d95a 603 /****************************************************************************************/
antbig 12:14729d584500 604 /* FUNCTION NAME: initRobotActionneur */
antbig 12:14729d584500 605 /* DESCRIPTION : Initialiser la position des actionneurs du robot */
antbig 12:14729d584500 606 /****************************************************************************************/
antbig 12:14729d584500 607 void initRobotActionneur(void)
antbig 12:14729d584500 608 {
ClementBreteau 21:7c60d6dfcab5 609
antbig 12:14729d584500 610 }
antbig 12:14729d584500 611
antbig 12:14729d584500 612 /****************************************************************************************/
antbig 9:d0042422d95a 613 /* FUNCTION NAME: runTest */
antbig 9:d0042422d95a 614 /* DESCRIPTION : tester l'ensemble des actionneurs du robot */
antbig 9:d0042422d95a 615 /****************************************************************************************/
antbig 9:d0042422d95a 616 void runRobotTest(void)
antbig 9:d0042422d95a 617 {
ClementBreteau 18:cc5fec34ed9c 618 /*
antbig 12:14729d584500 619 int waitTime = 500;
antbig 12:14729d584500 620
antbig 9:d0042422d95a 621 //Test des AX12 dans l'ordre
antbig 9:d0042422d95a 622 doAction(111,0,0);//Fermeture pince arrière haute
antbig 12:14729d584500 623 wait_ms(waitTime);
antbig 9:d0042422d95a 624 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 625 wait_ms(waitTime);
antbig 9:d0042422d95a 626 doAction(113,0,0);//Fermeture pince arrière basse
antbig 12:14729d584500 627 wait_ms(waitTime);
antbig 9:d0042422d95a 628 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 629 wait_ms(waitTime);
antbig 9:d0042422d95a 630 doAction(115,0,0);//Fermeture porte arrière
antbig 12:14729d584500 631 wait_ms(waitTime);
antbig 9:d0042422d95a 632 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 633 wait_ms(waitTime);
antbig 12:14729d584500 634 doAction(101,0,0);//Fermer les portes avant
antbig 12:14729d584500 635 wait_ms(waitTime);
antbig 12:14729d584500 636 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 637 wait_ms(waitTime);
antbig 12:14729d584500 638 doAction(103,0,0);//Descendre le peigne
antbig 12:14729d584500 639 wait_ms(waitTime);
ClementBreteau 18:cc5fec34ed9c 640 doAction(102,0,0);//Remonter le peigne*/
antbig 0:ad97421fb1fb 641 }
antbig 3:19f2285a4757 642
antbig 4:88431b537477 643 /****************************************************************************************/
antbig 4:88431b537477 644 /* FUNCTION NAME: SelectStrategy */
antbig 4:88431b537477 645 /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
antbig 4:88431b537477 646 /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
antbig 4:88431b537477 647 /****************************************************************************************/
antbig 4:88431b537477 648 int SelectStrategy(unsigned char id)
antbig 4:88431b537477 649 {
antbig 4:88431b537477 650 switch(id)
antbig 4:88431b537477 651 {
ClementBreteau 15:c2fc239e85df 652 // strat de match
antbig 4:88431b537477 653 case 1:
antbig 11:ed13a480ddca 654 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 11:ed13a480ddca 655 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 656 case 2:
antbig 11:ed13a480ddca 657 strcpy(cheminFileStart,"/local/strat2.txt");
antbig 11:ed13a480ddca 658 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 659 case 3:
antbig 11:ed13a480ddca 660 strcpy(cheminFileStart,"/local/strat3.txt");
antbig 11:ed13a480ddca 661 return FileExists(cheminFileStart);
antbig 12:14729d584500 662 case 4:
antbig 12:14729d584500 663 strcpy(cheminFileStart,"/local/strat4.txt");
antbig 12:14729d584500 664 return FileExists(cheminFileStart);
antbig 12:14729d584500 665 case 5:
antbig 12:14729d584500 666 strcpy(cheminFileStart,"/local/strat5.txt");
antbig 12:14729d584500 667 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 668 case 6:
ClementBreteau 14:c8fc06c4887f 669 strcpy(cheminFileStart,"/local/strat6.txt");
ClementBreteau 14:c8fc06c4887f 670 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 671 case 7:
ClementBreteau 14:c8fc06c4887f 672 strcpy(cheminFileStart,"/local/strat7.txt");
ClementBreteau 14:c8fc06c4887f 673 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 674 case 8:
ClementBreteau 14:c8fc06c4887f 675 strcpy(cheminFileStart,"/local/strat8.txt");
ClementBreteau 14:c8fc06c4887f 676 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 677 case 9:
ClementBreteau 14:c8fc06c4887f 678 strcpy(cheminFileStart,"/local/strat9.txt");
ClementBreteau 14:c8fc06c4887f 679 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 680 case 10:
ClementBreteau 14:c8fc06c4887f 681 strcpy(cheminFileStart,"/local/strat10.txt");
ClementBreteau 14:c8fc06c4887f 682 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 683
ClementBreteau 15:c2fc239e85df 684 // strat de demo
ClementBreteau 14:c8fc06c4887f 685 case 0x10:
ClementBreteau 14:c8fc06c4887f 686 strcpy(cheminFileStart,"/local/moteur.txt");
ClementBreteau 14:c8fc06c4887f 687 return FileExists(cheminFileStart);
ClementBreteau 14:c8fc06c4887f 688 case 0x11:
ClementBreteau 15:c2fc239e85df 689 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 690 strcpy(cheminFileStart,"/local/bras.txt");
ClementBreteau 15:c2fc239e85df 691 #else
ClementBreteau 15:c2fc239e85df 692 strcpy(cheminFileStart,"/local/porteAvant.txt");
ClementBreteau 15:c2fc239e85df 693 #endif
ClementBreteau 14:c8fc06c4887f 694 return FileExists(cheminFileStart);
ClementBreteau 15:c2fc239e85df 695 case 0x12:
ClementBreteau 15:c2fc239e85df 696 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 697 strcpy(cheminFileStart,"/local/balancier.txt");
ClementBreteau 15:c2fc239e85df 698 #else
ClementBreteau 15:c2fc239e85df 699 strcpy(cheminFileStart,"/local/mainTourneuse.txt");
ClementBreteau 15:c2fc239e85df 700 #endif
ClementBreteau 15:c2fc239e85df 701 return FileExists(cheminFileStart);
antbig 11:ed13a480ddca 702 default:
antbig 12:14729d584500 703 strcpy(cheminFileStart,"/local/strat1.txt");
antbig 11:ed13a480ddca 704 return 0;
antbig 4:88431b537477 705 }
antbig 4:88431b537477 706 }
antbig 4:88431b537477 707
antbig 12:14729d584500 708 /****************************************************************************************/
antbig 12:14729d584500 709 /* FUNCTION NAME: needToStop */
antbig 12:14729d584500 710 /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
antbig 12:14729d584500 711 /****************************************************************************************/
antbig 12:14729d584500 712 unsigned char needToStop(void)
antbig 12:14729d584500 713 {
antbig 12:14729d584500 714 return isStopEnable;
antbig 12:14729d584500 715 }
antbig 12:14729d584500 716
antbig 12:14729d584500 717 /****************************************************************************************/
antbig 12:14729d584500 718 /* FUNCTION NAME: doBeforeEndAction */
antbig 12:14729d584500 719 /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
antbig 12:14729d584500 720 /****************************************************************************************/
antbig 12:14729d584500 721 void doBeforeEndAction(void)
antbig 12:14729d584500 722 {
antbig 12:14729d584500 723 doAction(110,0,0);//Ouverture pince arrière haute
antbig 12:14729d584500 724 doAction(112,0,0);//Ouverture pince arrière basse
antbig 12:14729d584500 725 doAction(114,0,0);//Ouverture porte arrière
antbig 12:14729d584500 726 doAction(100,0,0);//Ouvrir les portes avant
antbig 12:14729d584500 727 doAction(102,0,0);//Remonter le peigne
antbig 12:14729d584500 728 }
antbig 12:14729d584500 729
antbig 3:19f2285a4757 730 #endif