CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Robots/Strategie_big.cpp@29:41e02746041d, 2018-04-06 (annotated)

Committer:
Sitkah
Date:
Fri Apr 06 13:47:19 2018 +0000
Revision:
29:41e02746041d
Parent:
28:acd18776ed2d
Child:
30:a1e37af4bbde
version fusionn?e

Who changed what in which revision?

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