CRAC Team / CRAC-Strat_2017_homologation_gros_rob_testAck

homologation gros robot et test avec les ack de la carte a tout faire

Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by CRAC Team

peripheriques/actionneurs.cpp@18:cc5fec34ed9c, 2017-05-22 (annotated)

Committer:
ClementBreteau
Date:
Mon May 22 15:01:49 2017 +0000
Revision:
18:cc5fec34ed9c
Parent:
16:7321fb3bb396
Child:
19:b4b91258c275
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ClementBreteau 16:7321fb3bb396 1 #include "peripheriques.h"
ClementBreteau 18:cc5fec34ed9c 2 /* contient les fonctions qui servent à utiliser les AX12 et les moteurs sur le petit robot*/
ClementBreteau 18:cc5fec34ed9c 3 /*
ClementBreteau 18:cc5fec34ed9c 4 DigitalIn IO1(p23);
ClementBreteau 18:cc5fec34ed9c 5 DigitalIn IO2(p24);
ClementBreteau 18:cc5fec34ed9c 6 DigitalIn IO3(p25);
ClementBreteau 18:cc5fec34ed9c 7 DigitalIn IO4(p26);
ClementBreteau 16:7321fb3bb396 8
ClementBreteau 18:cc5fec34ed9c 9 AnalogIn A_in1(p15);
ClementBreteau 18:cc5fec34ed9c 10 AnalogIn A_in2(p16);
ClementBreteau 18:cc5fec34ed9c 11 AnalogIn A_in3(p17);
ClementBreteau 18:cc5fec34ed9c 12 AnalogIn A_in4(p18);
ClementBreteau 18:cc5fec34ed9c 13 AnalogIn A_in5(p19);
ClementBreteau 18:cc5fec34ed9c 14 AnalogIn A_in6(p20);
ClementBreteau 18:cc5fec34ed9c 15
ClementBreteau 18:cc5fec34ed9c 16 PwmOut IRL_1(p21);
ClementBreteau 18:cc5fec34ed9c 17 PwmOut IRL_2(p22);
ClementBreteau 18:cc5fec34ed9c 18 */
ClementBreteau 16:7321fb3bb396 19 PwmOut motGauche(p21);
ClementBreteau 16:7321fb3bb396 20 PwmOut motDroit(p22);
ClementBreteau 16:7321fb3bb396 21 Timer t;
ClementBreteau 16:7321fb3bb396 22
ClementBreteau 18:cc5fec34ed9c 23 unsigned char action_precedente = 0;
ClementBreteau 18:cc5fec34ed9c 24
ClementBreteau 18:cc5fec34ed9c 25 /* DECLARATION VARIABLES */
ClementBreteau 16:7321fb3bb396 26
ClementBreteau 18:cc5fec34ed9c 27 extern unsigned char FlagAx12;
ClementBreteau 16:7321fb3bb396 28
ClementBreteau 18:cc5fec34ed9c 29 extern DigitalOut led2;
ClementBreteau 18:cc5fec34ed9c 30 extern Serial pc;
ClementBreteau 18:cc5fec34ed9c 31 extern Timer t;
ClementBreteau 18:cc5fec34ed9c 32 extern void GetPositionAx12(void);
ClementBreteau 16:7321fb3bb396 33
ClementBreteau 18:cc5fec34ed9c 34 AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
ClementBreteau 18:cc5fec34ed9c 35 *CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
ClementBreteau 18:cc5fec34ed9c 36 *CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR;
ClementBreteau 18:cc5fec34ed9c 37
ClementBreteau 18:cc5fec34ed9c 38
ClementBreteau 16:7321fb3bb396 39
ClementBreteau 16:7321fb3bb396 40 /****************************************************************************************/
ClementBreteau 16:7321fb3bb396 41 /* FUNCTION NAME: Tourner_module_gauche */
ClementBreteau 16:7321fb3bb396 42 /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
ClementBreteau 16:7321fb3bb396 43 /****************************************************************************************/
ClementBreteau 16:7321fb3bb396 44 void Tourner_module_gauche(void){
ClementBreteau 16:7321fb3bb396 45 while(dataCouleurGauche() == false){
ClementBreteau 16:7321fb3bb396 46 printf("ici");
ClementBreteau 16:7321fb3bb396 47 moteurDroitPWM(0.2);
ClementBreteau 16:7321fb3bb396 48 }
ClementBreteau 16:7321fb3bb396 49 moteurDroitPWM(0);
ClementBreteau 16:7321fb3bb396 50 }
ClementBreteau 18:cc5fec34ed9c 51 /****************************************************************************************/
ClementBreteau 18:cc5fec34ed9c 52 /* FUNCTION NAME: Tourner_module_droit */
ClementBreteau 18:cc5fec34ed9c 53 /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
ClementBreteau 18:cc5fec34ed9c 54 /****************************************************************************************/
ClementBreteau 18:cc5fec34ed9c 55 void Tourner_module_droit(void){
ClementBreteau 18:cc5fec34ed9c 56 while(dataCouleurDroit() == false){
ClementBreteau 18:cc5fec34ed9c 57 printf("ici");
ClementBreteau 18:cc5fec34ed9c 58 moteurGauchePWM(0.2);
ClementBreteau 18:cc5fec34ed9c 59 }
ClementBreteau 18:cc5fec34ed9c 60 moteurGauchePWM(0);
ClementBreteau 18:cc5fec34ed9c 61 }
ClementBreteau 16:7321fb3bb396 62
ClementBreteau 16:7321fb3bb396 63
ClementBreteau 16:7321fb3bb396 64 /********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 65
ClementBreteau 16:7321fb3bb396 66
ClementBreteau 16:7321fb3bb396 67 /*********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 68 /* FUNCTION NAME: moteurGauchePWM */
ClementBreteau 16:7321fb3bb396 69 /* DESCRIPTION : bouge le moteur gauche */
ClementBreteau 16:7321fb3bb396 70 /*********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 71 void moteurGauchePWM(float pwm){
ClementBreteau 16:7321fb3bb396 72 motGauche.write(pwm);
ClementBreteau 16:7321fb3bb396 73 }
ClementBreteau 16:7321fb3bb396 74
ClementBreteau 16:7321fb3bb396 75 /*********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 76 /* FUNCTION NAME: moteurDroitPWM */
ClementBreteau 16:7321fb3bb396 77 /* DESCRIPTION : bouge le moteur gauche */
ClementBreteau 16:7321fb3bb396 78 /*********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 79 void moteurDroitPWM(float pwm){
ClementBreteau 16:7321fb3bb396 80 motDroit.write(pwm);
ClementBreteau 16:7321fb3bb396 81 }
ClementBreteau 16:7321fb3bb396 82
ClementBreteau 16:7321fb3bb396 83 /*********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 84 /* FUNCTION NAME: initMoteurs */
ClementBreteau 16:7321fb3bb396 85 /* DESCRIPTION : init les moteurs des mains du petit robot */
ClementBreteau 16:7321fb3bb396 86 /*********************************************************************************************************/
ClementBreteau 16:7321fb3bb396 87 void initMoteurs(void){
ClementBreteau 16:7321fb3bb396 88 motGauche.period(T_MOT);
ClementBreteau 16:7321fb3bb396 89 motDroit.period(T_MOT);
ClementBreteau 16:7321fb3bb396 90 motGauche.write(0.0);
ClementBreteau 16:7321fb3bb396 91 motDroit.write(0.0);
ClementBreteau 18:cc5fec34ed9c 92 }
ClementBreteau 18:cc5fec34ed9c 93
ClementBreteau 18:cc5fec34ed9c 94 void initialisation_AX12(void)
ClementBreteau 18:cc5fec34ed9c 95 {
ClementBreteau 18:cc5fec34ed9c 96 short vitesse=700;
ClementBreteau 18:cc5fec34ed9c 97
ClementBreteau 18:cc5fec34ed9c 98 BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);
ClementBreteau 18:cc5fec34ed9c 99 CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000);
ClementBreteau 18:cc5fec34ed9c 100 PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
ClementBreteau 18:cc5fec34ed9c 101 PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
ClementBreteau 18:cc5fec34ed9c 102 DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
ClementBreteau 18:cc5fec34ed9c 103
ClementBreteau 18:cc5fec34ed9c 104 CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
ClementBreteau 18:cc5fec34ed9c 105 CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
ClementBreteau 18:cc5fec34ed9c 106 EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
ClementBreteau 18:cc5fec34ed9c 107
ClementBreteau 18:cc5fec34ed9c 108 CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
ClementBreteau 18:cc5fec34ed9c 109 CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
ClementBreteau 18:cc5fec34ed9c 110 EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
ClementBreteau 18:cc5fec34ed9c 111
ClementBreteau 18:cc5fec34ed9c 112 BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
ClementBreteau 18:cc5fec34ed9c 113 BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
ClementBreteau 18:cc5fec34ed9c 114 BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
ClementBreteau 18:cc5fec34ed9c 115
ClementBreteau 18:cc5fec34ed9c 116 BaseBrasCentralPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 117 CoudeBrasCentralPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 118 PinceDBrasCentralPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 119 PinceGBrasCentralPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 120 DoigtBrasCentralPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 121
ClementBreteau 18:cc5fec34ed9c 122 CrocBrasGauchePR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 123 CoudeBrasGauchePR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 124 EpauleBrasGauchePR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 125
ClementBreteau 18:cc5fec34ed9c 126 CrocBrasDroitPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 127 CoudeBrasDroitPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 128 EpauleBrasDroitPR->Set_Goal_speed(vitesse);
ClementBreteau 18:cc5fec34ed9c 129
ClementBreteau 18:cc5fec34ed9c 130 BaseBrasCentralPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 131 CoudeBrasCentralPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 132 PinceDBrasCentralPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 133 PinceGBrasCentralPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 134 DoigtBrasCentralPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 135
ClementBreteau 18:cc5fec34ed9c 136 CrocBrasGauchePR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 137 CoudeBrasGauchePR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 138 EpauleBrasGauchePR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 139
ClementBreteau 18:cc5fec34ed9c 140 CrocBrasDroitPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 141 CoudeBrasDroitPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 142 EpauleBrasDroitPR->Set_Mode(0);
ClementBreteau 18:cc5fec34ed9c 143 }
ClementBreteau 18:cc5fec34ed9c 144
ClementBreteau 18:cc5fec34ed9c 145 void GetPositionAx12(void) {
ClementBreteau 18:cc5fec34ed9c 146
ClementBreteau 18:cc5fec34ed9c 147 printf("\n\r ");
ClementBreteau 18:cc5fec34ed9c 148
ClementBreteau 18:cc5fec34ed9c 149 printf("BaseC : %lf \n\r ", BaseBrasCentralPR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 150 printf("CoudeC : %lf \n\r ", CoudeBrasCentralPR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 151 printf("PinceCD : %lf \n\r ", PinceDBrasCentralPR->Get_Position());
ClementBreteau 18:cc5fec34ed9c 152 printf("PinceCG : %lf \n\r ", PinceGBrasCentralPR->Get_Position());
ClementBreteau 18:cc5fec34ed9c 153 printf("DoigtC : %lf \n\r ", DoigtBrasCentralPR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 154
ClementBreteau 18:cc5fec34ed9c 155 printf("EpauleG : %lf \n\r ", EpauleBrasGauchePR->Get_Position());
ClementBreteau 18:cc5fec34ed9c 156 printf("CoudeG : %lf \n\r ", CoudeBrasGauchePR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 157 printf("CrocG : %lf \n\r ", CrocBrasGauchePR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 158
ClementBreteau 18:cc5fec34ed9c 159 printf("EpauleD : %lf \n\r ", EpauleBrasDroitPR->Get_Position());
ClementBreteau 18:cc5fec34ed9c 160 printf("CoudeD : %lf \n\r ", CoudeBrasDroitPR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 161 printf("CrocD : %lf \n\r ", CrocBrasDroitPR->Get_Position() );
ClementBreteau 18:cc5fec34ed9c 162 }
ClementBreteau 18:cc5fec34ed9c 163
ClementBreteau 18:cc5fec34ed9c 164
ClementBreteau 18:cc5fec34ed9c 165 /****************************************************************************************/
ClementBreteau 18:cc5fec34ed9c 166 /* FUNCTION NAME: Automate_ax12 */
ClementBreteau 18:cc5fec34ed9c 167 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
ClementBreteau 18:cc5fec34ed9c 168 /****************************************************************************************/
ClementBreteau 18:cc5fec34ed9c 169 void AX12_automate(unsigned char etat_ax12){
ClementBreteau 18:cc5fec34ed9c 170
ClementBreteau 18:cc5fec34ed9c 171 unsigned short speed;
ClementBreteau 18:cc5fec34ed9c 172 unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
ClementBreteau 18:cc5fec34ed9c 173 GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
ClementBreteau 18:cc5fec34ed9c 174 GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
ClementBreteau 18:cc5fec34ed9c 175
ClementBreteau 18:cc5fec34ed9c 176 speed = 1000;
ClementBreteau 18:cc5fec34ed9c 177
ClementBreteau 18:cc5fec34ed9c 178 switch(etat_ax12){
ClementBreteau 18:cc5fec34ed9c 179
ClementBreteau 18:cc5fec34ed9c 180 case AX12_PINCE_CENTRALE_POSITION_INITIALE :
ClementBreteau 18:cc5fec34ed9c 181 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 182 speed = 511;
ClementBreteau 18:cc5fec34ed9c 183 GoalPosDoigt=1150;
ClementBreteau 18:cc5fec34ed9c 184 GoalPosBase=1490;
ClementBreteau 18:cc5fec34ed9c 185 GoalPosCoude=1470;
ClementBreteau 18:cc5fec34ed9c 186 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 187 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 188 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 189
ClementBreteau 18:cc5fec34ed9c 190 break;
ClementBreteau 18:cc5fec34ed9c 191
ClementBreteau 18:cc5fec34ed9c 192 case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
ClementBreteau 18:cc5fec34ed9c 193 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 194 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 195 GoalPosBase=170;
ClementBreteau 18:cc5fec34ed9c 196 GoalPosCoude=1000;
ClementBreteau 18:cc5fec34ed9c 197 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 198 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 199 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 200
ClementBreteau 18:cc5fec34ed9c 201 break;
ClementBreteau 18:cc5fec34ed9c 202
ClementBreteau 18:cc5fec34ed9c 203 case AX12_PINCE_CENTRALE_PRISE_MODULE :
ClementBreteau 18:cc5fec34ed9c 204 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 205 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 206 GoalPosBase=170;
ClementBreteau 18:cc5fec34ed9c 207 GoalPosCoude=1000;
ClementBreteau 18:cc5fec34ed9c 208 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 209 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 210 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 211
ClementBreteau 18:cc5fec34ed9c 212 break;
ClementBreteau 18:cc5fec34ed9c 213
ClementBreteau 18:cc5fec34ed9c 214 case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
ClementBreteau 18:cc5fec34ed9c 215 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 216 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 217 GoalPosBase=1300;
ClementBreteau 18:cc5fec34ed9c 218 GoalPosCoude=700;
ClementBreteau 18:cc5fec34ed9c 219 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 220 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 221 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 222
ClementBreteau 18:cc5fec34ed9c 223 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 224 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 225 GoalPosBase=1450;//1050;
ClementBreteau 18:cc5fec34ed9c 226 GoalPosCoude=700;//1528;
ClementBreteau 18:cc5fec34ed9c 227 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 228 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 229 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 230
ClementBreteau 18:cc5fec34ed9c 231 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 232 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 233 GoalPosBase=1450;//1050;
ClementBreteau 18:cc5fec34ed9c 234 GoalPosCoude=1250;//1528;
ClementBreteau 18:cc5fec34ed9c 235 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 236 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 237 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 238
ClementBreteau 18:cc5fec34ed9c 239 break;
ClementBreteau 18:cc5fec34ed9c 240
ClementBreteau 18:cc5fec34ed9c 241 case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
ClementBreteau 18:cc5fec34ed9c 242 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 243 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 244 GoalPosBase=1000;
ClementBreteau 18:cc5fec34ed9c 245 GoalPosCoude=443;
ClementBreteau 18:cc5fec34ed9c 246 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 247 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 248 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 249
ClementBreteau 18:cc5fec34ed9c 250 break;
ClementBreteau 18:cc5fec34ed9c 251
ClementBreteau 18:cc5fec34ed9c 252 case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
ClementBreteau 18:cc5fec34ed9c 253 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 254 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 255 GoalPosBase=639;
ClementBreteau 18:cc5fec34ed9c 256 GoalPosCoude=557;
ClementBreteau 18:cc5fec34ed9c 257 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 258 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 259 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 260
ClementBreteau 18:cc5fec34ed9c 261 wait(TIME*10);
ClementBreteau 18:cc5fec34ed9c 262 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 263 GoalPosBase=400;
ClementBreteau 18:cc5fec34ed9c 264 GoalPosCoude=400;
ClementBreteau 18:cc5fec34ed9c 265 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 266 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 267 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 268
ClementBreteau 18:cc5fec34ed9c 269
ClementBreteau 18:cc5fec34ed9c 270 break;
ClementBreteau 18:cc5fec34ed9c 271
ClementBreteau 18:cc5fec34ed9c 272 case AX12_PINCE_CENTRALE_DEPOSER :
ClementBreteau 18:cc5fec34ed9c 273 //DEPOSER
ClementBreteau 18:cc5fec34ed9c 274 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 275 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 276 GoalPosBase=440;
ClementBreteau 18:cc5fec34ed9c 277 GoalPosCoude=440;
ClementBreteau 18:cc5fec34ed9c 278 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 279 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 280 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 281
ClementBreteau 18:cc5fec34ed9c 282
ClementBreteau 18:cc5fec34ed9c 283 break;
ClementBreteau 18:cc5fec34ed9c 284
ClementBreteau 18:cc5fec34ed9c 285 case AX12_PINCE_CENTRALE_DEPOT_HAUT :
ClementBreteau 18:cc5fec34ed9c 286 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 287 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 288 GoalPosBase=1050;
ClementBreteau 18:cc5fec34ed9c 289 GoalPosCoude=1528;
ClementBreteau 18:cc5fec34ed9c 290 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 291 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 292 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 293
ClementBreteau 18:cc5fec34ed9c 294 wait(TIME*10);
ClementBreteau 18:cc5fec34ed9c 295 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 296 GoalPosBase=1050;
ClementBreteau 18:cc5fec34ed9c 297 GoalPosCoude=1528;
ClementBreteau 18:cc5fec34ed9c 298 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 299 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 300 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 301
ClementBreteau 18:cc5fec34ed9c 302 wait(TIME*10);
ClementBreteau 18:cc5fec34ed9c 303 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 304 GoalPosBase=1100;
ClementBreteau 18:cc5fec34ed9c 305 GoalPosCoude=700;
ClementBreteau 18:cc5fec34ed9c 306 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 307 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 308 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 309
ClementBreteau 18:cc5fec34ed9c 310
ClementBreteau 18:cc5fec34ed9c 311 break;
ClementBreteau 18:cc5fec34ed9c 312
ClementBreteau 18:cc5fec34ed9c 313 case AX12_GAUCHE_CROC_OUVERT :
ClementBreteau 18:cc5fec34ed9c 314 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 315 CrocBrasGauchePR->Set_Secure_Goal(200);
ClementBreteau 18:cc5fec34ed9c 316
ClementBreteau 18:cc5fec34ed9c 317 break;
ClementBreteau 18:cc5fec34ed9c 318
ClementBreteau 18:cc5fec34ed9c 319 case AX12_GAUCHE_CROC_FERME :
ClementBreteau 18:cc5fec34ed9c 320 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 321 CrocBrasGauchePR->Set_Secure_Goal(240);
ClementBreteau 18:cc5fec34ed9c 322
ClementBreteau 18:cc5fec34ed9c 323 break;
ClementBreteau 18:cc5fec34ed9c 324
ClementBreteau 18:cc5fec34ed9c 325 case AX12_GAUCHE_CROC_INITIALE :
ClementBreteau 18:cc5fec34ed9c 326 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 327 CrocBrasGauchePR->Set_Secure_Goal(300);
ClementBreteau 18:cc5fec34ed9c 328
ClementBreteau 18:cc5fec34ed9c 329 break;
ClementBreteau 18:cc5fec34ed9c 330
ClementBreteau 18:cc5fec34ed9c 331 case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
ClementBreteau 18:cc5fec34ed9c 332 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 333 speed = 511;
ClementBreteau 18:cc5fec34ed9c 334 GoalPosCoudeTournanteG=1450;
ClementBreteau 18:cc5fec34ed9c 335 GoalPosEpauleTournanteG=600;
ClementBreteau 18:cc5fec34ed9c 336 mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
ClementBreteau 18:cc5fec34ed9c 337
ClementBreteau 18:cc5fec34ed9c 338
ClementBreteau 18:cc5fec34ed9c 339 break;
ClementBreteau 18:cc5fec34ed9c 340
ClementBreteau 18:cc5fec34ed9c 341 case AX12_TOURNANTE_GAUCHE_PREPARATION :
ClementBreteau 18:cc5fec34ed9c 342 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 343 speed = 511;
ClementBreteau 18:cc5fec34ed9c 344 GoalPosCoudeTournanteG=930;
ClementBreteau 18:cc5fec34ed9c 345 GoalPosEpauleTournanteG=1962;
ClementBreteau 18:cc5fec34ed9c 346 mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
ClementBreteau 18:cc5fec34ed9c 347
ClementBreteau 18:cc5fec34ed9c 348
ClementBreteau 18:cc5fec34ed9c 349 break;
ClementBreteau 18:cc5fec34ed9c 350
ClementBreteau 18:cc5fec34ed9c 351 case AX12_TOURNANTE_GAUCHE_MODULE :
ClementBreteau 18:cc5fec34ed9c 352 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 353 speed = 511;
ClementBreteau 18:cc5fec34ed9c 354 GoalPosCoudeTournanteG=894;
ClementBreteau 18:cc5fec34ed9c 355 GoalPosEpauleTournanteG=2200;
ClementBreteau 18:cc5fec34ed9c 356 mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
ClementBreteau 18:cc5fec34ed9c 357
ClementBreteau 18:cc5fec34ed9c 358
ClementBreteau 18:cc5fec34ed9c 359 break;
ClementBreteau 18:cc5fec34ed9c 360
ClementBreteau 18:cc5fec34ed9c 361 case AX12_DROIT_CROC_OUVERT :
ClementBreteau 18:cc5fec34ed9c 362 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 363 CrocBrasDroitPR->Set_Secure_Goal(106);
ClementBreteau 18:cc5fec34ed9c 364
ClementBreteau 18:cc5fec34ed9c 365 break;
ClementBreteau 18:cc5fec34ed9c 366
ClementBreteau 18:cc5fec34ed9c 367 case AX12_DROIT_CROC_FERME :
ClementBreteau 18:cc5fec34ed9c 368 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 369 CrocBrasDroitPR->Set_Secure_Goal(55);
ClementBreteau 18:cc5fec34ed9c 370
ClementBreteau 18:cc5fec34ed9c 371 break;
ClementBreteau 18:cc5fec34ed9c 372
ClementBreteau 18:cc5fec34ed9c 373 case AX12_DROIT_CROC_INITIALE :
ClementBreteau 18:cc5fec34ed9c 374 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 375 CrocBrasDroitPR->Set_Secure_Goal(0);
ClementBreteau 18:cc5fec34ed9c 376
ClementBreteau 18:cc5fec34ed9c 377 break;
ClementBreteau 18:cc5fec34ed9c 378
ClementBreteau 18:cc5fec34ed9c 379 case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
ClementBreteau 18:cc5fec34ed9c 380 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 381 speed = 511;
ClementBreteau 18:cc5fec34ed9c 382 GoalPosCoudeTournanteD=1610;
ClementBreteau 18:cc5fec34ed9c 383 GoalPosEpauleTournanteD=2337;
ClementBreteau 18:cc5fec34ed9c 384 mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
ClementBreteau 18:cc5fec34ed9c 385
ClementBreteau 18:cc5fec34ed9c 386
ClementBreteau 18:cc5fec34ed9c 387 break;
ClementBreteau 18:cc5fec34ed9c 388
ClementBreteau 18:cc5fec34ed9c 389 case AX12_TOURNANTE_DROIT_PREPARATION :
ClementBreteau 18:cc5fec34ed9c 390 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 391 speed = 511;
ClementBreteau 18:cc5fec34ed9c 392 GoalPosCoudeTournanteD=930;
ClementBreteau 18:cc5fec34ed9c 393 GoalPosEpauleTournanteD=1962;
ClementBreteau 18:cc5fec34ed9c 394 mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
ClementBreteau 18:cc5fec34ed9c 395
ClementBreteau 18:cc5fec34ed9c 396
ClementBreteau 18:cc5fec34ed9c 397 break;
ClementBreteau 18:cc5fec34ed9c 398
ClementBreteau 18:cc5fec34ed9c 399 case AX12_TOURNANTE_DROIT_MODULE :
ClementBreteau 18:cc5fec34ed9c 400 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 401 speed = 511;
ClementBreteau 18:cc5fec34ed9c 402 GoalPosCoudeTournanteD=894;
ClementBreteau 18:cc5fec34ed9c 403 GoalPosEpauleTournanteD=2200;
ClementBreteau 18:cc5fec34ed9c 404 mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
ClementBreteau 18:cc5fec34ed9c 405
ClementBreteau 18:cc5fec34ed9c 406
ClementBreteau 18:cc5fec34ed9c 407 break;
ClementBreteau 18:cc5fec34ed9c 408
ClementBreteau 18:cc5fec34ed9c 409 case AX12_DEFAUT :
ClementBreteau 18:cc5fec34ed9c 410 break;
ClementBreteau 18:cc5fec34ed9c 411
ClementBreteau 18:cc5fec34ed9c 412 case AX12_POSITION :
ClementBreteau 18:cc5fec34ed9c 413 GetPositionAx12();
ClementBreteau 18:cc5fec34ed9c 414 break;
ClementBreteau 18:cc5fec34ed9c 415 }
ClementBreteau 18:cc5fec34ed9c 416 }
ClementBreteau 18:cc5fec34ed9c 417
ClementBreteau 18:cc5fec34ed9c 418 void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
ClementBreteau 18:cc5fec34ed9c 419 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
ClementBreteau 18:cc5fec34ed9c 420 unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
ClementBreteau 18:cc5fec34ed9c 421 unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
ClementBreteau 18:cc5fec34ed9c 422 unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
ClementBreteau 18:cc5fec34ed9c 423 {
ClementBreteau 18:cc5fec34ed9c 424 char TabBrasCentralPR[25];
ClementBreteau 18:cc5fec34ed9c 425 unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
ClementBreteau 18:cc5fec34ed9c 426 Timer timeOut;
ClementBreteau 18:cc5fec34ed9c 427
ClementBreteau 18:cc5fec34ed9c 428
ClementBreteau 18:cc5fec34ed9c 429 GPosition1_1=((unsigned long)GPosition1*341/1000);
ClementBreteau 18:cc5fec34ed9c 430 GPosition2_1=((unsigned long)GPosition2*341/1000);
ClementBreteau 18:cc5fec34ed9c 431 GPosition3_1=((unsigned long)GPosition3*341/1000);
ClementBreteau 18:cc5fec34ed9c 432 GPosition4_1=((unsigned long)GPosition4*341/1000);
ClementBreteau 18:cc5fec34ed9c 433 GPosition5_1=((unsigned long)GPosition5*341/1000);
ClementBreteau 18:cc5fec34ed9c 434
ClementBreteau 18:cc5fec34ed9c 435 TabBrasCentralPR[0] = ID1;
ClementBreteau 18:cc5fec34ed9c 436 TabBrasCentralPR[1] = GPosition1_1;
ClementBreteau 18:cc5fec34ed9c 437 TabBrasCentralPR[2] = GPosition1_1>>8;
ClementBreteau 18:cc5fec34ed9c 438 TabBrasCentralPR[3] = GSpeed1;
ClementBreteau 18:cc5fec34ed9c 439 TabBrasCentralPR[4] = GSpeed1>>8;
ClementBreteau 18:cc5fec34ed9c 440
ClementBreteau 18:cc5fec34ed9c 441 TabBrasCentralPR[5] = ID2;
ClementBreteau 18:cc5fec34ed9c 442 TabBrasCentralPR[6] = GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 443 TabBrasCentralPR[7] = GPosition2_1>>8;
ClementBreteau 18:cc5fec34ed9c 444 TabBrasCentralPR[8] = GSpeed2;
ClementBreteau 18:cc5fec34ed9c 445 TabBrasCentralPR[9] = GSpeed2>>8;
ClementBreteau 18:cc5fec34ed9c 446
ClementBreteau 18:cc5fec34ed9c 447 TabBrasCentralPR[10] = ID3;
ClementBreteau 18:cc5fec34ed9c 448 TabBrasCentralPR[11] = GPosition3_1;
ClementBreteau 18:cc5fec34ed9c 449 TabBrasCentralPR[12] = GPosition3_1>>8;
ClementBreteau 18:cc5fec34ed9c 450 TabBrasCentralPR[13] = GSpeed3;
ClementBreteau 18:cc5fec34ed9c 451 TabBrasCentralPR[14] = GSpeed3>>8 ;
ClementBreteau 18:cc5fec34ed9c 452
ClementBreteau 18:cc5fec34ed9c 453 TabBrasCentralPR[15] = ID4;
ClementBreteau 18:cc5fec34ed9c 454 TabBrasCentralPR[16] = GPosition4_1;
ClementBreteau 18:cc5fec34ed9c 455 TabBrasCentralPR[17] = GPosition4_1>>8;
ClementBreteau 18:cc5fec34ed9c 456 TabBrasCentralPR[18] = GSpeed4;
ClementBreteau 18:cc5fec34ed9c 457 TabBrasCentralPR[19] = GSpeed4>>8 ;
ClementBreteau 18:cc5fec34ed9c 458
ClementBreteau 18:cc5fec34ed9c 459 TabBrasCentralPR[20] = ID5;
ClementBreteau 18:cc5fec34ed9c 460 TabBrasCentralPR[21] = GPosition5_1;
ClementBreteau 18:cc5fec34ed9c 461 TabBrasCentralPR[22] = GPosition5_1>>8;
ClementBreteau 18:cc5fec34ed9c 462 TabBrasCentralPR[23] = GSpeed5;
ClementBreteau 18:cc5fec34ed9c 463 TabBrasCentralPR[24] = GSpeed5>>8 ;
ClementBreteau 18:cc5fec34ed9c 464
ClementBreteau 18:cc5fec34ed9c 465
ClementBreteau 18:cc5fec34ed9c 466 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 467 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 468
ClementBreteau 18:cc5fec34ed9c 469 timeOut.start;
ClementBreteau 18:cc5fec34ed9c 470 while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*90/100) || (timeOut.read_ms() > 100)) {
ClementBreteau 18:cc5fec34ed9c 471 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 472 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 473
ClementBreteau 18:cc5fec34ed9c 474 }
ClementBreteau 18:cc5fec34ed9c 475
ClementBreteau 18:cc5fec34ed9c 476 timeOut.reset;
ClementBreteau 18:cc5fec34ed9c 477 while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*90/100)|| (timeOut.read_ms() > 100)) {
ClementBreteau 18:cc5fec34ed9c 478 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 479 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 480 }
ClementBreteau 18:cc5fec34ed9c 481
ClementBreteau 18:cc5fec34ed9c 482 timeOut.reset;
ClementBreteau 18:cc5fec34ed9c 483 while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*110/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*90/100)|| (timeOut.read_ms() > 100)) {
ClementBreteau 18:cc5fec34ed9c 484 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 485 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 486 }
ClementBreteau 18:cc5fec34ed9c 487
ClementBreteau 18:cc5fec34ed9c 488 timeOut.reset;
ClementBreteau 18:cc5fec34ed9c 489 while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*110/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*90/100)|| (timeOut.read_ms() > 100)) {
ClementBreteau 18:cc5fec34ed9c 490 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 491 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 492 }
ClementBreteau 18:cc5fec34ed9c 493
ClementBreteau 18:cc5fec34ed9c 494 timeOut.reset;
ClementBreteau 18:cc5fec34ed9c 495 while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*110/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*90/100)|| (timeOut.read_ms() > 100)) {
ClementBreteau 18:cc5fec34ed9c 496 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 497 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 498 }
ClementBreteau 18:cc5fec34ed9c 499
ClementBreteau 18:cc5fec34ed9c 500
ClementBreteau 18:cc5fec34ed9c 501 }
ClementBreteau 18:cc5fec34ed9c 502
ClementBreteau 18:cc5fec34ed9c 503
ClementBreteau 18:cc5fec34ed9c 504 void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
ClementBreteau 18:cc5fec34ed9c 505 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
ClementBreteau 18:cc5fec34ed9c 506 {
ClementBreteau 18:cc5fec34ed9c 507 char TabBrasGauchePR[10];
ClementBreteau 18:cc5fec34ed9c 508 unsigned short GPosition1_1, GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 509
ClementBreteau 18:cc5fec34ed9c 510 GPosition1_1=((unsigned long)GPosition1*341/1000);
ClementBreteau 18:cc5fec34ed9c 511 GPosition2_1=((unsigned long)GPosition2*341/1000);
ClementBreteau 18:cc5fec34ed9c 512
ClementBreteau 18:cc5fec34ed9c 513 TabBrasGauchePR[0] = ID1;
ClementBreteau 18:cc5fec34ed9c 514 TabBrasGauchePR[1] = GPosition1_1;
ClementBreteau 18:cc5fec34ed9c 515 TabBrasGauchePR[2] = GPosition1_1>>8;
ClementBreteau 18:cc5fec34ed9c 516 TabBrasGauchePR[3] = GSpeed1;
ClementBreteau 18:cc5fec34ed9c 517 TabBrasGauchePR[4] = GSpeed1>>8;
ClementBreteau 18:cc5fec34ed9c 518
ClementBreteau 18:cc5fec34ed9c 519 TabBrasGauchePR[5] = ID2;
ClementBreteau 18:cc5fec34ed9c 520 TabBrasGauchePR[6] = GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 521 TabBrasGauchePR[7] = GPosition2_1>>8;
ClementBreteau 18:cc5fec34ed9c 522 TabBrasGauchePR[8] = GSpeed2;
ClementBreteau 18:cc5fec34ed9c 523 TabBrasGauchePR[9] = GSpeed2>>8;
ClementBreteau 18:cc5fec34ed9c 524
ClementBreteau 18:cc5fec34ed9c 525
ClementBreteau 18:cc5fec34ed9c 526 BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
ClementBreteau 18:cc5fec34ed9c 527 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 528
ClementBreteau 18:cc5fec34ed9c 529 while (((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)<GPosition1*95/100)) {
ClementBreteau 18:cc5fec34ed9c 530 BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
ClementBreteau 18:cc5fec34ed9c 531 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 532 }
ClementBreteau 18:cc5fec34ed9c 533
ClementBreteau 18:cc5fec34ed9c 534 while (((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)<GPosition2*95/100)) {
ClementBreteau 18:cc5fec34ed9c 535 BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
ClementBreteau 18:cc5fec34ed9c 536 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 537 }
ClementBreteau 18:cc5fec34ed9c 538 }
ClementBreteau 18:cc5fec34ed9c 539
ClementBreteau 18:cc5fec34ed9c 540 void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
ClementBreteau 18:cc5fec34ed9c 541 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
ClementBreteau 18:cc5fec34ed9c 542 {
ClementBreteau 18:cc5fec34ed9c 543 char TabBrasDroitPR[10];
ClementBreteau 18:cc5fec34ed9c 544 unsigned short GPosition1_1, GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 545
ClementBreteau 18:cc5fec34ed9c 546 GPosition1_1=((unsigned long)GPosition1*341/1000);
ClementBreteau 18:cc5fec34ed9c 547 GPosition2_1=((unsigned long)GPosition2*341/1000);
ClementBreteau 18:cc5fec34ed9c 548
ClementBreteau 18:cc5fec34ed9c 549 TabBrasDroitPR[0] = ID1;
ClementBreteau 18:cc5fec34ed9c 550 TabBrasDroitPR[1] = GPosition1_1;
ClementBreteau 18:cc5fec34ed9c 551 TabBrasDroitPR[2] = GPosition1_1>>8;
ClementBreteau 18:cc5fec34ed9c 552 TabBrasDroitPR[3] = GSpeed1;
ClementBreteau 18:cc5fec34ed9c 553 TabBrasDroitPR[4] = GSpeed1>>8;
ClementBreteau 18:cc5fec34ed9c 554
ClementBreteau 18:cc5fec34ed9c 555 TabBrasDroitPR[5] = ID2;
ClementBreteau 18:cc5fec34ed9c 556 TabBrasDroitPR[6] = GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 557 TabBrasDroitPR[7] = GPosition2_1>>8;
ClementBreteau 18:cc5fec34ed9c 558 TabBrasDroitPR[8] = GSpeed2;
ClementBreteau 18:cc5fec34ed9c 559 TabBrasDroitPR[9] = GSpeed2>>8;
ClementBreteau 18:cc5fec34ed9c 560
ClementBreteau 18:cc5fec34ed9c 561
ClementBreteau 18:cc5fec34ed9c 562 BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
ClementBreteau 18:cc5fec34ed9c 563 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 564
ClementBreteau 18:cc5fec34ed9c 565 while (((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)<GPosition1*95/100)) {
ClementBreteau 18:cc5fec34ed9c 566 BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
ClementBreteau 18:cc5fec34ed9c 567 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 568 }
ClementBreteau 18:cc5fec34ed9c 569
ClementBreteau 18:cc5fec34ed9c 570 while (((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)<GPosition2*95/100)) {
ClementBreteau 18:cc5fec34ed9c 571 BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
ClementBreteau 18:cc5fec34ed9c 572 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 573 }
ClementBreteau 18:cc5fec34ed9c 574 }
ClementBreteau 18:cc5fec34ed9c 575