![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Robots/Strategie_big.cpp@28:acd18776ed2d, 2017-05-27 (annotated)
- Committer:
- antbig
- Date:
- Sat May 27 05:40:26 2017 +0000
- Revision:
- 28:acd18776ed2d
- Parent:
- 26:2f4fcc2354f3
- Child:
- 29:41e02746041d
version pour du 27/05 07h40
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 15:c2fc239e85df | 1 | |
antbig | 1:116040d14164 | 2 | #include "StrategieManager.h" |
antbig | 3:19f2285a4757 | 3 | #ifdef ROBOT_BIG |
antbig | 3:19f2285a4757 | 4 | #include "Config_big.h" |
antbig | 0:ad97421fb1fb | 5 | |
antbig | 12:14729d584500 | 6 | unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises |
antbig | 12:14729d584500 | 7 | |
antbig | 0:ad97421fb1fb | 8 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 9 | /* FUNCTION NAME: doFunnyAction */ |
antbig | 0:ad97421fb1fb | 10 | /* DESCRIPTION : Permet de faire la funny action en fin de partie */ |
antbig | 0:ad97421fb1fb | 11 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 12 | void doFunnyAction(void) { |
ClementBreteau | 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 | 25:f140c93a8666 | 314 | if (speed == BRAS_AVANT)localData = 0x0A; |
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 | 25:f140c93a8666 | 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); |
antbig | 28:acd18776ed2d | 455 | waitingAckFrom = 0; |
antbig | 28:acd18776ed2d | 456 | waitingAckID = 0; |
ClementBreteau | 18:cc5fec34ed9c | 457 | break; |
ClementBreteau | 18:cc5fec34ed9c | 458 | |
ClementBreteau | 18:cc5fec34ed9c | 459 | case 133: // stop turbine |
ClementBreteau | 18:cc5fec34ed9c | 460 | msgTx.id=0x403; |
ClementBreteau | 18:cc5fec34ed9c | 461 | msgTx.format=CANStandard; |
ClementBreteau | 18:cc5fec34ed9c | 462 | msgTx.type=CANData; |
ClementBreteau | 18:cc5fec34ed9c | 463 | msgTx.len=2; |
ClementBreteau | 18:cc5fec34ed9c | 464 | msgTx.data[0]=0; |
ClementBreteau | 18:cc5fec34ed9c | 465 | msgTx.data[1]=BRAS_AVANT; |
ClementBreteau | 18:cc5fec34ed9c | 466 | can1.write(msgTx); |
antbig | 28:acd18776ed2d | 467 | waitingAckFrom = 0; |
antbig | 28:acd18776ed2d | 468 | waitingAckID = 0; |
ClementBreteau | 18:cc5fec34ed9c | 469 | break; |
ClementBreteau | 18:cc5fec34ed9c | 470 | |
ClementBreteau | 18:cc5fec34ed9c | 471 | case 140:// lanceur ON |
ClementBreteau | 18:cc5fec34ed9c | 472 | msgTx.id=0x402; |
ClementBreteau | 18:cc5fec34ed9c | 473 | msgTx.format=CANStandard; |
ClementBreteau | 18:cc5fec34ed9c | 474 | msgTx.type=CANData; |
ClementBreteau | 18:cc5fec34ed9c | 475 | msgTx.len=2; |
ClementBreteau | 18:cc5fec34ed9c | 476 | |
ClementBreteau | 18:cc5fec34ed9c | 477 | msgTx.data[0]=1; |
ClementBreteau | 18:cc5fec34ed9c | 478 | msgTx.data[1]=0x02; |
ClementBreteau | 18:cc5fec34ed9c | 479 | can1.write(msgTx); |
ClementBreteau | 18:cc5fec34ed9c | 480 | break; |
ClementBreteau | 18:cc5fec34ed9c | 481 | |
ClementBreteau | 18:cc5fec34ed9c | 482 | case 141: // lanceur OFF |
ClementBreteau | 18:cc5fec34ed9c | 483 | msgTx.id=0x402; |
ClementBreteau | 18:cc5fec34ed9c | 484 | msgTx.format=CANStandard; |
ClementBreteau | 18:cc5fec34ed9c | 485 | msgTx.type=CANData; |
ClementBreteau | 18:cc5fec34ed9c | 486 | msgTx.len=2; |
ClementBreteau | 18:cc5fec34ed9c | 487 | |
ClementBreteau | 18:cc5fec34ed9c | 488 | msgTx.data[0]=0; |
ClementBreteau | 18:cc5fec34ed9c | 489 | msgTx.data[1]=0x02; |
ClementBreteau | 15:c2fc239e85df | 490 | can1.write(msgTx); |
antbig | 8:0edc7dfb7f7e | 491 | break; |
ClementBreteau | 15:c2fc239e85df | 492 | |
ClementBreteau | 18:cc5fec34ed9c | 493 | case 150: |
ClementBreteau | 18:cc5fec34ed9c | 494 | msgTx.id=0x404; |
antbig | 12:14729d584500 | 495 | msgTx.format=CANStandard; |
antbig | 12:14729d584500 | 496 | msgTx.type=CANData; |
ClementBreteau | 18:cc5fec34ed9c | 497 | msgTx.len=2; |
ClementBreteau | 18:cc5fec34ed9c | 498 | |
ClementBreteau | 18:cc5fec34ed9c | 499 | msgTx.data[0]=1; |
ClementBreteau | 18:cc5fec34ed9c | 500 | msgTx.data[1]=BRAS_AVANT; |
ClementBreteau | 18:cc5fec34ed9c | 501 | |
antbig | 12:14729d584500 | 502 | can1.write(msgTx); |
antbig | 8:0edc7dfb7f7e | 503 | break; |
ClementBreteau | 18:cc5fec34ed9c | 504 | |
ClementBreteau | 18:cc5fec34ed9c | 505 | case 151: |
ClementBreteau | 18:cc5fec34ed9c | 506 | msgTx.id=0x404; |
antbig | 12:14729d584500 | 507 | msgTx.format=CANStandard; |
antbig | 12:14729d584500 | 508 | msgTx.type=CANData; |
ClementBreteau | 18:cc5fec34ed9c | 509 | msgTx.len=2; |
ClementBreteau | 18:cc5fec34ed9c | 510 | |
ClementBreteau | 18:cc5fec34ed9c | 511 | msgTx.data[0]=0; |
ClementBreteau | 18:cc5fec34ed9c | 512 | msgTx.data[1]=BRAS_AVANT; |
ClementBreteau | 18:cc5fec34ed9c | 513 | |
antbig | 12:14729d584500 | 514 | can1.write(msgTx); |
antbig | 12:14729d584500 | 515 | break; |
antbig | 12:14729d584500 | 516 | |
antbig | 12:14729d584500 | 517 | case 10://Désactiver le stop |
antbig | 12:14729d584500 | 518 | isStopEnable = 0; |
antbig | 12:14729d584500 | 519 | break; |
antbig | 12:14729d584500 | 520 | case 11://Activer le stop |
antbig | 12:14729d584500 | 521 | isStopEnable = 1; |
antbig | 12:14729d584500 | 522 | break; |
antbig | 12:14729d584500 | 523 | case 20://Désactiver l'asservissement |
antbig | 12:14729d584500 | 524 | setAsservissementEtat(0); |
antbig | 12:14729d584500 | 525 | break; |
antbig | 12:14729d584500 | 526 | case 21://Activer l'asservissement |
antbig | 12:14729d584500 | 527 | setAsservissementEtat(1); |
antbig | 12:14729d584500 | 528 | break; |
antbig | 12:14729d584500 | 529 | |
antbig | 12:14729d584500 | 530 | case 22://Changer la vitesse du robot |
antbig | 12:14729d584500 | 531 | SendSpeed(speed,(unsigned short)angle); |
ClementBreteau | 26:2f4fcc2354f3 | 532 | waitingAckFrom = 0; |
ClementBreteau | 26:2f4fcc2354f3 | 533 | waitingAckID = 0; |
ClementBreteau | 26:2f4fcc2354f3 | 534 | wait(0.2); |
antbig | 12:14729d584500 | 535 | break; |
antbig | 12:14729d584500 | 536 | |
antbig | 12:14729d584500 | 537 | case 30://Action tempo |
antbig | 12:14729d584500 | 538 | wait_ms(speed); |
antbig | 12:14729d584500 | 539 | break; |
antbig | 12:14729d584500 | 540 | |
ClementBreteau | 18:cc5fec34ed9c | 541 | /*case 40: // demande au telemetre la position d'un objet |
ClementBreteau | 14:c8fc06c4887f | 542 | //SendRawId(TELEMETRE_RECHERCHE_OBJET); |
ClementBreteau | 15:c2fc239e85df | 543 | |
ClementBreteau | 15:c2fc239e85df | 544 | modeTelemetre = 1; |
ClementBreteau | 15:c2fc239e85df | 545 | |
ClementBreteau | 15:c2fc239e85df | 546 | //angle = angle /10; |
ClementBreteau | 15:c2fc239e85df | 547 | |
ClementBreteau | 15:c2fc239e85df | 548 | msgTx.id=TELEMETRE_OBJET; |
ClementBreteau | 15:c2fc239e85df | 549 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 550 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 551 | msgTx.len=1; |
ClementBreteau | 15:c2fc239e85df | 552 | // indice du module sur le terrain |
ClementBreteau | 15:c2fc239e85df | 553 | msgTx.data[0] = (unsigned char)speed; |
ClementBreteau | 15:c2fc239e85df | 554 | |
ClementBreteau | 18:cc5fec34ed9c | 555 | |
ClementBreteau | 15:c2fc239e85df | 556 | // x sur 2 octets |
ClementBreteau | 15:c2fc239e85df | 557 | msgTx.data[0]=(unsigned char)speed; |
ClementBreteau | 15:c2fc239e85df | 558 | msgTx.data[1]=(unsigned char)(speed>>8); |
ClementBreteau | 15:c2fc239e85df | 559 | // y sur 2 octets |
ClementBreteau | 15:c2fc239e85df | 560 | msgTx.data[2]=(unsigned char)angle; |
ClementBreteau | 15:c2fc239e85df | 561 | msgTx.data[3]=(unsigned char)(angle>>8); |
ClementBreteau | 15:c2fc239e85df | 562 | // theta signé sur 2 octets |
ClementBreteau | 15:c2fc239e85df | 563 | //msgTx.data[4]=(unsigned char)theta; |
ClementBreteau | 15:c2fc239e85df | 564 | //msgTx.data[5]=(unsigned char)(theta>>8); |
ClementBreteau | 15:c2fc239e85df | 565 | msgTx.data[4]=0; |
ClementBreteau | 18:cc5fec34ed9c | 566 | msgTx.data[5]=0; |
ClementBreteau | 15:c2fc239e85df | 567 | |
ClementBreteau | 15:c2fc239e85df | 568 | can1.write(msgTx); |
ClementBreteau | 15:c2fc239e85df | 569 | |
ClementBreteau | 18:cc5fec34ed9c | 570 | break;*/ |
ClementBreteau | 14:c8fc06c4887f | 571 | |
ClementBreteau | 18:cc5fec34ed9c | 572 | /* case 130://Lancer mouvement de sortie de la zone de départ |
ClementBreteau | 15:c2fc239e85df | 573 | msgTx.id=ACTION_BIG_DEMARRAGE; |
ClementBreteau | 15:c2fc239e85df | 574 | msgTx.format=CANStandard; |
ClementBreteau | 15:c2fc239e85df | 575 | msgTx.type=CANData; |
ClementBreteau | 15:c2fc239e85df | 576 | msgTx.len=1; |
ClementBreteau | 15:c2fc239e85df | 577 | msgTx.data[0] = (unsigned char)speed; |
ClementBreteau | 15:c2fc239e85df | 578 | can1.write(msgTx); |
ClementBreteau | 18:cc5fec34ed9c | 579 | break;*/ |
antbig | 12:14729d584500 | 580 | |
antbig | 0:ad97421fb1fb | 581 | default: |
antbig | 0:ad97421fb1fb | 582 | return 0;//L'action n'existe pas, il faut utiliser le CAN |
antbig | 0:ad97421fb1fb | 583 | |
antbig | 0:ad97421fb1fb | 584 | } |
antbig | 0:ad97421fb1fb | 585 | return 1;//L'action est spécifique. |
antbig | 0:ad97421fb1fb | 586 | |
antbig | 0:ad97421fb1fb | 587 | } |
antbig | 0:ad97421fb1fb | 588 | |
antbig | 0:ad97421fb1fb | 589 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 590 | /* FUNCTION NAME: initRobot */ |
antbig | 0:ad97421fb1fb | 591 | /* DESCRIPTION : initialiser le robot */ |
antbig | 0:ad97421fb1fb | 592 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 593 | void initRobot(void) |
antbig | 9:d0042422d95a | 594 | { |
antbig | 9:d0042422d95a | 595 | //Enregistrement de tous les AX12 présent sur la carte |
ClementBreteau | 15:c2fc239e85df | 596 | /*AX12_register(5, AX12_SERIAL2); |
antbig | 8:0edc7dfb7f7e | 597 | AX12_register(18, AX12_SERIAL2); |
antbig | 8:0edc7dfb7f7e | 598 | AX12_register(13, AX12_SERIAL2); |
antbig | 12:14729d584500 | 599 | AX12_register(1, AX12_SERIAL1); |
antbig | 12:14729d584500 | 600 | AX12_register(11, AX12_SERIAL1); |
antbig | 12:14729d584500 | 601 | AX12_register(8, AX12_SERIAL1); |
ClementBreteau | 15:c2fc239e85df | 602 | AX12_register(7, AX12_SERIAL2);*/ |
antbig | 9:d0042422d95a | 603 | |
antbig | 12:14729d584500 | 604 | //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION); |
antbig | 12:14729d584500 | 605 | //AX12_processChange(); |
antbig | 11:ed13a480ddca | 606 | //runRobotTest(); |
ClementBreteau | 16:7321fb3bb396 | 607 | |
antbig | 9:d0042422d95a | 608 | } |
antbig | 9:d0042422d95a | 609 | |
antbig | 9:d0042422d95a | 610 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 611 | /* FUNCTION NAME: initRobotActionneur */ |
antbig | 12:14729d584500 | 612 | /* DESCRIPTION : Initialiser la position des actionneurs du robot */ |
antbig | 12:14729d584500 | 613 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 614 | void initRobotActionneur(void) |
antbig | 12:14729d584500 | 615 | { |
ClementBreteau | 23:ab87d308eaf9 | 616 | /*doAction(100,1,0); |
ClementBreteau | 23:ab87d308eaf9 | 617 | doAction(100,2,0); |
ClementBreteau | 23:ab87d308eaf9 | 618 | doAction(110,0,0); |
ClementBreteau | 23:ab87d308eaf9 | 619 | doAction(120,0,0); |
ClementBreteau | 23:ab87d308eaf9 | 620 | doAction(131,0,0);*/ |
ClementBreteau | 23:ab87d308eaf9 | 621 | |
antbig | 12:14729d584500 | 622 | } |
antbig | 12:14729d584500 | 623 | |
antbig | 12:14729d584500 | 624 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 625 | /* FUNCTION NAME: runTest */ |
antbig | 9:d0042422d95a | 626 | /* DESCRIPTION : tester l'ensemble des actionneurs du robot */ |
antbig | 9:d0042422d95a | 627 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 628 | void runRobotTest(void) |
antbig | 9:d0042422d95a | 629 | { |
ClementBreteau | 18:cc5fec34ed9c | 630 | /* |
antbig | 12:14729d584500 | 631 | int waitTime = 500; |
antbig | 12:14729d584500 | 632 | |
antbig | 9:d0042422d95a | 633 | //Test des AX12 dans l'ordre |
antbig | 9:d0042422d95a | 634 | doAction(111,0,0);//Fermeture pince arrière haute |
antbig | 12:14729d584500 | 635 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 636 | doAction(110,0,0);//Ouverture pince arrière haute |
antbig | 12:14729d584500 | 637 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 638 | doAction(113,0,0);//Fermeture pince arrière basse |
antbig | 12:14729d584500 | 639 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 640 | doAction(112,0,0);//Ouverture pince arrière basse |
antbig | 12:14729d584500 | 641 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 642 | doAction(115,0,0);//Fermeture porte arrière |
antbig | 12:14729d584500 | 643 | wait_ms(waitTime); |
antbig | 9:d0042422d95a | 644 | doAction(114,0,0);//Ouverture porte arrière |
antbig | 12:14729d584500 | 645 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 646 | doAction(101,0,0);//Fermer les portes avant |
antbig | 12:14729d584500 | 647 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 648 | doAction(100,0,0);//Ouvrir les portes avant |
antbig | 12:14729d584500 | 649 | wait_ms(waitTime); |
antbig | 12:14729d584500 | 650 | doAction(103,0,0);//Descendre le peigne |
antbig | 12:14729d584500 | 651 | wait_ms(waitTime); |
ClementBreteau | 18:cc5fec34ed9c | 652 | doAction(102,0,0);//Remonter le peigne*/ |
antbig | 0:ad97421fb1fb | 653 | } |
antbig | 3:19f2285a4757 | 654 | |
antbig | 4:88431b537477 | 655 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 656 | /* FUNCTION NAME: SelectStrategy */ |
antbig | 4:88431b537477 | 657 | /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */ |
antbig | 4:88431b537477 | 658 | /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */ |
antbig | 4:88431b537477 | 659 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 660 | int SelectStrategy(unsigned char id) |
antbig | 4:88431b537477 | 661 | { |
antbig | 4:88431b537477 | 662 | switch(id) |
antbig | 4:88431b537477 | 663 | { |
ClementBreteau | 15:c2fc239e85df | 664 | // strat de match |
antbig | 4:88431b537477 | 665 | case 1: |
antbig | 11:ed13a480ddca | 666 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 11:ed13a480ddca | 667 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 668 | case 2: |
antbig | 11:ed13a480ddca | 669 | strcpy(cheminFileStart,"/local/strat2.txt"); |
antbig | 11:ed13a480ddca | 670 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 671 | case 3: |
antbig | 11:ed13a480ddca | 672 | strcpy(cheminFileStart,"/local/strat3.txt"); |
antbig | 11:ed13a480ddca | 673 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 674 | case 4: |
antbig | 12:14729d584500 | 675 | strcpy(cheminFileStart,"/local/strat4.txt"); |
antbig | 12:14729d584500 | 676 | return FileExists(cheminFileStart); |
antbig | 12:14729d584500 | 677 | case 5: |
antbig | 12:14729d584500 | 678 | strcpy(cheminFileStart,"/local/strat5.txt"); |
antbig | 12:14729d584500 | 679 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 680 | case 6: |
ClementBreteau | 14:c8fc06c4887f | 681 | strcpy(cheminFileStart,"/local/strat6.txt"); |
ClementBreteau | 14:c8fc06c4887f | 682 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 683 | case 7: |
ClementBreteau | 14:c8fc06c4887f | 684 | strcpy(cheminFileStart,"/local/strat7.txt"); |
ClementBreteau | 14:c8fc06c4887f | 685 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 686 | case 8: |
ClementBreteau | 14:c8fc06c4887f | 687 | strcpy(cheminFileStart,"/local/strat8.txt"); |
ClementBreteau | 14:c8fc06c4887f | 688 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 689 | case 9: |
ClementBreteau | 14:c8fc06c4887f | 690 | strcpy(cheminFileStart,"/local/strat9.txt"); |
ClementBreteau | 14:c8fc06c4887f | 691 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 692 | case 10: |
ClementBreteau | 14:c8fc06c4887f | 693 | strcpy(cheminFileStart,"/local/strat10.txt"); |
ClementBreteau | 14:c8fc06c4887f | 694 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 695 | |
ClementBreteau | 15:c2fc239e85df | 696 | // strat de demo |
ClementBreteau | 14:c8fc06c4887f | 697 | case 0x10: |
ClementBreteau | 14:c8fc06c4887f | 698 | strcpy(cheminFileStart,"/local/moteur.txt"); |
ClementBreteau | 14:c8fc06c4887f | 699 | return FileExists(cheminFileStart); |
ClementBreteau | 14:c8fc06c4887f | 700 | case 0x11: |
ClementBreteau | 15:c2fc239e85df | 701 | #ifdef ROBOT_BIG |
ClementBreteau | 15:c2fc239e85df | 702 | strcpy(cheminFileStart,"/local/bras.txt"); |
ClementBreteau | 15:c2fc239e85df | 703 | #else |
ClementBreteau | 15:c2fc239e85df | 704 | strcpy(cheminFileStart,"/local/porteAvant.txt"); |
ClementBreteau | 15:c2fc239e85df | 705 | #endif |
ClementBreteau | 14:c8fc06c4887f | 706 | return FileExists(cheminFileStart); |
ClementBreteau | 15:c2fc239e85df | 707 | case 0x12: |
ClementBreteau | 15:c2fc239e85df | 708 | #ifdef ROBOT_BIG |
ClementBreteau | 15:c2fc239e85df | 709 | strcpy(cheminFileStart,"/local/balancier.txt"); |
ClementBreteau | 15:c2fc239e85df | 710 | #else |
ClementBreteau | 15:c2fc239e85df | 711 | strcpy(cheminFileStart,"/local/mainTourneuse.txt"); |
ClementBreteau | 15:c2fc239e85df | 712 | #endif |
ClementBreteau | 15:c2fc239e85df | 713 | return FileExists(cheminFileStart); |
antbig | 11:ed13a480ddca | 714 | default: |
antbig | 12:14729d584500 | 715 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 11:ed13a480ddca | 716 | return 0; |
antbig | 4:88431b537477 | 717 | } |
antbig | 4:88431b537477 | 718 | } |
antbig | 4:88431b537477 | 719 | |
antbig | 12:14729d584500 | 720 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 721 | /* FUNCTION NAME: needToStop */ |
antbig | 12:14729d584500 | 722 | /* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */ |
antbig | 12:14729d584500 | 723 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 724 | unsigned char needToStop(void) |
antbig | 12:14729d584500 | 725 | { |
antbig | 12:14729d584500 | 726 | return isStopEnable; |
antbig | 12:14729d584500 | 727 | } |
antbig | 12:14729d584500 | 728 | |
antbig | 12:14729d584500 | 729 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 730 | /* FUNCTION NAME: doBeforeEndAction */ |
antbig | 12:14729d584500 | 731 | /* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */ |
antbig | 12:14729d584500 | 732 | /****************************************************************************************/ |
antbig | 12:14729d584500 | 733 | void doBeforeEndAction(void) |
antbig | 12:14729d584500 | 734 | { |
antbig | 12:14729d584500 | 735 | doAction(110,0,0);//Ouverture pince arrière haute |
antbig | 12:14729d584500 | 736 | doAction(112,0,0);//Ouverture pince arrière basse |
antbig | 12:14729d584500 | 737 | doAction(114,0,0);//Ouverture porte arrière |
antbig | 12:14729d584500 | 738 | doAction(100,0,0);//Ouvrir les portes avant |
antbig | 12:14729d584500 | 739 | doAction(102,0,0);//Remonter le peigne |
antbig | 12:14729d584500 | 740 | } |
antbig | 12:14729d584500 | 741 | |
antbig | 3:19f2285a4757 | 742 | #endif |