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@21:7c60d6dfcab5, 2017-05-25 (annotated)

Committer:
ClementBreteau
Date:
Thu May 25 06:36:48 2017 +0000
Revision:
21:7c60d6dfcab5
Parent:
19:b4b91258c275 
homologation gros robot avec test au niveau des ack

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 break;
ClementBreteau 18:cc5fec34ed9c 190
ClementBreteau 18:cc5fec34ed9c 191 case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
ClementBreteau 18:cc5fec34ed9c 192 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 193 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 194 GoalPosBase=170;
ClementBreteau 18:cc5fec34ed9c 195 GoalPosCoude=1000;
ClementBreteau 18:cc5fec34ed9c 196 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 197 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 198 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 199
ClementBreteau 18:cc5fec34ed9c 200 break;
ClementBreteau 18:cc5fec34ed9c 201
ClementBreteau 18:cc5fec34ed9c 202 case AX12_PINCE_CENTRALE_PRISE_MODULE :
ClementBreteau 18:cc5fec34ed9c 203 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 204 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 205 GoalPosBase=170;
ClementBreteau 18:cc5fec34ed9c 206 GoalPosCoude=1000;
ClementBreteau 18:cc5fec34ed9c 207 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 208 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 209 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 210
ClementBreteau 18:cc5fec34ed9c 211 break;
ClementBreteau 18:cc5fec34ed9c 212
ClementBreteau 18:cc5fec34ed9c 213 case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
ClementBreteau 18:cc5fec34ed9c 214 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 215 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 216 GoalPosBase=1300;
ClementBreteau 18:cc5fec34ed9c 217 GoalPosCoude=700;
ClementBreteau 18:cc5fec34ed9c 218 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 219 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 220 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 221
ClementBreteau 18:cc5fec34ed9c 222 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 223 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 224 GoalPosBase=1450;//1050;
ClementBreteau 18:cc5fec34ed9c 225 GoalPosCoude=700;//1528;
ClementBreteau 18:cc5fec34ed9c 226 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 227 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 228 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 229
ClementBreteau 18:cc5fec34ed9c 230 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 231 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 232 GoalPosBase=1450;//1050;
ClementBreteau 18:cc5fec34ed9c 233 GoalPosCoude=1250;//1528;
ClementBreteau 18:cc5fec34ed9c 234 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 235 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 236 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 237
ClementBreteau 18:cc5fec34ed9c 238 break;
ClementBreteau 18:cc5fec34ed9c 239
ClementBreteau 18:cc5fec34ed9c 240 case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
ClementBreteau 18:cc5fec34ed9c 241 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 242 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 243 GoalPosBase=1000;
ClementBreteau 18:cc5fec34ed9c 244 GoalPosCoude=443;
ClementBreteau 18:cc5fec34ed9c 245 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 246 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 247 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 248
ClementBreteau 18:cc5fec34ed9c 249 break;
ClementBreteau 18:cc5fec34ed9c 250
ClementBreteau 18:cc5fec34ed9c 251 case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
ClementBreteau 18:cc5fec34ed9c 252 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 253 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 254 GoalPosBase=639;
ClementBreteau 18:cc5fec34ed9c 255 GoalPosCoude=557;
ClementBreteau 18:cc5fec34ed9c 256 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 257 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 258 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 259
ClementBreteau 18:cc5fec34ed9c 260 wait(TIME*10);
ClementBreteau 18:cc5fec34ed9c 261 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 262 GoalPosBase=400;
ClementBreteau 18:cc5fec34ed9c 263 GoalPosCoude=400;
ClementBreteau 18:cc5fec34ed9c 264 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 265 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 266 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 267
ClementBreteau 18:cc5fec34ed9c 268
ClementBreteau 18:cc5fec34ed9c 269 break;
ClementBreteau 18:cc5fec34ed9c 270
ClementBreteau 18:cc5fec34ed9c 271 case AX12_PINCE_CENTRALE_DEPOSER :
ClementBreteau 18:cc5fec34ed9c 272 //DEPOSER
ClementBreteau 18:cc5fec34ed9c 273 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 274 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 275 GoalPosBase=440;
ClementBreteau 18:cc5fec34ed9c 276 GoalPosCoude=440;
ClementBreteau 18:cc5fec34ed9c 277 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 278 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 279 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 280
ClementBreteau 18:cc5fec34ed9c 281
ClementBreteau 18:cc5fec34ed9c 282 break;
ClementBreteau 18:cc5fec34ed9c 283
ClementBreteau 18:cc5fec34ed9c 284 case AX12_PINCE_CENTRALE_DEPOT_HAUT :
ClementBreteau 18:cc5fec34ed9c 285 wait(TIME*5);
ClementBreteau 18:cc5fec34ed9c 286 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 287 GoalPosBase=1050;
ClementBreteau 18:cc5fec34ed9c 288 GoalPosCoude=1528;
ClementBreteau 18:cc5fec34ed9c 289 GoalPosPinceG=1090;
ClementBreteau 18:cc5fec34ed9c 290 GoalPosPinceD=1930;
ClementBreteau 18:cc5fec34ed9c 291 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 292
ClementBreteau 18:cc5fec34ed9c 293 wait(TIME*10);
ClementBreteau 18:cc5fec34ed9c 294 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 295 GoalPosBase=1050;
ClementBreteau 18:cc5fec34ed9c 296 GoalPosCoude=1528;
ClementBreteau 18:cc5fec34ed9c 297 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 298 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 299 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 300
ClementBreteau 18:cc5fec34ed9c 301 wait(TIME*10);
ClementBreteau 18:cc5fec34ed9c 302 GoalPosDoigt=90;
ClementBreteau 18:cc5fec34ed9c 303 GoalPosBase=1100;
ClementBreteau 18:cc5fec34ed9c 304 GoalPosCoude=700;
ClementBreteau 18:cc5fec34ed9c 305 GoalPosPinceG=500;
ClementBreteau 18:cc5fec34ed9c 306 GoalPosPinceD=2500;
ClementBreteau 18:cc5fec34ed9c 307 mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
ClementBreteau 18:cc5fec34ed9c 308
ClementBreteau 18:cc5fec34ed9c 309
ClementBreteau 18:cc5fec34ed9c 310 break;
ClementBreteau 18:cc5fec34ed9c 311
ClementBreteau 18:cc5fec34ed9c 312 case AX12_GAUCHE_CROC_OUVERT :
ClementBreteau 18:cc5fec34ed9c 313 wait_ms(TIME);
SquirrelGod 19:b4b91258c275 314 CrocBrasGauchePR->Set_Secure_Goal(110);
ClementBreteau 18:cc5fec34ed9c 315 break;
ClementBreteau 18:cc5fec34ed9c 316
ClementBreteau 18:cc5fec34ed9c 317 case AX12_GAUCHE_CROC_FERME :
ClementBreteau 18:cc5fec34ed9c 318 wait_ms(TIME);
SquirrelGod 19:b4b91258c275 319 CrocBrasGauchePR->Set_Secure_Goal(147);
ClementBreteau 18:cc5fec34ed9c 320 break;
ClementBreteau 18:cc5fec34ed9c 321
ClementBreteau 18:cc5fec34ed9c 322 case AX12_GAUCHE_CROC_INITIALE :
ClementBreteau 18:cc5fec34ed9c 323 wait_ms(TIME);
SquirrelGod 19:b4b91258c275 324 CrocBrasGauchePR->Set_Secure_Goal(232);
ClementBreteau 18:cc5fec34ed9c 325 break;
ClementBreteau 18:cc5fec34ed9c 326
ClementBreteau 18:cc5fec34ed9c 327 case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
ClementBreteau 18:cc5fec34ed9c 328 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 329 speed = 511;
ClementBreteau 18:cc5fec34ed9c 330 GoalPosCoudeTournanteG=1450;
ClementBreteau 18:cc5fec34ed9c 331 GoalPosEpauleTournanteG=600;
ClementBreteau 18:cc5fec34ed9c 332 mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
ClementBreteau 18:cc5fec34ed9c 333
ClementBreteau 18:cc5fec34ed9c 334
ClementBreteau 18:cc5fec34ed9c 335 break;
ClementBreteau 18:cc5fec34ed9c 336
ClementBreteau 18:cc5fec34ed9c 337 case AX12_TOURNANTE_GAUCHE_PREPARATION :
ClementBreteau 18:cc5fec34ed9c 338 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 339 speed = 511;
ClementBreteau 18:cc5fec34ed9c 340 GoalPosCoudeTournanteG=930;
ClementBreteau 18:cc5fec34ed9c 341 GoalPosEpauleTournanteG=1962;
ClementBreteau 18:cc5fec34ed9c 342 mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
ClementBreteau 18:cc5fec34ed9c 343
ClementBreteau 18:cc5fec34ed9c 344
ClementBreteau 18:cc5fec34ed9c 345 break;
ClementBreteau 18:cc5fec34ed9c 346
ClementBreteau 18:cc5fec34ed9c 347 case AX12_TOURNANTE_GAUCHE_MODULE :
ClementBreteau 18:cc5fec34ed9c 348 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 349 speed = 511;
ClementBreteau 18:cc5fec34ed9c 350 GoalPosCoudeTournanteG=894;
ClementBreteau 18:cc5fec34ed9c 351 GoalPosEpauleTournanteG=2200;
ClementBreteau 18:cc5fec34ed9c 352 mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
ClementBreteau 18:cc5fec34ed9c 353
ClementBreteau 18:cc5fec34ed9c 354
ClementBreteau 18:cc5fec34ed9c 355 break;
ClementBreteau 18:cc5fec34ed9c 356
ClementBreteau 18:cc5fec34ed9c 357 case AX12_DROIT_CROC_OUVERT :
ClementBreteau 18:cc5fec34ed9c 358 wait_ms(TIME);
SquirrelGod 19:b4b91258c275 359 CrocBrasDroitPR->Set_Secure_Goal(189);
ClementBreteau 18:cc5fec34ed9c 360
ClementBreteau 18:cc5fec34ed9c 361 break;
ClementBreteau 18:cc5fec34ed9c 362
ClementBreteau 18:cc5fec34ed9c 363 case AX12_DROIT_CROC_FERME :
ClementBreteau 18:cc5fec34ed9c 364 wait_ms(TIME);
SquirrelGod 19:b4b91258c275 365 CrocBrasDroitPR->Set_Secure_Goal(149);
ClementBreteau 18:cc5fec34ed9c 366
ClementBreteau 18:cc5fec34ed9c 367 break;
ClementBreteau 18:cc5fec34ed9c 368
ClementBreteau 18:cc5fec34ed9c 369 case AX12_DROIT_CROC_INITIALE :
ClementBreteau 18:cc5fec34ed9c 370 wait_ms(TIME);
SquirrelGod 19:b4b91258c275 371 CrocBrasDroitPR->Set_Secure_Goal(67);
ClementBreteau 18:cc5fec34ed9c 372 break;
ClementBreteau 18:cc5fec34ed9c 373
ClementBreteau 18:cc5fec34ed9c 374 case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
ClementBreteau 18:cc5fec34ed9c 375 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 376 speed = 511;
ClementBreteau 18:cc5fec34ed9c 377 GoalPosCoudeTournanteD=1610;
ClementBreteau 18:cc5fec34ed9c 378 GoalPosEpauleTournanteD=2337;
ClementBreteau 18:cc5fec34ed9c 379 mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
ClementBreteau 18:cc5fec34ed9c 380
ClementBreteau 18:cc5fec34ed9c 381
ClementBreteau 18:cc5fec34ed9c 382 break;
ClementBreteau 18:cc5fec34ed9c 383
ClementBreteau 18:cc5fec34ed9c 384 case AX12_TOURNANTE_DROIT_PREPARATION :
ClementBreteau 18:cc5fec34ed9c 385 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 386 speed = 511;
ClementBreteau 18:cc5fec34ed9c 387 GoalPosCoudeTournanteD=930;
ClementBreteau 18:cc5fec34ed9c 388 GoalPosEpauleTournanteD=1962;
ClementBreteau 18:cc5fec34ed9c 389 mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
ClementBreteau 18:cc5fec34ed9c 390
ClementBreteau 18:cc5fec34ed9c 391
ClementBreteau 18:cc5fec34ed9c 392 break;
ClementBreteau 18:cc5fec34ed9c 393
ClementBreteau 18:cc5fec34ed9c 394 case AX12_TOURNANTE_DROIT_MODULE :
ClementBreteau 18:cc5fec34ed9c 395 wait_ms(TIME);
ClementBreteau 18:cc5fec34ed9c 396 speed = 511;
ClementBreteau 18:cc5fec34ed9c 397 GoalPosCoudeTournanteD=894;
ClementBreteau 18:cc5fec34ed9c 398 GoalPosEpauleTournanteD=2200;
ClementBreteau 18:cc5fec34ed9c 399 mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
ClementBreteau 18:cc5fec34ed9c 400
ClementBreteau 18:cc5fec34ed9c 401
ClementBreteau 18:cc5fec34ed9c 402 break;
ClementBreteau 18:cc5fec34ed9c 403
ClementBreteau 18:cc5fec34ed9c 404 case AX12_DEFAUT :
ClementBreteau 18:cc5fec34ed9c 405 break;
ClementBreteau 18:cc5fec34ed9c 406
ClementBreteau 18:cc5fec34ed9c 407 case AX12_POSITION :
ClementBreteau 18:cc5fec34ed9c 408 GetPositionAx12();
ClementBreteau 18:cc5fec34ed9c 409 break;
ClementBreteau 18:cc5fec34ed9c 410 }
ClementBreteau 18:cc5fec34ed9c 411 }
ClementBreteau 18:cc5fec34ed9c 412
ClementBreteau 18:cc5fec34ed9c 413 void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
ClementBreteau 18:cc5fec34ed9c 414 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
ClementBreteau 18:cc5fec34ed9c 415 unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
ClementBreteau 18:cc5fec34ed9c 416 unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
ClementBreteau 18:cc5fec34ed9c 417 unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
ClementBreteau 18:cc5fec34ed9c 418 {
ClementBreteau 18:cc5fec34ed9c 419 char TabBrasCentralPR[25];
ClementBreteau 18:cc5fec34ed9c 420 unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
ClementBreteau 18:cc5fec34ed9c 421 Timer timeOut;
ClementBreteau 18:cc5fec34ed9c 422
ClementBreteau 18:cc5fec34ed9c 423
ClementBreteau 18:cc5fec34ed9c 424 GPosition1_1=((unsigned long)GPosition1*341/1000);
ClementBreteau 18:cc5fec34ed9c 425 GPosition2_1=((unsigned long)GPosition2*341/1000);
ClementBreteau 18:cc5fec34ed9c 426 GPosition3_1=((unsigned long)GPosition3*341/1000);
ClementBreteau 18:cc5fec34ed9c 427 GPosition4_1=((unsigned long)GPosition4*341/1000);
ClementBreteau 18:cc5fec34ed9c 428 GPosition5_1=((unsigned long)GPosition5*341/1000);
ClementBreteau 18:cc5fec34ed9c 429
ClementBreteau 18:cc5fec34ed9c 430 TabBrasCentralPR[0] = ID1;
ClementBreteau 18:cc5fec34ed9c 431 TabBrasCentralPR[1] = GPosition1_1;
ClementBreteau 18:cc5fec34ed9c 432 TabBrasCentralPR[2] = GPosition1_1>>8;
ClementBreteau 18:cc5fec34ed9c 433 TabBrasCentralPR[3] = GSpeed1;
ClementBreteau 18:cc5fec34ed9c 434 TabBrasCentralPR[4] = GSpeed1>>8;
ClementBreteau 18:cc5fec34ed9c 435
ClementBreteau 18:cc5fec34ed9c 436 TabBrasCentralPR[5] = ID2;
ClementBreteau 18:cc5fec34ed9c 437 TabBrasCentralPR[6] = GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 438 TabBrasCentralPR[7] = GPosition2_1>>8;
ClementBreteau 18:cc5fec34ed9c 439 TabBrasCentralPR[8] = GSpeed2;
ClementBreteau 18:cc5fec34ed9c 440 TabBrasCentralPR[9] = GSpeed2>>8;
ClementBreteau 18:cc5fec34ed9c 441
ClementBreteau 18:cc5fec34ed9c 442 TabBrasCentralPR[10] = ID3;
ClementBreteau 18:cc5fec34ed9c 443 TabBrasCentralPR[11] = GPosition3_1;
ClementBreteau 18:cc5fec34ed9c 444 TabBrasCentralPR[12] = GPosition3_1>>8;
ClementBreteau 18:cc5fec34ed9c 445 TabBrasCentralPR[13] = GSpeed3;
ClementBreteau 18:cc5fec34ed9c 446 TabBrasCentralPR[14] = GSpeed3>>8 ;
ClementBreteau 18:cc5fec34ed9c 447
ClementBreteau 18:cc5fec34ed9c 448 TabBrasCentralPR[15] = ID4;
ClementBreteau 18:cc5fec34ed9c 449 TabBrasCentralPR[16] = GPosition4_1;
ClementBreteau 18:cc5fec34ed9c 450 TabBrasCentralPR[17] = GPosition4_1>>8;
ClementBreteau 18:cc5fec34ed9c 451 TabBrasCentralPR[18] = GSpeed4;
ClementBreteau 18:cc5fec34ed9c 452 TabBrasCentralPR[19] = GSpeed4>>8 ;
ClementBreteau 18:cc5fec34ed9c 453
ClementBreteau 18:cc5fec34ed9c 454 TabBrasCentralPR[20] = ID5;
ClementBreteau 18:cc5fec34ed9c 455 TabBrasCentralPR[21] = GPosition5_1;
ClementBreteau 18:cc5fec34ed9c 456 TabBrasCentralPR[22] = GPosition5_1>>8;
ClementBreteau 18:cc5fec34ed9c 457 TabBrasCentralPR[23] = GSpeed5;
ClementBreteau 18:cc5fec34ed9c 458 TabBrasCentralPR[24] = GSpeed5>>8 ;
ClementBreteau 18:cc5fec34ed9c 459
ClementBreteau 18:cc5fec34ed9c 460
ClementBreteau 18:cc5fec34ed9c 461 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 462 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 463
SquirrelGod 19:b4b91258c275 464 timeOut.start();
SquirrelGod 19:b4b91258c275 465 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 466 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 467 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 468
ClementBreteau 18:cc5fec34ed9c 469 }
ClementBreteau 18:cc5fec34ed9c 470
SquirrelGod 19:b4b91258c275 471 timeOut.reset();
SquirrelGod 19:b4b91258c275 472 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 473 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 474 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 475 }
ClementBreteau 18:cc5fec34ed9c 476
SquirrelGod 19:b4b91258c275 477 timeOut.reset();
SquirrelGod 19:b4b91258c275 478 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 479 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 480 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 481 }
ClementBreteau 18:cc5fec34ed9c 482
SquirrelGod 19:b4b91258c275 483 timeOut.reset();
SquirrelGod 19:b4b91258c275 484 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 485 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 486 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 487 }
ClementBreteau 18:cc5fec34ed9c 488
SquirrelGod 19:b4b91258c275 489 timeOut.reset();
SquirrelGod 19:b4b91258c275 490 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 491 BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
ClementBreteau 18:cc5fec34ed9c 492 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 493 }
ClementBreteau 18:cc5fec34ed9c 494
ClementBreteau 18:cc5fec34ed9c 495
ClementBreteau 18:cc5fec34ed9c 496 }
ClementBreteau 18:cc5fec34ed9c 497
ClementBreteau 18:cc5fec34ed9c 498
ClementBreteau 18:cc5fec34ed9c 499 void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
ClementBreteau 18:cc5fec34ed9c 500 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
ClementBreteau 18:cc5fec34ed9c 501 {
ClementBreteau 18:cc5fec34ed9c 502 char TabBrasGauchePR[10];
ClementBreteau 18:cc5fec34ed9c 503 unsigned short GPosition1_1, GPosition2_1;
SquirrelGod 19:b4b91258c275 504 Timer timeOut;
ClementBreteau 18:cc5fec34ed9c 505
ClementBreteau 18:cc5fec34ed9c 506 GPosition1_1=((unsigned long)GPosition1*341/1000);
ClementBreteau 18:cc5fec34ed9c 507 GPosition2_1=((unsigned long)GPosition2*341/1000);
ClementBreteau 18:cc5fec34ed9c 508
ClementBreteau 18:cc5fec34ed9c 509 TabBrasGauchePR[0] = ID1;
ClementBreteau 18:cc5fec34ed9c 510 TabBrasGauchePR[1] = GPosition1_1;
ClementBreteau 18:cc5fec34ed9c 511 TabBrasGauchePR[2] = GPosition1_1>>8;
ClementBreteau 18:cc5fec34ed9c 512 TabBrasGauchePR[3] = GSpeed1;
ClementBreteau 18:cc5fec34ed9c 513 TabBrasGauchePR[4] = GSpeed1>>8;
ClementBreteau 18:cc5fec34ed9c 514
ClementBreteau 18:cc5fec34ed9c 515 TabBrasGauchePR[5] = ID2;
ClementBreteau 18:cc5fec34ed9c 516 TabBrasGauchePR[6] = GPosition2_1;
ClementBreteau 18:cc5fec34ed9c 517 TabBrasGauchePR[7] = GPosition2_1>>8;
ClementBreteau 18:cc5fec34ed9c 518 TabBrasGauchePR[8] = GSpeed2;
ClementBreteau 18:cc5fec34ed9c 519 TabBrasGauchePR[9] = GSpeed2>>8;
ClementBreteau 18:cc5fec34ed9c 520
ClementBreteau 18:cc5fec34ed9c 521
ClementBreteau 18:cc5fec34ed9c 522 BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
ClementBreteau 18:cc5fec34ed9c 523 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 524
SquirrelGod 19:b4b91258c275 525 timeOut.start();
SquirrelGod 19:b4b91258c275 526 while (((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)<GPosition1*90/100) || (timeOut.read_ms() < 100)) {
ClementBreteau 18:cc5fec34ed9c 527 BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
SquirrelGod 19:b4b91258c275 528 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 529 }
SquirrelGod 19:b4b91258c275 530 timeOut.reset();
ClementBreteau 18:cc5fec34ed9c 531
SquirrelGod 19:b4b91258c275 532 while (((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)<GPosition2*90/100) || (timeOut.read_ms() < 100)) {
ClementBreteau 18:cc5fec34ed9c 533 BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
SquirrelGod 19:b4b91258c275 534 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 535 }
SquirrelGod 19:b4b91258c275 536 timeOut.reset();
ClementBreteau 18:cc5fec34ed9c 537 }
ClementBreteau 18:cc5fec34ed9c 538
ClementBreteau 18:cc5fec34ed9c 539 void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
ClementBreteau 18:cc5fec34ed9c 540 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
ClementBreteau 18:cc5fec34ed9c 541 {
ClementBreteau 18:cc5fec34ed9c 542 char TabBrasDroitPR[10];
ClementBreteau 18:cc5fec34ed9c 543 unsigned short GPosition1_1, GPosition2_1;
SquirrelGod 19:b4b91258c275 544 Timer timeOut;
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
SquirrelGod 19:b4b91258c275 565 timeOut.start();
SquirrelGod 19:b4b91258c275 566 while (((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)<GPosition1*90/100) || (timeOut.read_ms() < 100)) {
ClementBreteau 18:cc5fec34ed9c 567 BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
SquirrelGod 19:b4b91258c275 568 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 569 }
SquirrelGod 19:b4b91258c275 570 timeOut.reset();
ClementBreteau 18:cc5fec34ed9c 571
SquirrelGod 19:b4b91258c275 572 while (((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)<GPosition2*90/100) || (timeOut.read_ms() < 100)) {
ClementBreteau 18:cc5fec34ed9c 573 BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
SquirrelGod 19:b4b91258c275 574 wait(TIME);
ClementBreteau 18:cc5fec34ed9c 575 }
SquirrelGod 19:b4b91258c275 576 timeOut.reset();
ClementBreteau 18:cc5fec34ed9c 577 }
ClementBreteau 18:cc5fec34ed9c 578