code petit robot pour homologation
Fork of CRAC-Strat_2017_V2 by
peripheriques/actionneurs.cpp@16:7321fb3bb396, 2017-05-19 (annotated)
- Committer:
- ClementBreteau
- Date:
- Fri May 19 17:13:46 2017 +0000
- Revision:
- 16:7321fb3bb396
- Child:
- 18:cc5fec34ed9c
strat du robot, ann?e 2017, le 19 mai ? 19h
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 16:7321fb3bb396 | 1 | #include "peripheriques.h" |
ClementBreteau | 16:7321fb3bb396 | 2 | /* contient les fonctiones qui servent à utiliser les AX12 et les moteurs sur le petit robot*/ |
ClementBreteau | 16:7321fb3bb396 | 3 | |
ClementBreteau | 16:7321fb3bb396 | 4 | PwmOut motGauche(p21); |
ClementBreteau | 16:7321fb3bb396 | 5 | PwmOut motDroit(p22); |
ClementBreteau | 16:7321fb3bb396 | 6 | |
ClementBreteau | 16:7321fb3bb396 | 7 | // AX12 partie centrale du petit robot |
ClementBreteau | 16:7321fb3bb396 | 8 | AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre; |
ClementBreteau | 16:7321fb3bb396 | 9 | AX12 *partieDoigtCentre; |
ClementBreteau | 16:7321fb3bb396 | 10 | AX12 *multipleCentre; |
ClementBreteau | 16:7321fb3bb396 | 11 | // AX12 partie gauche du robot |
ClementBreteau | 16:7321fb3bb396 | 12 | AX12 *partieBasseGauche, *partieMainGauche; |
ClementBreteau | 16:7321fb3bb396 | 13 | AX12 *partiePorteGauche; |
ClementBreteau | 16:7321fb3bb396 | 14 | AX12 *multipleGauche; |
ClementBreteau | 16:7321fb3bb396 | 15 | |
ClementBreteau | 16:7321fb3bb396 | 16 | // AX12 partie droite du robot |
ClementBreteau | 16:7321fb3bb396 | 17 | AX12 *partieBasseDroite, *partieMainDroite; |
ClementBreteau | 16:7321fb3bb396 | 18 | AX12 *partiePorteDroite; |
ClementBreteau | 16:7321fb3bb396 | 19 | AX12 *multipleDroite; |
ClementBreteau | 16:7321fb3bb396 | 20 | |
ClementBreteau | 16:7321fb3bb396 | 21 | Serial pc(USBTX, USBRX); |
ClementBreteau | 16:7321fb3bb396 | 22 | |
ClementBreteau | 16:7321fb3bb396 | 23 | Timer t; |
ClementBreteau | 16:7321fb3bb396 | 24 | Ticker flipper; |
ClementBreteau | 16:7321fb3bb396 | 25 | unsigned char action_precedente = 0; |
ClementBreteau | 16:7321fb3bb396 | 26 | |
ClementBreteau | 16:7321fb3bb396 | 27 | |
ClementBreteau | 16:7321fb3bb396 | 28 | ////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// |
ClementBreteau | 16:7321fb3bb396 | 29 | static char TAB1[20]= {0x05,0x15, 0x02, 0xFF, 0x00, ///Position initiale |
ClementBreteau | 16:7321fb3bb396 | 30 | 0x06,0x00, 0x02, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 31 | 0x07,0x90, 0x01, 0xB1, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 32 | 0x08,0x58, 0x02, 0x79, 0x00}; |
ClementBreteau | 16:7321fb3bb396 | 33 | |
ClementBreteau | 16:7321fb3bb396 | 34 | static char TAB2[20]= {0x05,0x50, 0x00, 0xFF, 0x02, ///Preparation prise |
ClementBreteau | 16:7321fb3bb396 | 35 | 0x06,0x50, 0x01, 0xFF, 0x02, |
ClementBreteau | 16:7321fb3bb396 | 36 | 0x07,0xF4, 0x01, 0xFF, 0x02, |
ClementBreteau | 16:7321fb3bb396 | 37 | 0x08,0xF4, 0x01, 0xFF, 0x02}; |
ClementBreteau | 16:7321fb3bb396 | 38 | |
ClementBreteau | 16:7321fb3bb396 | 39 | static char TAB3[20]= {0x05,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee) |
ClementBreteau | 16:7321fb3bb396 | 40 | 0x06,0x50, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 41 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 42 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 43 | |
ClementBreteau | 16:7321fb3bb396 | 44 | static char TAB4[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air) |
ClementBreteau | 16:7321fb3bb396 | 45 | 0x06,0x50, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 46 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 47 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 48 | |
ClementBreteau | 16:7321fb3bb396 | 49 | static char TAB5[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) |
ClementBreteau | 16:7321fb3bb396 | 50 | 0x06,0xF4, 0x01, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 51 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 52 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 53 | |
ClementBreteau | 16:7321fb3bb396 | 54 | static char TAB6[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
ClementBreteau | 16:7321fb3bb396 | 55 | 0x06,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 56 | 0x07,0x4d, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 57 | 0x08,0x9a, 0x02, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 58 | |
ClementBreteau | 16:7321fb3bb396 | 59 | static char TAB7[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air) |
ClementBreteau | 16:7321fb3bb396 | 60 | 0x06,0xB0, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 61 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 62 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
ClementBreteau | 16:7321fb3bb396 | 63 | |
ClementBreteau | 16:7321fb3bb396 | 64 | static char TAB8[20]= {0x05,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas |
ClementBreteau | 16:7321fb3bb396 | 65 | 0x06,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 66 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 67 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
ClementBreteau | 16:7321fb3bb396 | 68 | |
ClementBreteau | 16:7321fb3bb396 | 69 | static char TAB9[20]= {0x05,0x40, 0x00, 0xFF, 0x03, ///Deposer |
ClementBreteau | 16:7321fb3bb396 | 70 | 0x06,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 71 | 0x07,0xD0, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 72 | 0x08,0x35, 0x03, 0xFF, 0x00}; |
ClementBreteau | 16:7321fb3bb396 | 73 | |
ClementBreteau | 16:7321fb3bb396 | 74 | static char TAB10[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) |
ClementBreteau | 16:7321fb3bb396 | 75 | 0x06,0x00, 0x01, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 76 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 77 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 78 | |
ClementBreteau | 16:7321fb3bb396 | 79 | static char TAB11[20]= {0x05,0x60, 0x00, 0xFF, 0x03, ///Pousser_module |
ClementBreteau | 16:7321fb3bb396 | 80 | 0x06,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 81 | 0x07,0xC5, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 82 | 0x08,0x4D, 0x03, 0xFF, 0x00}; |
ClementBreteau | 16:7321fb3bb396 | 83 | |
ClementBreteau | 16:7321fb3bb396 | 84 | static char TAB12[20]= {0x05,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale |
ClementBreteau | 16:7321fb3bb396 | 85 | 0x06,0x00, 0x02, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 86 | 0x07,0xF4, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 87 | 0x08,0xF4, 0x01, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 88 | |
ClementBreteau | 16:7321fb3bb396 | 89 | static char TAB13[20]= {0x05,0xF4, 0x00, 0xFF, 0x03, ///Deposer |
ClementBreteau | 16:7321fb3bb396 | 90 | 0x06,0xA0, 0x02, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 91 | 0x07,0xD0, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 92 | 0x08,0x35, 0x03, 0xFF, 0x00}; |
ClementBreteau | 16:7321fb3bb396 | 93 | |
ClementBreteau | 16:7321fb3bb396 | 94 | static char TAB14[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
ClementBreteau | 16:7321fb3bb396 | 95 | 0x06,0x42, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 96 | 0x07,0x4d, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 97 | 0x08,0x9a, 0x02, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 98 | |
ClementBreteau | 16:7321fb3bb396 | 99 | static char TAB15[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (module sur tige) |
ClementBreteau | 16:7321fb3bb396 | 100 | 0x06,0x42, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 101 | 0x07,0xC5, 0x00, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 102 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 103 | |
ClementBreteau | 16:7321fb3bb396 | 104 | static char TAB16[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) |
ClementBreteau | 16:7321fb3bb396 | 105 | 0x06,0x42, 0x00, 0xFF, 0x00, |
ClementBreteau | 16:7321fb3bb396 | 106 | 0x07,0x2c, 0x01, 0xFF, 0x03, |
ClementBreteau | 16:7321fb3bb396 | 107 | 0x08,0x4D, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 108 | ////////////////////// TABLEAU BRAS GAUCHE /////////////////////////// |
ClementBreteau | 16:7321fb3bb396 | 109 | static char TAB21[10]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale |
ClementBreteau | 16:7321fb3bb396 | 110 | 0x02,0xF4, 0x01, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 111 | |
ClementBreteau | 16:7321fb3bb396 | 112 | static char TAB22[10]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner |
ClementBreteau | 16:7321fb3bb396 | 113 | 0x02,0x40, 0x03, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 114 | |
ClementBreteau | 16:7321fb3bb396 | 115 | static char TAB23[10]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module |
ClementBreteau | 16:7321fb3bb396 | 116 | 0x02,0xE5, 0x02, 0xFF, 0x03}; |
ClementBreteau | 16:7321fb3bb396 | 117 | |
ClementBreteau | 16:7321fb3bb396 | 118 | |
ClementBreteau | 16:7321fb3bb396 | 119 | |
ClementBreteau | 16:7321fb3bb396 | 120 | |
ClementBreteau | 16:7321fb3bb396 | 121 | /* ANGLE */ |
ClementBreteau | 16:7321fb3bb396 | 122 | |
ClementBreteau | 16:7321fb3bb396 | 123 | /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 |
ClementBreteau | 16:7321fb3bb396 | 124 | 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 |
ClementBreteau | 16:7321fb3bb396 | 125 | 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 |
ClementBreteau | 16:7321fb3bb396 | 126 | 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 |
ClementBreteau | 16:7321fb3bb396 | 127 | 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 |
ClementBreteau | 16:7321fb3bb396 | 128 | 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 |
ClementBreteau | 16:7321fb3bb396 | 129 | 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 |
ClementBreteau | 16:7321fb3bb396 | 130 | 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 |
ClementBreteau | 16:7321fb3bb396 | 131 | 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 |
ClementBreteau | 16:7321fb3bb396 | 132 | 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ |
ClementBreteau | 16:7321fb3bb396 | 133 | |
ClementBreteau | 16:7321fb3bb396 | 134 | /* NUMERO AX12 */ |
ClementBreteau | 16:7321fb3bb396 | 135 | |
ClementBreteau | 16:7321fb3bb396 | 136 | /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 |
ClementBreteau | 16:7321fb3bb396 | 137 | 1 = 0x01 | 10 = 0x0A |
ClementBreteau | 16:7321fb3bb396 | 138 | 2 = 0x02 | 11 = 0x0B |
ClementBreteau | 16:7321fb3bb396 | 139 | 3 = 0x03 | 12 = 0x0C |
ClementBreteau | 16:7321fb3bb396 | 140 | 4 = 0x04 | 13 = 0x0D |
ClementBreteau | 16:7321fb3bb396 | 141 | 5 = 0x05 | 14 = 0x0E |
ClementBreteau | 16:7321fb3bb396 | 142 | 6 = 0x06 | 15 = 0x0F |
ClementBreteau | 16:7321fb3bb396 | 143 | 7 = 0x07 | 16 = 0x10 |
ClementBreteau | 16:7321fb3bb396 | 144 | 8 = 0x08 | 17 = 0x11 */ |
ClementBreteau | 16:7321fb3bb396 | 145 | |
ClementBreteau | 16:7321fb3bb396 | 146 | |
ClementBreteau | 16:7321fb3bb396 | 147 | |
ClementBreteau | 16:7321fb3bb396 | 148 | |
ClementBreteau | 16:7321fb3bb396 | 149 | |
ClementBreteau | 16:7321fb3bb396 | 150 | void declarationAX12(void){ |
ClementBreteau | 16:7321fb3bb396 | 151 | //Pince centrale |
ClementBreteau | 16:7321fb3bb396 | 152 | partieBasseCentre = new AX12(p9, p10, 5, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 153 | partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 154 | partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 155 | partiePoignetCentre = new AX12(p9, p10, 6, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 156 | //doigt central |
ClementBreteau | 16:7321fb3bb396 | 157 | partieDoigtCentre = new AX12(p9, p10, 4, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 158 | |
ClementBreteau | 16:7321fb3bb396 | 159 | multipleCentre = new AX12(p9,p10,0xFE,1000000); |
ClementBreteau | 16:7321fb3bb396 | 160 | |
ClementBreteau | 16:7321fb3bb396 | 161 | //Bras de gauche |
ClementBreteau | 16:7321fb3bb396 | 162 | partieBasseGauche = new AX12(p13, p14, 1, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 163 | partieMainGauche = new AX12(p13, p14, 2, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 164 | //Porte gauche |
ClementBreteau | 16:7321fb3bb396 | 165 | partiePorteGauche = new AX12(p13, p14, 3, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 166 | |
ClementBreteau | 16:7321fb3bb396 | 167 | multipleGauche = new AX12(p13,p14,0xFE,1000000); |
ClementBreteau | 16:7321fb3bb396 | 168 | |
ClementBreteau | 16:7321fb3bb396 | 169 | //Bras de droite |
ClementBreteau | 16:7321fb3bb396 | 170 | partieBasseDroite = new AX12(p28, p27, 1, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 171 | partieMainDroite = new AX12(p28, p27, 2, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 172 | //Porte droite |
ClementBreteau | 16:7321fb3bb396 | 173 | partiePorteDroite = new AX12(p28, p27, 3, 1000000); |
ClementBreteau | 16:7321fb3bb396 | 174 | |
ClementBreteau | 16:7321fb3bb396 | 175 | multipleDroite = new AX12(p28,p27,0xFE,1000000); |
ClementBreteau | 16:7321fb3bb396 | 176 | |
ClementBreteau | 16:7321fb3bb396 | 177 | |
ClementBreteau | 16:7321fb3bb396 | 178 | |
ClementBreteau | 16:7321fb3bb396 | 179 | |
ClementBreteau | 16:7321fb3bb396 | 180 | |
ClementBreteau | 16:7321fb3bb396 | 181 | } |
ClementBreteau | 16:7321fb3bb396 | 182 | |
ClementBreteau | 16:7321fb3bb396 | 183 | |
ClementBreteau | 16:7321fb3bb396 | 184 | void initAX12() |
ClementBreteau | 16:7321fb3bb396 | 185 | { |
ClementBreteau | 16:7321fb3bb396 | 186 | declarationAX12(); |
ClementBreteau | 16:7321fb3bb396 | 187 | |
ClementBreteau | 16:7321fb3bb396 | 188 | // init des AX12 de la partie CENTRALE du robot |
ClementBreteau | 16:7321fb3bb396 | 189 | partieBasseCentre-> Set_Goal_speed(VITESSE); // VITESSE (0-1023) |
ClementBreteau | 16:7321fb3bb396 | 190 | |
ClementBreteau | 16:7321fb3bb396 | 191 | partieMainGauche-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 192 | partieMainDroite-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 193 | partiePoignetCentre-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 194 | partieDoigtCentre-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 195 | |
ClementBreteau | 16:7321fb3bb396 | 196 | partieBasseCentre-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 197 | partieMainGauche-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 198 | partieMainDroite-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 199 | partiePoignetCentre-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 200 | partieDoigtCentre-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 201 | |
ClementBreteau | 16:7321fb3bb396 | 202 | // init des AX12 de ela partie GAUCHE du robot |
ClementBreteau | 16:7321fb3bb396 | 203 | partieBasseGauche-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 204 | partieMainGauche-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 205 | partiePorteGauche-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 206 | |
ClementBreteau | 16:7321fb3bb396 | 207 | partieBasseGauche-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 208 | partieMainGauche-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 209 | partiePorteGauche-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 210 | |
ClementBreteau | 16:7321fb3bb396 | 211 | // init des AX12 de ela partie DROITE du robot |
ClementBreteau | 16:7321fb3bb396 | 212 | partieBasseDroite-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 213 | partieMainDroite-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 214 | //partiePorteDroite-> Set_Goal_speed(VITESSE); |
ClementBreteau | 16:7321fb3bb396 | 215 | |
ClementBreteau | 16:7321fb3bb396 | 216 | partieBasseDroite-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 217 | partieMainDroite-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 218 | //partiePorteDroite-> Set_Mode(0); |
ClementBreteau | 16:7321fb3bb396 | 219 | |
ClementBreteau | 16:7321fb3bb396 | 220 | |
ClementBreteau | 16:7321fb3bb396 | 221 | } |
ClementBreteau | 16:7321fb3bb396 | 222 | |
ClementBreteau | 16:7321fb3bb396 | 223 | |
ClementBreteau | 16:7321fb3bb396 | 224 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 225 | /* FUNCTION NAME: Initialisation_position */ |
ClementBreteau | 16:7321fb3bb396 | 226 | /* DESCRIPTION : Fonction qui place les bras en position verticale */ |
ClementBreteau | 16:7321fb3bb396 | 227 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 228 | void Initialisation_position(void){ |
ClementBreteau | 16:7321fb3bb396 | 229 | multipleCentre->multiple_goal_and_speed(5,TAB12); |
ClementBreteau | 16:7321fb3bb396 | 230 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 231 | multipleCentre->multiple_goal_and_speed(5,TAB1); |
ClementBreteau | 16:7321fb3bb396 | 232 | //wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 233 | multipleGauche->multiple_goal_and_speed(3,TAB21); |
ClementBreteau | 16:7321fb3bb396 | 234 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 235 | } |
ClementBreteau | 16:7321fb3bb396 | 236 | |
ClementBreteau | 16:7321fb3bb396 | 237 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 238 | /* FUNCTION NAME: Preparation_prise */ |
ClementBreteau | 16:7321fb3bb396 | 239 | /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ |
ClementBreteau | 16:7321fb3bb396 | 240 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 241 | void Preparation_prise(void){ |
ClementBreteau | 16:7321fb3bb396 | 242 | /*if (action_precedente == 0){ |
ClementBreteau | 16:7321fb3bb396 | 243 | multipleCentre->multiple_goal_and_speed(5,TAB12); |
ClementBreteau | 16:7321fb3bb396 | 244 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 245 | action_precedente = 1; |
ClementBreteau | 16:7321fb3bb396 | 246 | }*/ |
ClementBreteau | 16:7321fb3bb396 | 247 | multipleCentre->multiple_goal_and_speed(5,TAB2); |
ClementBreteau | 16:7321fb3bb396 | 248 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 249 | } |
ClementBreteau | 16:7321fb3bb396 | 250 | |
ClementBreteau | 16:7321fb3bb396 | 251 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 252 | /* FUNCTION NAME: Stockage_haut */ |
ClementBreteau | 16:7321fb3bb396 | 253 | /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ |
ClementBreteau | 16:7321fb3bb396 | 254 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 255 | void Stockage_haut(void){ |
ClementBreteau | 16:7321fb3bb396 | 256 | multipleCentre->multiple_goal_and_speed(5,TAB3); |
ClementBreteau | 16:7321fb3bb396 | 257 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 258 | multipleCentre->multiple_goal_and_speed(5,TAB4); |
ClementBreteau | 16:7321fb3bb396 | 259 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 260 | multipleCentre->multiple_goal_and_speed(5,TAB5); |
ClementBreteau | 16:7321fb3bb396 | 261 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 262 | multipleCentre->multiple_goal_and_speed(5,TAB6); |
ClementBreteau | 16:7321fb3bb396 | 263 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 264 | } |
ClementBreteau | 16:7321fb3bb396 | 265 | |
ClementBreteau | 16:7321fb3bb396 | 266 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 267 | /* FUNCTION NAME: Stockage_bas */ |
ClementBreteau | 16:7321fb3bb396 | 268 | /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ |
ClementBreteau | 16:7321fb3bb396 | 269 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 270 | void Stockage_bas(void){ |
ClementBreteau | 16:7321fb3bb396 | 271 | multipleCentre->multiple_goal_and_speed(5,TAB3); |
ClementBreteau | 16:7321fb3bb396 | 272 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 273 | multipleCentre->multiple_goal_and_speed(5,TAB7); |
ClementBreteau | 16:7321fb3bb396 | 274 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 275 | } |
ClementBreteau | 16:7321fb3bb396 | 276 | |
ClementBreteau | 16:7321fb3bb396 | 277 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 278 | /* FUNCTION NAME: Deposer */ |
ClementBreteau | 16:7321fb3bb396 | 279 | /* DESCRIPTION : Fonction qui permet de déposer le module */ |
ClementBreteau | 16:7321fb3bb396 | 280 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 281 | void Deposer(void){ |
ClementBreteau | 16:7321fb3bb396 | 282 | multipleCentre->multiple_goal_and_speed(5,TAB9); |
ClementBreteau | 16:7321fb3bb396 | 283 | wait(TIME/5); |
ClementBreteau | 16:7321fb3bb396 | 284 | multipleCentre->multiple_goal_and_speed(5,TAB8); |
ClementBreteau | 16:7321fb3bb396 | 285 | wait(TIME/5); |
ClementBreteau | 16:7321fb3bb396 | 286 | //multipleCentre->multiple_goal_and_speed(5,TAB13); |
ClementBreteau | 16:7321fb3bb396 | 287 | wait(TIME/5); |
ClementBreteau | 16:7321fb3bb396 | 288 | } |
ClementBreteau | 16:7321fb3bb396 | 289 | |
ClementBreteau | 16:7321fb3bb396 | 290 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 291 | /* FUNCTION NAME: Preparation_depot_bas */ |
ClementBreteau | 16:7321fb3bb396 | 292 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ |
ClementBreteau | 16:7321fb3bb396 | 293 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 294 | void Preparation_depot_bas(void){ |
ClementBreteau | 16:7321fb3bb396 | 295 | multipleCentre->multiple_goal_and_speed(5,TAB8); |
ClementBreteau | 16:7321fb3bb396 | 296 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 297 | } |
ClementBreteau | 16:7321fb3bb396 | 298 | |
ClementBreteau | 16:7321fb3bb396 | 299 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 300 | /* FUNCTION NAME: Preparation_depot_haut */ |
ClementBreteau | 16:7321fb3bb396 | 301 | /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ |
ClementBreteau | 16:7321fb3bb396 | 302 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 303 | void Preparation_depot_haut(void){ |
ClementBreteau | 16:7321fb3bb396 | 304 | multipleCentre->multiple_goal_and_speed(5,TAB14); |
ClementBreteau | 16:7321fb3bb396 | 305 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 306 | multipleCentre->multiple_goal_and_speed(5,TAB16); |
ClementBreteau | 16:7321fb3bb396 | 307 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 308 | multipleCentre->multiple_goal_and_speed(5,TAB15); |
ClementBreteau | 16:7321fb3bb396 | 309 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 310 | multipleCentre->multiple_goal_and_speed(5,TAB10); |
ClementBreteau | 16:7321fb3bb396 | 311 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 312 | multipleCentre->multiple_goal_and_speed(5,TAB8); |
ClementBreteau | 16:7321fb3bb396 | 313 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 314 | } |
ClementBreteau | 16:7321fb3bb396 | 315 | |
ClementBreteau | 16:7321fb3bb396 | 316 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 317 | /* FUNCTION NAME: Pousser_module */ |
ClementBreteau | 16:7321fb3bb396 | 318 | /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ |
ClementBreteau | 16:7321fb3bb396 | 319 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 320 | void Pousser_module(void){ |
ClementBreteau | 16:7321fb3bb396 | 321 | multipleCentre->multiple_goal_and_speed(5,TAB11); |
ClementBreteau | 16:7321fb3bb396 | 322 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 323 | } |
ClementBreteau | 16:7321fb3bb396 | 324 | |
ClementBreteau | 16:7321fb3bb396 | 325 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 326 | /* FUNCTION NAME: Initialisation_gauche */ |
ClementBreteau | 16:7321fb3bb396 | 327 | /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ |
ClementBreteau | 16:7321fb3bb396 | 328 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 329 | void Initialisation_gauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 330 | //trois_myAX12_2->Set_Secure_Goal(235); |
ClementBreteau | 16:7321fb3bb396 | 331 | multipleGauche->multiple_goal_and_speed(5,TAB22); |
ClementBreteau | 16:7321fb3bb396 | 332 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 333 | multipleGauche->multiple_goal_and_speed(5,TAB21); |
ClementBreteau | 16:7321fb3bb396 | 334 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 335 | } |
ClementBreteau | 16:7321fb3bb396 | 336 | |
ClementBreteau | 16:7321fb3bb396 | 337 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 338 | /* FUNCTION NAME: Preparation_prise_gauche */ |
ClementBreteau | 16:7321fb3bb396 | 339 | /* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */ |
ClementBreteau | 16:7321fb3bb396 | 340 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 341 | void Preparation_prise_gauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 342 | //trois_myAX12_2->Set_Secure_Goal(120); |
ClementBreteau | 16:7321fb3bb396 | 343 | //multiplePorteGauche->multiple_goal_and_speed(5,TAB); |
ClementBreteau | 16:7321fb3bb396 | 344 | } |
ClementBreteau | 16:7321fb3bb396 | 345 | |
ClementBreteau | 16:7321fb3bb396 | 346 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 347 | /* FUNCTION NAME: Prendre_module_gauche */ |
ClementBreteau | 16:7321fb3bb396 | 348 | /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ |
ClementBreteau | 16:7321fb3bb396 | 349 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 350 | void Prendre_module_gauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 351 | //trois_myAX12_2->Set_Secure_Goal(160); |
ClementBreteau | 16:7321fb3bb396 | 352 | //multiplePorteGauche->multiple_goal_and_speed(5,TAB); |
ClementBreteau | 16:7321fb3bb396 | 353 | } |
ClementBreteau | 16:7321fb3bb396 | 354 | |
ClementBreteau | 16:7321fb3bb396 | 355 | /***************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 356 | /* FUNCTION NAME: RangerBrasGauche */ |
ClementBreteau | 16:7321fb3bb396 | 357 | /* DESCRIPTION : Fonction range le bras gauche */ |
ClementBreteau | 16:7321fb3bb396 | 358 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 359 | void RangerBrasGauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 360 | multipleGauche->multiple_goal_and_speed(3,TAB21); |
ClementBreteau | 16:7321fb3bb396 | 361 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 362 | } |
ClementBreteau | 16:7321fb3bb396 | 363 | |
ClementBreteau | 16:7321fb3bb396 | 364 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 365 | /* FUNCTION NAME: Preparation_module_gauche */ |
ClementBreteau | 16:7321fb3bb396 | 366 | /* DESCRIPTION : Fonction qui prepare la tournante */ |
ClementBreteau | 16:7321fb3bb396 | 367 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 368 | void Preparation_module_gauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 369 | multipleGauche->multiple_goal_and_speed(5,TAB22); |
ClementBreteau | 16:7321fb3bb396 | 370 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 371 | } |
ClementBreteau | 16:7321fb3bb396 | 372 | |
ClementBreteau | 16:7321fb3bb396 | 373 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 374 | /* FUNCTION NAME: Tourner_module_gauche */ |
ClementBreteau | 16:7321fb3bb396 | 375 | /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ |
ClementBreteau | 16:7321fb3bb396 | 376 | /****************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 377 | void Tourner_module_gauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 378 | //#define POSITION_TOURNER 0xE5 |
ClementBreteau | 16:7321fb3bb396 | 379 | //#define POSITION_MAX 0xFF |
ClementBreteau | 16:7321fb3bb396 | 380 | /*int position = POSITION_TOURNER; |
ClementBreteau | 16:7321fb3bb396 | 381 | TAB23[6] = POSITION_TOURNER;*/ |
ClementBreteau | 16:7321fb3bb396 | 382 | |
ClementBreteau | 16:7321fb3bb396 | 383 | //multipleGauche->multiple_goal_and_speed(5,TAB23); |
ClementBreteau | 16:7321fb3bb396 | 384 | wait(TIME); |
ClementBreteau | 16:7321fb3bb396 | 385 | /*while((dataPressionGauche == false)&&(position < POSITION_MAX)){ |
ClementBreteau | 16:7321fb3bb396 | 386 | position++; |
ClementBreteau | 16:7321fb3bb396 | 387 | TAB23[6] = position; |
ClementBreteau | 16:7321fb3bb396 | 388 | multipleGauche->multiple_goal_and_speed(5,TAB23); |
ClementBreteau | 16:7321fb3bb396 | 389 | }*/ |
ClementBreteau | 16:7321fb3bb396 | 390 | |
ClementBreteau | 16:7321fb3bb396 | 391 | while(dataCouleurGauche() == false){ |
ClementBreteau | 16:7321fb3bb396 | 392 | printf("ici"); |
ClementBreteau | 16:7321fb3bb396 | 393 | moteurDroitPWM(0.2); |
ClementBreteau | 16:7321fb3bb396 | 394 | } |
ClementBreteau | 16:7321fb3bb396 | 395 | moteurDroitPWM(0); |
ClementBreteau | 16:7321fb3bb396 | 396 | } |
ClementBreteau | 16:7321fb3bb396 | 397 | |
ClementBreteau | 16:7321fb3bb396 | 398 | |
ClementBreteau | 16:7321fb3bb396 | 399 | void getPosiotionCentrale(void){ |
ClementBreteau | 16:7321fb3bb396 | 400 | pc.printf("\n\r * pince avant * \n\r"); |
ClementBreteau | 16:7321fb3bb396 | 401 | |
ClementBreteau | 16:7321fb3bb396 | 402 | pc.printf("base : %lf \n\r ", partieBasseCentre->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 403 | pc.printf("coude : %lf \n\r ", partiePoignetCentre->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 404 | pc.printf("doigtG: %lf \n\r ", partieMainGaucheCentre->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 405 | pc.printf("doigtD: %lf \n\n\r ", partieMainDroiteCentre->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 406 | |
ClementBreteau | 16:7321fb3bb396 | 407 | wait(0.2); |
ClementBreteau | 16:7321fb3bb396 | 408 | } |
ClementBreteau | 16:7321fb3bb396 | 409 | |
ClementBreteau | 16:7321fb3bb396 | 410 | void getPosiotionGauche(void){ |
ClementBreteau | 16:7321fb3bb396 | 411 | pc.printf("\n\r * bras gauche *\n\r"); |
ClementBreteau | 16:7321fb3bb396 | 412 | |
ClementBreteau | 16:7321fb3bb396 | 413 | pc.printf("epaule : %lf \n\r ",partieBasseGauche->Get_Position()); |
ClementBreteau | 16:7321fb3bb396 | 414 | pc.printf("main : %lf \n\r ",partieMainGauche->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 415 | pc.printf("porte : %lf \n\r ",partiePorteGauche->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 416 | |
ClementBreteau | 16:7321fb3bb396 | 417 | wait(0.2); |
ClementBreteau | 16:7321fb3bb396 | 418 | } |
ClementBreteau | 16:7321fb3bb396 | 419 | |
ClementBreteau | 16:7321fb3bb396 | 420 | void getPosiotionDroite(void){ |
ClementBreteau | 16:7321fb3bb396 | 421 | pc.printf("\n\r * bras droit *\n\r"); |
ClementBreteau | 16:7321fb3bb396 | 422 | |
ClementBreteau | 16:7321fb3bb396 | 423 | pc.printf("epaule : %lf \n\r ",partieBasseDroite->Get_Position()); |
ClementBreteau | 16:7321fb3bb396 | 424 | pc.printf("main : %lf \n\r ",partieMainDroite->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 425 | pc.printf("porte : %lf \n\r ",partiePorteDroite->Get_Position() ); |
ClementBreteau | 16:7321fb3bb396 | 426 | |
ClementBreteau | 16:7321fb3bb396 | 427 | |
ClementBreteau | 16:7321fb3bb396 | 428 | wait(0.2); |
ClementBreteau | 16:7321fb3bb396 | 429 | } |
ClementBreteau | 16:7321fb3bb396 | 430 | |
ClementBreteau | 16:7321fb3bb396 | 431 | |
ClementBreteau | 16:7321fb3bb396 | 432 | |
ClementBreteau | 16:7321fb3bb396 | 433 | |
ClementBreteau | 16:7321fb3bb396 | 434 | /********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 435 | |
ClementBreteau | 16:7321fb3bb396 | 436 | |
ClementBreteau | 16:7321fb3bb396 | 437 | /*********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 438 | /* FUNCTION NAME: moteurGauchePWM */ |
ClementBreteau | 16:7321fb3bb396 | 439 | /* DESCRIPTION : bouge le moteur gauche */ |
ClementBreteau | 16:7321fb3bb396 | 440 | /*********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 441 | void moteurGauchePWM(float pwm){ |
ClementBreteau | 16:7321fb3bb396 | 442 | motGauche.write(pwm); |
ClementBreteau | 16:7321fb3bb396 | 443 | } |
ClementBreteau | 16:7321fb3bb396 | 444 | |
ClementBreteau | 16:7321fb3bb396 | 445 | /*********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 446 | /* FUNCTION NAME: moteurDroitPWM */ |
ClementBreteau | 16:7321fb3bb396 | 447 | /* DESCRIPTION : bouge le moteur gauche */ |
ClementBreteau | 16:7321fb3bb396 | 448 | /*********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 449 | void moteurDroitPWM(float pwm){ |
ClementBreteau | 16:7321fb3bb396 | 450 | motDroit.write(pwm); |
ClementBreteau | 16:7321fb3bb396 | 451 | } |
ClementBreteau | 16:7321fb3bb396 | 452 | |
ClementBreteau | 16:7321fb3bb396 | 453 | /*********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 454 | /* FUNCTION NAME: initMoteurs */ |
ClementBreteau | 16:7321fb3bb396 | 455 | /* DESCRIPTION : init les moteurs des mains du petit robot */ |
ClementBreteau | 16:7321fb3bb396 | 456 | /*********************************************************************************************************/ |
ClementBreteau | 16:7321fb3bb396 | 457 | void initMoteurs(void){ |
ClementBreteau | 16:7321fb3bb396 | 458 | motGauche.period(T_MOT); |
ClementBreteau | 16:7321fb3bb396 | 459 | motDroit.period(T_MOT); |
ClementBreteau | 16:7321fb3bb396 | 460 | motGauche.write(0.0); |
ClementBreteau | 16:7321fb3bb396 | 461 | motDroit.write(0.0); |
ClementBreteau | 16:7321fb3bb396 | 462 | } |