CRAC Team / CRAC-Strat_2017-19-05-2017

strat des robots

Fork of CRAC-Strat_2017 by CRAC Team

peripheriques/actionneurs.cpp@17:d1594579eec6, 2017-05-19 (annotated)

Committer:
ClementBreteau
Date:
Fri May 19 17:14:07 2017 +0000
Revision:
17:d1594579eec6
Parent:
16:7321fb3bb396 
strat du robot, 19-05-2017, 19h

Who changed what in which revision?

UserRevisionLine numberNew 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 }