Strategie_13h30
Fork of CRAC-Strat_2017_homologation_gros_rob by
Robots/Strategie_big.cpp@22:8dec8824a6f7, 2017-05-25 (annotated)
- 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?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 15:c2fc239e85df | 1 | |
antbig | 1:116040d14164 | 2 | #include "StrategieManager.h" |
antbig | 3:19f2285a4757 | 3 | #ifdef ROBOT_BIG |
antbig | 3:19f2285a4757 | 4 | #include "Config_big.h" |
antbig | 0:ad97421fb1fb | 5 | |
antbig | 12:14729d584500 | 6 | unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises |
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 |