CRAC Team
/
AX12_PR
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
test_carteAToutFaire_PR
by 
CRAC Team
Revision 3:0175a071dd65, committed 2017-05-20
- Comitter:
- EdouardGE1
- Date:
- Sat May 20 09:36:35 2017 +0000
- Parent:
- 2:9d280856a536
- Child:
- 4:5cf096b1c65d
- Commit message:
- pour clement liste des fonctions des actions du petit robot
Changed in this revision
--- a/Actionneur.cpp Fri May 19 08:06:49 2017 +0000
+++ b/Actionneur.cpp Sat May 20 09:36:35 2017 +0000
@@ -1,4 +1,4 @@
-#include "Actionneur.h"
+#include "all_includes.h"
extern CAN can;
@@ -9,17 +9,13 @@
extern DigitalOut led2;
extern Serial pc;
+extern Timer t;
extern void GetPositionAx12(void);
-extern void gerer_turbine(unsigned char pwm_turbine);
-extern PwmOut PompeDroite;
-extern unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
- unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
- unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3);
-static float TAB_ANGLE1[4], TAB_ANGLE2[4];
-static char TAB_POSITION[4];
-AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
-AX12 *MilieuBrasPompe, *HautBrasPompe, *BasBrasPompe, *BrasPompeAx12, *TabPompeBras;
+AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
+ *CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
+ *CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR;
+
@@ -56,30 +52,70 @@
{
short vitesse=700;
- HautBrasPompe = new AX12(p9, p10, 3, 1000000);
- MilieuBrasPompe = new AX12(p9, p10, 2, 1000000);
- BasBrasPompe = new AX12(p9, p10, 1, 1000000);
+ BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);
+ CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000);
+ PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
+ PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
+ DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
- BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
+ CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
+ CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
+ EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
+
+ CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
+ CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
+ EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
+
+ BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
+ BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
+ BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
- HautBrasPompe->Set_Goal_speed(vitesse);
- MilieuBrasPompe->Set_Goal_speed(vitesse);
- BasBrasPompe->Set_Goal_speed(vitesse);
+ BaseBrasCentralPR->Set_Goal_speed(vitesse);
+ CoudeBrasCentralPR->Set_Goal_speed(vitesse);
+ PinceDBrasCentralPR->Set_Goal_speed(vitesse);
+ PinceGBrasCentralPR->Set_Goal_speed(vitesse);
+ DoigtBrasCentralPR->Set_Goal_speed(vitesse);
+
+ CrocBrasGauchePR->Set_Goal_speed(vitesse);
+ CoudeBrasGauchePR->Set_Goal_speed(vitesse);
+ EpauleBrasGauchePR->Set_Goal_speed(vitesse);
+
+ CrocBrasDroitPR->Set_Goal_speed(vitesse);
+ CoudeBrasDroitPR->Set_Goal_speed(vitesse);
+ EpauleBrasDroitPR->Set_Goal_speed(vitesse);
- HautBrasPompe->Set_Mode(0);
- MilieuBrasPompe->Set_Mode(0);
- BasBrasPompe->Set_Mode(0);
-
+ BaseBrasCentralPR->Set_Mode(0);
+ CoudeBrasCentralPR->Set_Mode(0);
+ PinceDBrasCentralPR->Set_Mode(0);
+ PinceGBrasCentralPR->Set_Mode(0);
+ DoigtBrasCentralPR->Set_Mode(0);
+
+ CrocBrasGauchePR->Set_Mode(0);
+ CoudeBrasGauchePR->Set_Mode(0);
+ EpauleBrasGauchePR->Set_Mode(0);
+
+ CrocBrasDroitPR->Set_Mode(0);
+ CoudeBrasDroitPR->Set_Mode(0);
+ EpauleBrasDroitPR->Set_Mode(0);
}
void GetPositionAx12(void) {
-
- pc.printf("\n\r * Bras Pompe * \n\r");
+
+ pc.printf("\n\r ");
+
+ pc.printf("BaseC : %lf \n\r ", BaseBrasCentralPR->Get_Position() );
+ pc.printf("CoudeC : %lf \n\r ", CoudeBrasCentralPR->Get_Position() );
+ pc.printf("PinceCD : %lf \n\r ", PinceDBrasCentralPR->Get_Position());
+ pc.printf("PinceCG : %lf \n\r ", PinceGBrasCentralPR->Get_Position());
+ pc.printf("DoigtC : %lf \n\r ", DoigtBrasCentralPR->Get_Position() );
- pc.printf("Haut : %lf \n\r ", HautBrasPompe->Get_Position() );
- pc.printf("Milieu : %lf \n\r ", MilieuBrasPompe->Get_Position() );
- pc.printf("Bas: %lf \n\r ", BasBrasPompe->Get_Position() );
+ pc.printf("EpauleG : %lf \n\r ", EpauleBrasGauchePR->Get_Position());
+ pc.printf("CoudeG : %lf \n\r ", CoudeBrasGauchePR->Get_Position() );
+ pc.printf("CrocG : %lf \n\r ", CrocBrasGauchePR->Get_Position() );
+ pc.printf("EpauleD : %lf \n\r ", EpauleBrasDroitPR->Get_Position());
+ pc.printf("CoudeD : %lf \n\r ", CoudeBrasDroitPR->Get_Position() );
+ pc.printf("CrocD : %lf \n\r ", CrocBrasDroitPR->Get_Position() );
}
@@ -87,391 +123,390 @@
/* FUNCTION NAME: Automate_ax12 */
/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
/****************************************************************************************/
-void AX12_automate(unsigned char etat_ax12, unsigned char choix_bras){
+void AX12_automate(unsigned char etat_ax12){
unsigned short speed;
-
- static unsigned char action = 0;
- unsigned int GoalPos1, GoalPos2, GoalPos3;
+ unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
+ GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
+ GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
+
+ speed = 1000;
+
switch(etat_ax12){
+
+ case AX12_PINCE_CENTRALE_POSITION_INITIALE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosDoigt=1150;
+ GoalPosBase=1490;
+ GoalPosCoude=1470;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+ Fin_action();
+ break;
+
+ case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=170;
+ GoalPosCoude=1000;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+ Fin_action();
+ break;
+
+ case AX12_PINCE_CENTRALE_PRISE_MODULE :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=170;
+ GoalPosCoude=1000;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+ Fin_action();
+ break;
+
+ case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=700;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*5);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=1200;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*5);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=1200;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+ Fin_action();
+ break;
- case AX12_GET_POSITION:
- GetPositionAx12();
+ case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=1200;
+ GoalPosCoude=443;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+ Fin_action();
break;
-
- case AX12_VIDER_FUSEE_POMPE:
-
- speed=511;
-
- GoalPos1=1500;
- GoalPos2=624;
- GoalPos3=1413;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos1+=900;
- GoalPos2=202;
- GoalPos3=2205;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(20*TIME);
- GoalPos3=2557;
- GoalPos2=173;
- GoalPos1=1947;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=2557;
- GoalPos2=370;
- GoalPos1=1798;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=2548;
- GoalPos2=683;
- GoalPos1=1499;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=2405;
- GoalPos2=1150;
- GoalPos1=1158;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
-
- PompeDroite.write(0);
+ case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=639;
+ GoalPosCoude=557;
+ GoalPosPinceG=400;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
- Fin_action();
- break;
-
- case AX12_VIDER_FUSEE_POMPEBLEU:
-
- speed=511;
-
- GoalPos3=2205;
- GoalPos2=202;
- GoalPos1=598;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=2557;
- GoalPos2=173;
- GoalPos1=1050;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(5*TIME);
- GoalPos3=2557;
- GoalPos2=370;
- GoalPos1=1199;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=2548;
- GoalPos2=683;
- GoalPos1=1499;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=2405;
- GoalPos2=1150;
- GoalPos1=1947;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
-
- PompeDroite.write(0);
-
- Fin_action();
+ wait(TIME*10);
+ GoalPosDoigt=90;
+ GoalPosBase=400;
+ GoalPosCoude=400;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ Fin_action();
break;
- case AX12_REMPLIR_BASE_POMPE:
-
- speed=511;
+ case AX12_PINCE_CENTRALE_DEPOSER :
+ //DEPOSER
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=440;
+ GoalPosCoude=440;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ Fin_action();
+ break;
+
+ case AX12_PINCE_CENTRALE_DEPOT_HAUT :
+ wait(TIME*5);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=1200;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*10);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=1200;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
- GoalPos3=2440;
- GoalPos2=302;
- GoalPos1=1950;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=1865;
- GoalPos2=252;
- GoalPos1=2400;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=1500;
- GoalPos2=460;
- GoalPos1=2400;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
- GoalPos3=1396;
- GoalPos2=600;
- GoalPos1=1399;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=700;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ Fin_action();
+ break;
+
+ case AX12_GAUCHE_CROC_OUVERT :
+ wait_ms(TIME);
+ CrocBrasGauchePR->Set_Secure_Goal(200);
+ Fin_action();
+ break;
- GoalPos3=1413;
- GoalPos2=624;
- GoalPos1=1500;
- mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
+ case AX12_GAUCHE_CROC_FERME :
+ wait_ms(TIME);
+ CrocBrasGauchePR->Set_Secure_Goal(240);
+ Fin_action();
+ break;
- Fin_action();
- break;
-
- case AX12_INITIALISATION :
- if (FlagAx12 == 1){
- Initialisation_position(1);
- //Check_positionAX12(&TAB1[25], 1);
- Initialisation_position(2);
- //Check_positionAX12(&TAB21[25], 2);
- FlagAx12 = 2;
- }
- else if (choix_bras > 1){
- Initialisation_position(choix_bras);
- }
+ case AX12_GAUCHE_CROC_INITIALE :
+ wait_ms(TIME);
+ CrocBrasGauchePR->Set_Secure_Goal(300);
+ Fin_action();
+ break;
+
+ case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteG=1450;
+ GoalPosEpauleTournanteG=600;
+ mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
+
+ Fin_action();
break;
-
- case AX12_PREPARATION_PRISE :
- Preparation_prise(choix_bras);
- if (action == 0){
- Fin_action();
- action ++;
- }
+
+ case AX12_TOURNANTE_GAUCHE_PREPARATION :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteG=930;
+ GoalPosEpauleTournanteG=1962;
+ mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
+
+ Fin_action();
break;
-
- case AX12_STOCKAGE_HAUT :
- Stockage_haut(choix_bras);
- etat_ax12 = AX12_DEFAUT;
- Fin_action();
+
+ case AX12_TOURNANTE_GAUCHE_MODULE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteG=894;
+ GoalPosEpauleTournanteG=2200;
+ mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
+
+ Fin_action();
break;
-
- case AX12_STOCKAGE_BAS :
- Stockage_bas(choix_bras);
- etat_ax12 = AX12_DEFAUT;
- Fin_action();
+
+ case AX12_DROIT_CROC_OUVERT :
+ wait_ms(TIME);
+ CrocBrasDroitPR->Set_Secure_Goal(106);
+ Fin_action();
break;
-
- case AX12_DEPOSER :
- Deposer(choix_bras);
- etat_ax12 = AX12_DEFAUT;
- Fin_action();
- break;
-
- case AX12_PREPARATION_DEPOT_BAS :
- Preparation_depot_bas(choix_bras);
- etat_ax12 = AX12_DEFAUT;
- Fin_action();
+
+ case AX12_DROIT_CROC_FERME :
+ wait_ms(TIME);
+ CrocBrasDroitPR->Set_Secure_Goal(55);
+ Fin_action();
+ break;
+
+ case AX12_DROIT_CROC_INITIALE :
+ wait_ms(TIME);
+ CrocBrasDroitPR->Set_Secure_Goal(0);
+ Fin_action();
+ break;
+
+ case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteD=1450;
+ GoalPosEpauleTournanteD=600;
+ mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
+
+ Fin_action();
break;
-
- case AX12_PREPARATION_DEPOT_HAUT :
- Preparation_depot_haut(choix_bras);
- etat_ax12 = AX12_DEFAUT;
- Fin_action();
+
+ case AX12_TOURNANTE_DROIT_PREPARATION :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteD=930;
+ GoalPosEpauleTournanteD=1962;
+ mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
+
+ Fin_action();
break;
-
- case AX12_POUSSER_MODULE :
- Pousser_module(choix_bras);
- etat_ax12 = AX12_DEFAUT;
- Fin_action();
+
+ case AX12_TOURNANTE_DROIT_MODULE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteD=894;
+ GoalPosEpauleTournanteD=2200;
+ mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
+
+ Fin_action();
break;
case AX12_DEFAUT :
- action = 0;
+ break;
+
+ case AX12_POSITION :
+ GetPositionAx12();
break;
}
}
-
-unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
- unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
- unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3)
+void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
+ unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
+ unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
+ unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
{
- char TabPompeBras[15];
- unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
+ char TabBrasCentralPR[25];
+ unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
+
+ GPosition1_1=((unsigned long)GPosition1*341/1000);
+ GPosition2_1=((unsigned long)GPosition2*341/1000);
+ GPosition3_1=((unsigned long)GPosition3*341/1000);
+ GPosition4_1=((unsigned long)GPosition4*341/1000);
+ GPosition5_1=((unsigned long)GPosition5*341/1000);
- GPosition1_1=((unsigned long)GPosition1*341/1000);
- GPosition2_1=((unsigned long)GPosition2*341/1000);
- GPosition3_1=((unsigned long)GPosition3*341/1000);
-
- TabPompeBras[0] = ID1;
- TabPompeBras[1] = GPosition1_1;
- TabPompeBras[2] = GPosition1_1>>8;
- TabPompeBras[3] = GSpeed1;
- TabPompeBras[4] = GSpeed1>>8; /// (haut, milieu, bas)
+ TabBrasCentralPR[0] = ID1;
+ TabBrasCentralPR[1] = GPosition1_1;
+ TabBrasCentralPR[2] = GPosition1_1>>8;
+ TabBrasCentralPR[3] = GSpeed1;
+ TabBrasCentralPR[4] = GSpeed1>>8;
- TabPompeBras[5] = ID2;
- TabPompeBras[6] = GPosition2_1;
- TabPompeBras[7] = GPosition2_1>>8;
- TabPompeBras[8] = GSpeed2;
- TabPompeBras[9] = GSpeed2>>8;
+ TabBrasCentralPR[5] = ID2;
+ TabBrasCentralPR[6] = GPosition2_1;
+ TabBrasCentralPR[7] = GPosition2_1>>8;
+ TabBrasCentralPR[8] = GSpeed2;
+ TabBrasCentralPR[9] = GSpeed2>>8;
- TabPompeBras[10] = ID3;
- TabPompeBras[11] = GPosition3_1;
- TabPompeBras[12] = GPosition3_1>>8;
- TabPompeBras[13] = GSpeed3;
- TabPompeBras[14] = GSpeed3>>8 ;
+ TabBrasCentralPR[10] = ID3;
+ TabBrasCentralPR[11] = GPosition3_1;
+ TabBrasCentralPR[12] = GPosition3_1>>8;
+ TabBrasCentralPR[13] = GSpeed3;
+ TabBrasCentralPR[14] = GSpeed3>>8 ;
+
+ TabBrasCentralPR[15] = ID4;
+ TabBrasCentralPR[16] = GPosition4_1;
+ TabBrasCentralPR[17] = GPosition4_1>>8;
+ TabBrasCentralPR[18] = GSpeed4;
+ TabBrasCentralPR[19] = GSpeed4>>8 ;
+
+ TabBrasCentralPR[20] = ID5;
+ TabBrasCentralPR[21] = GPosition5_1;
+ TabBrasCentralPR[22] = GPosition5_1>>8;
+ TabBrasCentralPR[23] = GSpeed5;
+ TabBrasCentralPR[24] = GSpeed5>>8 ;
- BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
- wait(TIME);
-
-
- while (((unsigned short)(HautBrasPompe->Get_Position()*10)>GPosition3*105/100) || ((unsigned short)(HautBrasPompe->Get_Position()*10)<GPosition3*95/100)) {
- BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
- wait(TIME*5);
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+
+ /*
+ while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*108/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*92/100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ pc.printf("1 \n\r ");
+ wait(5*TIME);
+ }
+
+ while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*108/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*92/100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ pc.printf("2 \n\r ");
+ wait(5*TIME);
+ }
+
+ while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*108/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*92/100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ pc.printf("3 \n\r ");
+ wait(5*TIME);
+ }
+
+ while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*108/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*92/100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ pc.printf("4 \n\r ");
+ wait(5*TIME);
}
- while (((unsigned short)(MilieuBrasPompe->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(MilieuBrasPompe->Get_Position()*10)<GPosition2*95/100)) {
- BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
- wait(TIME*5);
- }
-
-
- while (((unsigned short)(BasBrasPompe->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(BasBrasPompe->Get_Position()*10)<GPosition1*95/100)) {
- BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
- wait(TIME*5);
+ while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*108/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*92/100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ pc.printf("5 \n\r ");
+ wait(5*TIME);
}
-
-
-
-}
-
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_position */
-/* DESCRIPTION : Fonction qui place les bras en position verticale */
-/****************************************************************************************/
-void Initialisation_position(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB1);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
- wait(TIME);
- }
- else if (choix == 3){
- BrasPompeAx12->multiple_goal_and_speed(3,TabBrasPompePosInit);
- wait(TIME);
- }
-
+ */
}
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise */
-/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
-/****************************************************************************************/
-void Preparation_prise(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB2);
- wait(TIME);
- }
+
+void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
+{
+ char TabBrasGauchePR[10];
+ unsigned short GPosition1_1, GPosition2_1;
+
+ GPosition1_1=((unsigned long)GPosition1*341/1000);
+ GPosition2_1=((unsigned long)GPosition2*341/1000);
+
+ TabBrasGauchePR[0] = ID1;
+ TabBrasGauchePR[1] = GPosition1_1;
+ TabBrasGauchePR[2] = GPosition1_1>>8;
+ TabBrasGauchePR[3] = GSpeed1;
+ TabBrasGauchePR[4] = GSpeed1>>8;
+
+ TabBrasGauchePR[5] = ID2;
+ TabBrasGauchePR[6] = GPosition2_1;
+ TabBrasGauchePR[7] = GPosition2_1>>8;
+ TabBrasGauchePR[8] = GSpeed2;
+ TabBrasGauchePR[9] = GSpeed2>>8;
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
- wait(TIME);
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut */
-/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
-/****************************************************************************************/
-void Stockage_haut(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB3);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB4);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB5);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB6);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
- wait(TIME);
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas */
-/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
-/****************************************************************************************/
-void Stockage_bas(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB3);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB7);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
- wait(TIME);
- }
+
+ BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
+ wait(TIME);
+
}
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer */
-/* DESCRIPTION : Fonction qui permet de déposer le module */
-/****************************************************************************************/
-void Deposer(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB9);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
- wait(TIME);
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_bas */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
-/****************************************************************************************/
-void Preparation_depot_bas(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB8);
- wait(TIME);
- }
+void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
+{
+ char TabBrasDroitPR[10];
+ unsigned short GPosition1_1, GPosition2_1;
+
+ GPosition1_1=((unsigned long)GPosition1*341/1000);
+ GPosition2_1=((unsigned long)GPosition2*341/1000);
+
+ TabBrasDroitPR[0] = ID1;
+ TabBrasDroitPR[1] = GPosition1_1;
+ TabBrasDroitPR[2] = GPosition1_1>>8;
+ TabBrasDroitPR[3] = GSpeed1;
+ TabBrasDroitPR[4] = GSpeed1>>8;
+
+ TabBrasDroitPR[5] = ID2;
+ TabBrasDroitPR[6] = GPosition2_1;
+ TabBrasDroitPR[7] = GPosition2_1>>8;
+ TabBrasDroitPR[8] = GSpeed2;
+ TabBrasDroitPR[9] = GSpeed2>>8;
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
- wait(TIME);
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_haut */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
-/****************************************************************************************/
-void Preparation_depot_haut(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB6);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB5);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB10);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB8);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
- wait(TIME);
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module */
-/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
-/****************************************************************************************/
-void Pousser_module(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB11);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
- wait(TIME);
- }
+
+ BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
+ wait(TIME);
+
}
/****************************************************************************************/
@@ -485,63 +520,7 @@
msgTx.id = SERVO_AX12_END;
msgTx.len = 1;
- msgTx.data[0] = AX12_PREPARATION_PRISE;
+ msgTx.data[0] = 0;
can.write(msgTx);
}
-
-
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: Check_positionAX12 */
-/* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */
-/****************************************************************************************/
-void Check_positionAX12(char* TAB, unsigned char choix){
- int k=1, i=0;
- static float TAB_POS_TH[4];
-
- CANMessage msgTx=CANMessage();
- msgTx.id=SERVO_AX12_POSITION;
- msgTx.len=5;
-
- //PERMET DE VERIFIER LA POSITION D'UN AX12
- TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3);
- TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3);
- TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3);
- TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3);
- TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3);
- TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3);
- TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3);
- TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3);
-
- for(i=0; i<4; i++){
- TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8);
- k=k+5;
- }
-
- for(i=0; i<4; i++){
- if (choix == 1){
- if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
- TAB_POSITION[i] = 1;
- }
- else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
- TAB_POSITION[i] = 0;
- }
- }
- else if (choix == 2){
- if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
- TAB_POSITION[i] = 1;
- }
- else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
- TAB_POSITION[i] = 0;
- }
- }
- }
-
- msgTx.data[0] = choix;
- for(i=1; i<5; i++){
- msgTx.data[i] = TAB_POSITION[i];
- }
- can.write(msgTx);
-}
-
\ No newline at end of file
+
\ No newline at end of file
--- a/Actionneur.h Fri May 19 08:06:49 2017 +0000
+++ b/Actionneur.h Sat May 20 09:36:35 2017 +0000
@@ -1,20 +1,41 @@
-#include "all_includes.h"
-
-
-
-#define AX12_INITIALISATION 0
-#define AX12_PREPARATION_PRISE 1
-#define AX12_STOCKAGE_HAUT 2
-#define AX12_STOCKAGE_BAS 3
-#define AX12_DEPOSER 4
-#define AX12_PREPARATION_DEPOT_BAS 5
-#define AX12_PREPARATION_DEPOT_HAUT 6
-#define AX12_POUSSER_MODULE 7
-#define AX12_VIDER_FUSEE_POMPE 8
-#define AX12_REMPLIR_BASE_POMPE 9
-#define AX12_GET_POSITION 10
-#define AX12_VIDER_FUSEE_POMPEBLEU 11
-#define AX12_DEFAUT 20
+#define AX12_PINCE_CENTRALE_POSITION_INITIALE 1
+#define AX12_PINCE_CENTRALE_PREPARATION_PRISE 2
+#define AX12_PINCE_CENTRALE_PRISE_MODULE 3
+#define AX12_PINCE_CENTRALE_STOCKAGE_HAUT 4
+#define AX12_PINCE_CENTRALE_STOCKAGE_BAS 5
+#define AX12_PINCE_CENTRALE_PREPARATION_DEPOT 6
+#define AX12_PINCE_CENTRALE_DEPOSER 7
+#define AX12_PINCE_CENTRALE_DEPOT_HAUT 8
+
+#define AX12_GAUCHE_CROC_OUVERT 11
+#define AX12_GAUCHE_CROC_FERME 12
+#define AX12_DROIT_CROC_INITIALE 13
+
+
+#define AX12_DROIT_CROC_OUVERT 14
+#define AX12_DROIT_CROC_FERME 15
+#define AX12_GAUCHE_CROC_INITIALE 16
+
+#define AX12_TOURNANTE_GAUCHE_POSITION_INITIALE 21
+#define AX12_TOURNANTE_GAUCHE_PREPARATION 22
+#define AX12_TOURNANTE_GAUCHE_MODULE 23
+
+#define AX12_TOURNANTE_DROIT_POSITION_INITIALE 24
+#define AX12_TOURNANTE_DROIT_PREPARATION 25
+#define AX12_TOURNANTE_DROIT_MODULE 26
+
+#define AX12_POSITION 100
+#define AX12_DEFAUT 0
+
+#define AX12_DOIGT 4
+#define AX12_BASE 5
+#define AX12_COUDE 6
+#define AX12_PINCEG 7
+#define AX12_PINCED 8
+#define AX12_GAUCHE_EPAULE 1
+#define AX12_GAUCHE_COUDE 2
+#define AX12_DROIT_EPAULE 9
+#define AX12_DROIT_COUDE 10
#define TOLERANCE_AX12 50
@@ -35,291 +56,14 @@
/****************************************************************************************/
void Initialisation_position(unsigned char choix);
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise */
-/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
-/****************************************************************************************/
-void Preparation_prise(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut */
-/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
-/****************************************************************************************/
-void Stockage_haut(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas */
-/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
-/****************************************************************************************/
-void Stockage_bas(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer */
-/* DESCRIPTION : Fonction qui permet de déposer un module */
-/****************************************************************************************/
-void Deposer(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_bas */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
-/****************************************************************************************/
-void Preparation_depot_bas(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_haut */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
-/****************************************************************************************/
-void Preparation_depot_haut(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module */
-/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
-/****************************************************************************************/
-void Pousser_module(unsigned char choix);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Check_positionAX12 */
-/* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */
-/****************************************************************************************/
-void Check_positionAX12(char* TAB, unsigned char choix);
-
-
-///////////TABLEAU PINCE DROITE /////////////////////////// --> choix_bras == 1
-static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
- 0x04,0xF4, 0x01, 0xFF, 0x00,
- 0x07,0x00, 0x01, 0xFF, 0x00,
- 0x0F,0x00, 0x03, 0xFF, 0x00};
-
-static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
- 0x04,0xE9, 0x00, 0xFF, 0x03,
- 0x07,0xF4, 0x01, 0xFF, 0x03,
- 0x0F,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
- 0x04,0xE9, 0x00, 0xFF, 0x03,
- 0x07,0xD5, 0x00, 0xFF, 0x00,
- 0x0F,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
- 0x04,0x00, 0x02, 0xFF, 0x03,
- 0x07,0xD5, 0x00, 0xFF, 0x00,
- 0x0F,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
- 0x04,0x60, 0x01, 0xFF, 0x00,
- 0x07,0xD5, 0x00, 0xFF, 0x00,
- 0x0F,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x04,0x50, 0x01, 0xFF, 0x03,
- 0x07,0x0A, 0x01, 0xFF, 0x03,
- 0x0F,0xDD, 0x02, 0xFF, 0x03};
-
-static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
- 0x04,0x50, 0x03, 0xFF, 0x03,
- 0x07,0xD5, 0x00, 0xFF, 0x03,
- 0x0F,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer (descendre bras)
- 0x04,0x00, 0x03, 0xFF, 0x00,
- 0x07,0xD5, 0x00, 0xFF, 0x00,
- 0x0F,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer (pince ouverte)
- 0x04,0x00, 0x03, 0xFF, 0x03,
- 0x07,0x0A, 0x01, 0xFF, 0x03,
- 0x0F,0xDD, 0x02, 0xFF, 0x03};
-
-static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer (preparation module haut)
- 0x04,0x00, 0x02, 0xFF, 0x00,
- 0x07,0xD5, 0x00, 0xFF, 0x00,
- 0x0F,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
- 0x04,0x00, 0x03, 0xFF, 0x00,
- 0x07,0x0A, 0x01, 0xFF, 0x03,
- 0x0F,0xDD, 0x02, 0xFF, 0x03};
-
-
-//////////////////TABLEAU PINCE GAUCHE ///////////////////////////// --> choix_bras == 2
-static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
- 0x03,0xF4, 0x01, 0xFF, 0x00,
- 0x0D,0x00, 0x01, 0xFF, 0x00,
- 0x0E,0x00, 0x03, 0xFF, 0x00};
-
-static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
- 0x03,0xE9, 0x00, 0xFF, 0x03,
- 0x0D,0xF4, 0x01, 0xFF, 0x03,
- 0x0E,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
- 0x03,0xE9, 0x00, 0xFF, 0x03,
- 0x0D,0xD5, 0x00, 0xFF, 0x00,
- 0x0E,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
- 0x03,0x00, 0x02, 0xFF, 0x03,
- 0x0D,0xD5, 0x00, 0xFF, 0x00,
- 0x0E,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
- 0x03,0x60, 0x01, 0xFF, 0x00,
- 0x0D,0xD5, 0x00, 0xFF, 0x00,
- 0x0E,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x03,0x50, 0x01, 0xFF, 0x03,
- 0x0D,0x0A, 0x01, 0xFF, 0x03,
- 0x0E,0xDD, 0x02, 0xFF, 0x03};
-
-static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
- 0x03,0x50, 0x03, 0xFF, 0x03,
- 0x0D,0xD5, 0x00, 0xFF, 0x03,
- 0x0E,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer(descendre bras)
- 0x03,0x00, 0x03, 0xFF, 0x00,
- 0x0D,0xD5, 0x00, 0xFF, 0x00,
- 0x0E,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer(pince ouverte)
- 0x03,0x00, 0x03, 0xFF, 0x03,
- 0x0D,0xF4, 0x01, 0xFF, 0x03,
- 0x0E,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer(preparation module haut)
- 0x03,0x00, 0x02, 0xFF, 0x00,
- 0x0D,0xD5, 0x00, 0xFF, 0x00,
- 0x0E,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
- 0x03,0x00, 0x03, 0xFF, 0x00,
- 0x0D,0xD5, 0x00, 0xFF, 0x00,
- 0x0E,0x4D, 0x03, 0xFF, 0x00};
-
-
-char TabBrasPompePosInit[15]= {0x03,0xB0, 0x01, 0xFF, 0x00, /// (bras en bas)
- 0x02,0x00, 0x03, 0xFF, 0x00,
- 0x01,0xD5, 0x00, 0xFF, 0x00};
- /*
-char TabBrasPompeViderFuseeMvt1[15]= {0x03,0xFD, 0x01, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0xF3, 0x00, 0xFF, 0x00,
- 0x01,0x0B, 0x03, 0xFF, 0x00};
-
-
-
-char TabBrasPompeViderFuseeMvt2[15]= {0x03,0x18, 0x02, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0xA9, 0x00, 0xFF, 0x00,
- 0x01,0xC8, 0x01, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt3[15]= {0x03,0x3D, 0x03, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x45, 0x00, 0xFF, 0x00,
- 0x01,0x74, 0x01, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt4[15]= {0x03,0x7C, 0x03, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x71, 0x00, 0xFF, 0x00,
- 0x01,0xE5, 0x00, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt5[15]= {0x03,0x7C, 0x03, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x0D, 0x01, 0xFF, 0x00,
- 0x01,0x74, 0x00, 0xFF, 0x00};
-*/
-/*
-char TabBrasPompeViderFuseeMvt1[15]= {0x03,0xCE, 0x01, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0xDB, 0x00, 0xFF, 0x00,
- 0x01,0x2F, 0x03, 0xFF, 0x00};
-
-
-
-char TabBrasPompeViderFuseeMvt2[15]= {0x03,0x4D, 0x02, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x6D, 0x00, 0xFF, 0x00,
- 0x01,0x00, 0x02, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt3[15]= {0x03,0x4D, 0x03, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x4F, 0x00, 0xFF, 0x00,
- 0x01,0x32, 0x01, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt4[15]= {0x03,0x5B, 0x03, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0xF3, 0x00, 0xFF, 0x00,
- 0x01,0x5D, 0x00, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt5[15]= {0x03,0x39, 0x03, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x4C, 0x00, 0xFF, 0x00,
- 0x01,0x80, 0x00, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt6[15]= {0x03,0xBE, 0x02, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x3F, 0x00, 0xFF, 0x00,
- 0x01,0x80, 0x00, 0xFF, 0x00};
-
-char TabBrasPompeViderFuseeMvt7[15]= {0x03,0x18, 0x02, 0xFF, 0x00, /// (haut, milieu, bas)
- 0x02,0x9C, 0x00, 0xFF, 0x00,
- 0x01,0xF3, 0x01, 0xFF, 0x00};
-
- */
-
-char TabBrasPompeViderFuseeMvt1[15]= {0x03,0xE2, 0x01, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0xD5, 0x00, 0xFF, 0x03,
- 0x01,0xFF, 0x01, 0xFF, 0x03};
-
-
-
-char TabBrasPompeViderFuseeMvt2[15]= {0x03,0xF0, 0x02, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x45,0x00, 0xFF, 0x03,
- 0x01,0x32,0x03, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt3[15]= {0x03,0x68,0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x3B, 0x00, 0xFF, 0x03,
- 0x01,0x98, 0x02, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt33[15]= {0x03,0x68,0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x7E, 0x00, 0xFF, 0x03,
- 0x01,0x65, 0x02, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt4[15]= {0x03,0x65, 0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0xE9, 0x00, 0xFF, 0x03,
- 0x01,0xFF, 0x01, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt44[15]= {0x03,0x34, 0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x88, 0x01, 0xFF, 0x03,
- 0x01,0x8B, 0x01, 0xFF, 0x03};
-
-
-
-char TabBrasPompeViderFuseeMvt2_2[15]= {0x03,0xF0, 0x02, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x45,0x00, 0xFF, 0x03,
- 0x01,0xCC,0x00, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt3_2[15]= {0x03,0x68,0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x3B, 0x00, 0xFF, 0x03,
- 0x01,0x66, 0x01, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt33_2[15]= {0x03,0x68,0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x7E, 0x00, 0xFF, 0x03,
- 0x01,0x99, 0x01, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt4_2[15]= {0x03,0x65, 0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0xE9, 0x00, 0xFF, 0x03,
- 0x01,0xFF, 0x01, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt44_2[15]= {0x03,0x34, 0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x88, 0x01, 0xFF, 0x03,
- 0x01,0x98, 0x02, 0xFF, 0x03};
-
-
-
-char TabBrasPompeViderFuseeMvt5[15]= {0x03,0x40, 0x03, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x67, 0x00, 0xFF, 0x03,
- 0x01,0x32, 0x00, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt6[15]= {0x03,0x7C, 0x02, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x56, 0x00, 0xFF, 0x03,
- 0x01,0x32, 0x03, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt7[15]= {0x03,0x14, 0x02, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0x92, 0x00, 0xFF, 0x03,
- 0x01,0x32, 0x003, 0xFF, 0x03};
-
-char TabBrasPompeViderFuseeMvt8[15]= {0x03,0xDC, 0x01, 0xFF, 0x03, /// (haut, milieu, bas)
- 0x02,0xD5, 0x00, 0xFF, 0x03,
- 0x01,0xFF, 0x01, 0xFF, 0x03};
-
\ No newline at end of file
+void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
+ unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
+ unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
+ unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5);
+
+void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2);
+
+void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2);
\ No newline at end of file
--- a/CAN.cpp Fri May 19 08:06:49 2017 +0000
+++ b/CAN.cpp Sat May 20 09:36:35 2017 +0000
@@ -52,29 +52,7 @@
if(FIFO_occupation!=0) {
switch(msgRxBuffer[FIFO_lecture].id) {
-
- case SERVOVANNE:
- EtatServoVanne = msgRxBuffer[FIFO_lecture].data[0];
- break;
-
- case POMPE_DROITE:
- ActionPompe = 1;
- EtatPompeDroite = msgRxBuffer[FIFO_lecture].data[0];
- break;
-
- case POMPE_GAUCHE:
- ActionPompe = 1;
- EtatPompeGauche = msgRxBuffer[FIFO_lecture].data[0];
- break;
-
- case TURBINE:
- EtatTurbine = msgRxBuffer[FIFO_lecture].data[0];
- break;
-
- case LANCEUR:
- EtatLanceur = msgRxBuffer[FIFO_lecture].data[0];
- break;
-
+
case CHECK_AX12:
SendRawId(ALIVE_AX12);
FlagAx12 = 1;
@@ -84,7 +62,6 @@
ActionAx12=1;
EtatAx12 = msgRxBuffer[FIFO_lecture].data[0];
- ChoixBras = msgRxBuffer[FIFO_lecture].data[1];
//ACK de reception des actions a effectuer
msgTx.id = SERVO_AX12_ACK;
--- a/Turbine.cpp Fri May 19 08:06:49 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#include "mbed.h"
-
-extern PwmOut turbine;
-extern void gerer_turbine(unsigned short pwm_turbine);
-
-extern DigitalOut led2;
-
-void gerer_turbine(unsigned char pwm_turbine) {
-
- turbine.period_ms(20);
-
-
- if (pwm_turbine==0)
- turbine.pulsewidth_us(800);
- else if (pwm_turbine==1) {
- turbine.pulsewidth_us(1000+(10*pwm_turbine));
- }
-
-}
\ No newline at end of file
--- a/all_includes.h Fri May 19 08:06:49 2017 +0000 +++ b/all_includes.h Sat May 20 09:36:35 2017 +0000 @@ -3,14 +3,13 @@ #include "AX12.h" #include "ident_crac.h" #include "cmsis.h" - - +#include "Actionneur.h" #define SIZE_FIFO 20 #define TIME 0.1 -extern unsigned char EtatPompeDroite, EtatPompeGauche, EtatLanceur, EtatAx12, ChoixBras, ActionAx12, EtatTurbine, EtatServoVanne; -extern unsigned char action_a_effectuer, ActionPompe; +extern unsigned char EtatAx12, ActionAx12; +extern unsigned char action_a_effectuer; extern PwmOut PompeDroite; @@ -23,7 +22,6 @@ void canRx_ISR(void); void canProcessRx(void); void CAN2_wrFilter (uint32_t id); -void AX12_automate(unsigned char etat_ax12, unsigned char choix_bras); -void gerer_turbine(unsigned char pwm_turbine); +void AX12_automate(unsigned char etat_ax12);
--- a/main.cpp Fri May 19 08:06:49 2017 +0000
+++ b/main.cpp Sat May 20 09:36:35 2017 +0000
@@ -8,25 +8,11 @@
Serial pc(USBTX, USBRX);
-extern void gerer_turbine(unsigned char pwm_turbine);
-
-
-PwmOut PompeDroite(p21);
-//PwmOut PompeGauche(p22);
-PwmOut MotLanceur(p22);
-PwmOut turbine(p23);
-//PwmOut ServoVanne(p21);
-
-AnalogIn telemetre(p15);
-
-DigitalIn Jack(p25);
-
-
DigitalOut led(LED1);
DigitalOut led2(LED2);
-unsigned char EtatPompeDroite=0, EtatPompeGauche=0, EtatLanceur=0, EtatAx12=0, ChoixBras=0, ActionAx12=0, EtatTurbine=0, EtatServoVanne=0;
-unsigned char action_a_effectuer=0, ActionPompe=0;
+unsigned char EtatAx12, ActionAx12;
+unsigned char action_a_effectuer;
/*
@@ -54,31 +40,83 @@
can.frequency(1000000); // fréquence de travail 1Mbit/s
can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
-
// message CAN autorise a declencher l'interruption
-
- CAN2_wrFilter(POMPE_DROITE);
- CAN2_wrFilter(POMPE_GAUCHE);
-
- CAN2_wrFilter(LANCEUR);
- CAN2_wrFilter(TURBINE);
- CAN2_wrFilter(SERVOVANNE);
- CAN2_wrFilter(0x123);
-
CAN2_wrFilter(SERVO_AX12_ACTION);
CAN2_wrFilter(SERVO_AX12_ACK);
CAN2_wrFilter(SERVO_AX12_END);
CAN2_wrFilter(CHECK_AX12);
-
+
initialisation_AX12();
+
+ //PINCE CENTRALE
+ /*
+ AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_PRISE);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_PRISE_MODULE);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_HAUT);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_PRISE);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_PRISE_MODULE);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_BAS);
+ wait(2);
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_DEPOSER);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_DEPOT_HAUT);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
+ wait(1);
+ AX12_automate(AX12_PINCE_CENTRALE_DEPOSER);
+ wait(1);
+ */
- PompeDroite.period(0.001);
- //PompeGauche.period(0.001);
- MotLanceur.period(0.001);
- // ServoVanne.period(0.001);
+ //BRAS GAUCHE
+ /*
+ AX12_automate(AX12_GAUCHE_CROC_OUVERT);
+ wait(2);
+ AX12_automate(AX12_GAUCHE_CROC_FERME);
+ wait(3);
+ AX12_automate(AX12_GAUCHE_CROC_OUVERT);
+ wait(2);
+ AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
+ wait(2);
+ AX12_automate(AX12_TOURNANTE_GAUCHE_PREPARATION);
+ wait(2);
+ AX12_automate(AX12_TOURNANTE_GAUCHE_MODULE);
+ wait(2);
+ */
+ /*
+ //BRAS DROIT
+ AX12_automate(AX12_DROIT_CROC_OUVERT);
+ wait(2);
+ AX12_automate(AX12_DROIT_CROC_FERME);
+ wait(3);
+ AX12_automate(AX12_DROIT_CROC_OUVERT);
+ wait(2);
+ AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
+ wait(2);
+ AX12_automate(AX12_TOURNANTE_DROIT_PREPARATION);
+ wait(2);
+ AX12_automate(AX12_TOURNANTE_DROIT_MODULE);
+ wait(2);
+ */
+ AX12_automate(AX12_DROIT_CROC_INITIALE);
+ wait(2);
+ AX12_automate(AX12_GAUCHE_CROC_INITIALE);
+ wait(3);
+
+
while(1) {
+ AX12_automate(AX12_POSITION);
+
led = !led;
canProcessRx();//Traitement des trames CAN en attente
@@ -88,39 +126,12 @@
action_a_effectuer=0;
if (ActionAx12==1){
- AX12_automate(EtatAx12, ChoixBras);
+ AX12_automate(EtatAx12);
ActionAx12=0;
}
- if ((EtatPompeDroite==1)&&(ActionPompe==1))
- ActionPompe=0, PompeDroite.write(1);
- else if ((EtatPompeDroite==0)&&(ActionPompe==1))
- PompeDroite.write(0), ActionPompe=0;
-
- /*
- if (EtatPompeGauche==1)
- PompeGauche.write(1);
- else if (EtatPompeGauche==0)
- PompeGauche.write(0);
- */
-
- if (EtatTurbine==1)
- gerer_turbine(20);
- else if (EtatTurbine==0)
- gerer_turbine(0);
-
- if (EtatLanceur==1)
- MotLanceur.write(1);
- else if (EtatLanceur==0)
- MotLanceur.write(0);
- /*
- if (EtatServoVanne==1)
- ServoVanne.write(0.4);
- if (EtatServoVanne==0)
- ServoVanne.write(0);
- */
}
-
}
+
}