homologation gros robot et test avec les ack de la carte a tout faire
Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by
Robots/Strategie_small.cpp@10:a788d9cf60f2, 2016-04-29 (annotated)
- Committer:
- antbig
- Date:
- Fri Apr 29 09:14:27 2016 +0000
- Revision:
- 10:a788d9cf60f2
- Parent:
- 9:d0042422d95a
- Child:
- 11:ed13a480ddca
Mise en place des angles pour les pinces XL320
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
antbig | 3:19f2285a4757 | 1 | #include "StrategieManager.h" |
antbig | 3:19f2285a4757 | 2 | #ifdef ROBOT_SMALL |
antbig | 3:19f2285a4757 | 3 | #include "Config_small.h" |
antbig | 3:19f2285a4757 | 4 | |
antbig | 3:19f2285a4757 | 5 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 6 | /* FUNCTION NAME: doFunnyAction */ |
antbig | 3:19f2285a4757 | 7 | /* DESCRIPTION : Permet de faire la funny action en fin de partie */ |
antbig | 3:19f2285a4757 | 8 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 9 | void doFunnyAction(void) { |
antbig | 3:19f2285a4757 | 10 | |
antbig | 3:19f2285a4757 | 11 | |
antbig | 3:19f2285a4757 | 12 | } |
antbig | 10:a788d9cf60f2 | 13 | //L'angle est entre 0 et 1023 |
antbig | 10:a788d9cf60f2 | 14 | void XL320_setGoal(unsigned char id, unsigned short angle); |
antbig | 10:a788d9cf60f2 | 15 | |
antbig | 10:a788d9cf60f2 | 16 | void XL320_setGoal(unsigned char id, unsigned short angle) |
antbig | 10:a788d9cf60f2 | 17 | { |
antbig | 10:a788d9cf60f2 | 18 | CANMessage msgTx=CANMessage(); |
antbig | 10:a788d9cf60f2 | 19 | msgTx.id=SERVO_XL320; |
antbig | 10:a788d9cf60f2 | 20 | msgTx.len=3; |
antbig | 10:a788d9cf60f2 | 21 | msgTx.format=CANStandard; |
antbig | 10:a788d9cf60f2 | 22 | msgTx.type=CANData; |
antbig | 10:a788d9cf60f2 | 23 | msgTx.data[0]=(unsigned char)id; |
antbig | 10:a788d9cf60f2 | 24 | // from sur 2 octets |
antbig | 10:a788d9cf60f2 | 25 | msgTx.data[1]=(unsigned char)angle; |
antbig | 10:a788d9cf60f2 | 26 | msgTx.data[2]=(unsigned char)(angle>>8); |
antbig | 10:a788d9cf60f2 | 27 | |
antbig | 10:a788d9cf60f2 | 28 | can1.write(msgTx); |
antbig | 10:a788d9cf60f2 | 29 | } |
antbig | 3:19f2285a4757 | 30 | |
antbig | 3:19f2285a4757 | 31 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 32 | /* FUNCTION NAME: doAction */ |
antbig | 3:19f2285a4757 | 33 | /* DESCRIPTION : Effectuer une action specifique */ |
antbig | 3:19f2285a4757 | 34 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 35 | unsigned char doAction(unsigned char id, unsigned short speed, short angle) { |
antbig | 3:19f2285a4757 | 36 | switch(id) { |
antbig | 10:a788d9cf60f2 | 37 | case 101://Descendre le bras pour pecher les poissons |
antbig | 3:19f2285a4757 | 38 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche |
antbig | 3:19f2285a4757 | 39 | AX12_setGoal(AX12_ID_BRAS_BASE_INV,200,0x0FF); |
antbig | 3:19f2285a4757 | 40 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 41 | } else { |
antbig | 5:dcd817534b57 | 42 | AX12_setGoal(AX12_ID_BRAS_BASE,140,0x0FF); |
antbig | 5:dcd817534b57 | 43 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 44 | } |
antbig | 3:19f2285a4757 | 45 | break; |
antbig | 10:a788d9cf60f2 | 46 | case 102://Remonter le bras avec les poissons dessus |
antbig | 3:19f2285a4757 | 47 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche |
antbig | 5:dcd817534b57 | 48 | AX12_setGoal(AX12_ID_BRAS_BASE_INV,220,0x0FF); |
antbig | 3:19f2285a4757 | 49 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 50 | } else { |
antbig | 5:dcd817534b57 | 51 | AX12_setGoal(AX12_ID_BRAS_BASE,100,0x0FF); |
antbig | 5:dcd817534b57 | 52 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 53 | } |
antbig | 3:19f2285a4757 | 54 | break; |
antbig | 10:a788d9cf60f2 | 55 | case 103://Decendre à 30° |
antbig | 3:19f2285a4757 | 56 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche |
antbig | 3:19f2285a4757 | 57 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,90);//Ouverture du bras |
antbig | 3:19f2285a4757 | 58 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 59 | } else { |
antbig | 5:dcd817534b57 | 60 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR,250);//Ouverture du bras |
antbig | 5:dcd817534b57 | 61 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 62 | } |
antbig | 3:19f2285a4757 | 63 | break; |
antbig | 10:a788d9cf60f2 | 64 | case 104://ouvrir séparateur |
antbig | 3:19f2285a4757 | 65 | if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche |
antbig | 3:19f2285a4757 | 66 | AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF); |
antbig | 3:19f2285a4757 | 67 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras |
antbig | 3:19f2285a4757 | 68 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 69 | } else { |
antbig | 5:dcd817534b57 | 70 | AX12_setGoal(AX12_ID_BRAS_BASE,55,0x0FF); |
antbig | 5:dcd817534b57 | 71 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR,200);//fermer le bras |
antbig | 5:dcd817534b57 | 72 | AX12_processChange(); |
antbig | 3:19f2285a4757 | 73 | } |
antbig | 3:19f2285a4757 | 74 | break; |
antbig | 10:a788d9cf60f2 | 75 | case 105://Rentrer le bras dans le robot, fermer le séparateur |
antbig | 10:a788d9cf60f2 | 76 | //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras |
antbig | 10:a788d9cf60f2 | 77 | //AX12_processChange(); |
antbig | 10:a788d9cf60f2 | 78 | break; |
antbig | 10:a788d9cf60f2 | 79 | case 106://descendre l'atrape coquillages gauche si non inversé, droit si inversé |
antbig | 5:dcd817534b57 | 80 | //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras |
antbig | 5:dcd817534b57 | 81 | //AX12_processChange(); |
antbig | 5:dcd817534b57 | 82 | break; |
antbig | 10:a788d9cf60f2 | 83 | |
antbig | 10:a788d9cf60f2 | 84 | case 200://ouvir la pince gauche |
antbig | 10:a788d9cf60f2 | 85 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, 350); |
antbig | 10:a788d9cf60f2 | 86 | break; |
antbig | 10:a788d9cf60f2 | 87 | case 205://Sécuriser le palet gauche |
antbig | 10:a788d9cf60f2 | 88 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, 400); |
antbig | 10:a788d9cf60f2 | 89 | break; |
antbig | 10:a788d9cf60f2 | 90 | case 202://fermer pince gauche |
antbig | 10:a788d9cf60f2 | 91 | XL320_setGoal(XL320_ID_PINCE_GAUCHE, 650); |
antbig | 10:a788d9cf60f2 | 92 | break; |
antbig | 10:a788d9cf60f2 | 93 | |
antbig | 10:a788d9cf60f2 | 94 | case 201://ouvir la pince droite |
antbig | 10:a788d9cf60f2 | 95 | XL320_setGoal(XL320_ID_PINCE_DROITE, 400); |
antbig | 10:a788d9cf60f2 | 96 | break; |
antbig | 10:a788d9cf60f2 | 97 | case 204://Sécuriser le palet droit |
antbig | 10:a788d9cf60f2 | 98 | XL320_setGoal(XL320_ID_PINCE_DROITE, 300); |
antbig | 10:a788d9cf60f2 | 99 | break; |
antbig | 10:a788d9cf60f2 | 100 | case 203://fermer pince droite |
antbig | 10:a788d9cf60f2 | 101 | XL320_setGoal(XL320_ID_PINCE_DROITE, 70); |
antbig | 10:a788d9cf60f2 | 102 | break; |
antbig | 10:a788d9cf60f2 | 103 | |
antbig | 3:19f2285a4757 | 104 | default: |
antbig | 10:a788d9cf60f2 | 105 | return 0;//L'action n'existe pas, il faut utiliser le CAN |
antbig | 3:19f2285a4757 | 106 | |
antbig | 3:19f2285a4757 | 107 | } |
antbig | 9:d0042422d95a | 108 | //return 1;//L'action est spécifique. |
antbig | 3:19f2285a4757 | 109 | |
antbig | 3:19f2285a4757 | 110 | } |
antbig | 3:19f2285a4757 | 111 | |
antbig | 3:19f2285a4757 | 112 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 113 | /* FUNCTION NAME: initRobot */ |
antbig | 3:19f2285a4757 | 114 | /* DESCRIPTION : initialiser le robot */ |
antbig | 3:19f2285a4757 | 115 | /****************************************************************************************/ |
antbig | 3:19f2285a4757 | 116 | void initRobot(void) { |
antbig | 3:19f2285a4757 | 117 | /** |
antbig | 3:19f2285a4757 | 118 | On enregistre les id des AX12 présent sur la carte |
antbig | 3:19f2285a4757 | 119 | **/ |
antbig | 5:dcd817534b57 | 120 | AX12_register(1,AX12_SERIAL1,0x0FF); |
antbig | 5:dcd817534b57 | 121 | AX12_register(2,AX12_SERIAL1); |
antbig | 5:dcd817534b57 | 122 | AX12_register(18,AX12_SERIAL1); |
antbig | 5:dcd817534b57 | 123 | AX12_register(4,AX12_SERIAL2); |
antbig | 5:dcd817534b57 | 124 | AX12_register(16,AX12_SERIAL2); |
antbig | 5:dcd817534b57 | 125 | AX12_register(17,AX12_SERIAL2,0x0FF); |
antbig | 5:dcd817534b57 | 126 | /* |
antbig | 5:dcd817534b57 | 127 | AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF); |
antbig | 5:dcd817534b57 | 128 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras |
antbig | 5:dcd817534b57 | 129 | AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF); |
antbig | 5:dcd817534b57 | 130 | AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras |
antbig | 5:dcd817534b57 | 131 | AX12_processChange();*/ |
antbig | 3:19f2285a4757 | 132 | } |
antbig | 3:19f2285a4757 | 133 | |
antbig | 4:88431b537477 | 134 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 135 | /* FUNCTION NAME: runTest */ |
antbig | 9:d0042422d95a | 136 | /* DESCRIPTION : tester l'ensemble des actionneurs du robot */ |
antbig | 9:d0042422d95a | 137 | /****************************************************************************************/ |
antbig | 9:d0042422d95a | 138 | void runRobotTest(void) |
antbig | 9:d0042422d95a | 139 | { |
antbig | 9:d0042422d95a | 140 | |
antbig | 9:d0042422d95a | 141 | } |
antbig | 9:d0042422d95a | 142 | |
antbig | 9:d0042422d95a | 143 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 144 | /* FUNCTION NAME: SelectStrategy */ |
antbig | 4:88431b537477 | 145 | /* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */ |
antbig | 4:88431b537477 | 146 | /* RETURN : 0=> Erreur, 1=> OK si le fichier existe */ |
antbig | 4:88431b537477 | 147 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 148 | int SelectStrategy(unsigned char id) |
antbig | 4:88431b537477 | 149 | { |
antbig | 4:88431b537477 | 150 | switch(id) |
antbig | 4:88431b537477 | 151 | { |
antbig | 4:88431b537477 | 152 | case 1: |
antbig | 4:88431b537477 | 153 | strcpy(cheminFileStart,"/local/strat1.txt"); |
antbig | 4:88431b537477 | 154 | return FileExists(cheminFileStart); |
antbig | 4:88431b537477 | 155 | case 2: |
antbig | 4:88431b537477 | 156 | strcpy(cheminFileStart,"/local/strat2.txt"); |
antbig | 4:88431b537477 | 157 | return FileExists(cheminFileStart); |
antbig | 7:dcce34c7e06e | 158 | case 3: |
antbig | 7:dcce34c7e06e | 159 | strcpy(cheminFileStart,"/local/strat3.txt"); |
antbig | 7:dcce34c7e06e | 160 | return FileExists(cheminFileStart); |
antbig | 4:88431b537477 | 161 | default: |
antbig | 4:88431b537477 | 162 | strcpy(cheminFileStart,"/local/strat.txt"); |
antbig | 4:88431b537477 | 163 | return 0; |
antbig | 4:88431b537477 | 164 | } |
antbig | 4:88431b537477 | 165 | } |
antbig | 4:88431b537477 | 166 | |
antbig | 3:19f2285a4757 | 167 | #endif |