CRAC Team
test des capteurs/actionneurs petit robot
Fork of
mbed_tes_cpt
by 
CRAC Team
Actionneur.cpp
- Committer:
- matthieuvignon
- Date:
- 2017-05-25
- Revision:
- 5:7e1c328c5d50
- Parent:
- 3:43843ab8af47
File content as of revision 5:7e1c328c5d50:
#include "Actionneur.h"
extern CAN can;
/* DECLARATION VARIABLES */
extern unsigned char FlagAx12;
extern "C" void mbed_reset();//Pour pouvoir reset la carte
extern DigitalOut led2;
extern Serial pc;
extern void GetPositionAx12(void);
extern PwmOut Pompe;
#define MULT_TIMEOUT 30
unsigned char timeout=0;
Timeout flipper;
AX12 *Ax12_HautBrasModuleAvant, *Ax12_DevantPinceDroite, *Ax12_DevantPinceGauche, *Ax12_MilieuBrasModuleAvant, *BrasModuleAx12, *TabBrasModule;
AX12 *Ax12_MilieuBrasPompe, *Ax12_HautBrasPompe, *Ax12_BasBrasPompe, *BrasPompeAx12, *TabBrasPompe;
AX12 *Ax12_HautBrasAspirateur, *Ax12_BasBrasAspirateur, *BrasAspirateurAx12, *TabBrasAspirateur, *Ax12_FunnyAction;
AX12 *Ax12_Porte;
/* ANGLE */
/* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
30° = 0x64, 0x00 | 130°= 0xB1, 0x01
40° = 0x85, 0x00 | 140°= 0xD2, 0x01
50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
60° = 0xC8, 0x00 | 160°= 0x15, 0x02
70° = 0xE9, 0x00 | 170°= 0x36, 0x02
80° = 0x0A, 0x01 | 180°= 0x58, 0x02
90° = 0x2C, 0x01 | 190°= 0x79, 0x02
100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
/* NUMERO AX12 */
/* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
1 = 0x01 | 10 = 0x0A
2 = 0x02 | 11 = 0x0B
3 = 0x03 | 12 = 0x0C
4 = 0x04 | 13 = 0x0D
5 = 0x05 | 14 = 0x0E
6 = 0x06 | 15 = 0x0F
7 = 0x07 | 16 = 0x10
8 = 0x08 | 17 = 0x11 */
/* MAIN */
void initialisation_AX12(void)
{
short vitesse=900;
Ax12_HautBrasPompe = new AX12(p9, p10, 3, 1000000);
Ax12_MilieuBrasPompe = new AX12(p9, p10, 2, 1000000);
Ax12_BasBrasPompe = new AX12(p9, p10, 1, 1000000);
Ax12_HautBrasModuleAvant = new AX12(p13, p14, 4, 1000000);
Ax12_MilieuBrasModuleAvant = new AX12(p13, p14, 5, 1000000);
Ax12_DevantPinceGauche = new AX12(p13, p14, 6, 1000000);
Ax12_DevantPinceDroite = new AX12(p13, p14, 7, 1000000);
#ifdef AVANT
Ax12_HautBrasAspirateur = new AX12(p28, p27, 9, 1000000);
Ax12_BasBrasAspirateur = new AX12(p28, p27, 10, 1000000);
Ax12_FunnyAction = new AX12(p28, p27, 15, 1000000);
BrasAspirateurAx12 = new AX12(p28, p27,0xFE,1000000);
#endif
BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
BrasModuleAx12 = new AX12(p13, p14, 0xFE, 1000000);
#ifdef ARRIERE
Ax12_Porte = new AX12(p28, p27, 8, 1000000);
#endif
Ax12_HautBrasPompe->Set_Goal_speed(vitesse);
Ax12_MilieuBrasPompe->Set_Goal_speed(vitesse);
Ax12_BasBrasPompe->Set_Goal_speed(vitesse);
Ax12_HautBrasModuleAvant->Set_Goal_speed(vitesse);
Ax12_MilieuBrasModuleAvant->Set_Goal_speed(vitesse);
Ax12_DevantPinceGauche->Set_Goal_speed(vitesse);
Ax12_DevantPinceDroite->Set_Goal_speed(vitesse);
#ifdef AVANT
Ax12_HautBrasAspirateur->Set_Goal_speed(vitesse);
Ax12_BasBrasAspirateur->Set_Goal_speed(vitesse);
Ax12_FunnyAction->Set_Goal_speed(vitesse);
#endif
#ifdef ARRIERE
Ax12_Porte->Set_Goal_speed(vitesse);
#endif
Ax12_HautBrasPompe->Set_Mode(0);
Ax12_MilieuBrasPompe->Set_Mode(0);
Ax12_BasBrasPompe->Set_Mode(0);
Ax12_HautBrasModuleAvant->Set_Mode(0);
Ax12_MilieuBrasModuleAvant->Set_Mode(0);
Ax12_DevantPinceGauche->Set_Mode(0);
Ax12_DevantPinceDroite->Set_Mode(0);
#ifdef AVANT
Ax12_HautBrasAspirateur->Set_Mode(0);
Ax12_BasBrasAspirateur->Set_Mode(0);
Ax12_FunnyAction->Set_Mode(0);
#endif
#ifdef ARRIERE
Ax12_Porte->Set_Mode(0);
#endif
}
void GetPositionAx12(void) {
pc.printf("\n\r * Get Position * \n\r");
pc.printf("\n\r BRAS AVANT \n\r");
pc.printf("Haut Av: %lf \n\r ", Ax12_HautBrasModuleAvant->Get_Position() );
pc.printf("Milieu Av: %lf \n\r ", Ax12_MilieuBrasModuleAvant->Get_Position() );
pc.printf("Gauche Av: %lf \n\r ", Ax12_DevantPinceGauche->Get_Position() );
pc.printf("Droite Av: %lf \n\r ", Ax12_DevantPinceDroite->Get_Position() );
#ifdef AVANT
pc.printf("\n\r BRAS ASPIRATEUR \n\r");
pc.printf("Haut Asp : %lf \n\r ", Ax12_HautBrasAspirateur->Get_Position() );
pc.printf("Bas Asp : %lf \n\r ", Ax12_BasBrasAspirateur->Get_Position() );
pc.printf("Funny Action : %lf \n\r ", Ax12_FunnyAction->Get_Position() );
#endif
pc.printf("\n\r BRAS POMPE \n\r");
pc.printf("Haut Pompe : %lf \n\r ", Ax12_HautBrasPompe->Get_Position() );
pc.printf("Milieu Pompe : %lf \n\r ", Ax12_MilieuBrasPompe->Get_Position() );
pc.printf("Bas Pompe : %lf \n\r ", Ax12_BasBrasPompe->Get_Position() );
#ifdef ARRIERE
pc.printf("\n\r PORTE \n\r");
pc.printf("Haut Pompe : %lf \n\r ", Ax12_Porte->Get_Position() );
#endif
}
/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12 */
/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
/****************************************************************************************/
void AX12_automate(unsigned char etat_ax12){
unsigned short speed;
unsigned int GoalPos1, GoalPos2, GoalPos3, GoalPos4, GoalPos5, GoalPos6, GoalPos7, GoalPos9, GoalPos10, GoalPos15;
switch(etat_ax12){
case AX12_OUVRIR_PORTE:
Ax12_Porte->Set_Secure_Goal(60);
Fin_action();
break;
case AX12_FERMER_PORTE:
Ax12_Porte->Set_Secure_Goal(240);
Fin_action();
break;
case AX12_GET_POSITION:
GetPositionAx12();
Fin_action();
break;
case AX12_POS_INIT_POMPE_JAUNE_AVANT:
speed=1023;
GoalPos3=1443;
GoalPos2=625;
GoalPos1=1500;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_VIDER_FUSEE_POMPE_JAUNE_AVANT:
speed=1023;
GoalPos3=1440;
GoalPos2=603;
GoalPos1=2400;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1795; //1180
GoalPos2=340; //2000
GoalPos1=2393; //2400
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=2147; //640
GoalPos2=229; //2840
GoalPos1=2400; //2720
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=2346;
GoalPos2=205;
GoalPos1=2123;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_POSE_MODULE_POMPE_JAUNE_AVANT:
speed=300;
GoalPos3=2613;
GoalPos2=610;
GoalPos1=1352;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Pompe.write(0);
Fin_action();
break;
case AX12_POSE_MODULE_POMPE_BLEU_AVANT:
speed=300;
GoalPos3=2613;
GoalPos2=610;
GoalPos1=1352;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
speed=750;
GoalPos3=2610;
GoalPos2=850;
GoalPos1=2999;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Pompe.write(0);
Fin_action();
break;
case AX12_REMISE_PLACE_BRAS_POMPE_AVANT:
speed=1023;
GoalPos3=2200;
GoalPos2=214;
GoalPos1=2038;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1730;
GoalPos2=355;
GoalPos1=2197;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1487;
GoalPos2=500;
GoalPos1=2197;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1440;
GoalPos2=639;
GoalPos1=2400;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_REPOS_BRAS_POMPE_AVANT:
speed=1023;
GoalPos3=1890;
GoalPos2=242;
GoalPos1=1500;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_POS_INIT_POMPE_JAUNE_ARRIERE:
speed=1023;
GoalPos3=1615;
GoalPos2=2349;
GoalPos1=1500;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_VIDER_FUSEE_POMPE_JAUNE_ARRIERE:
speed=1023;
GoalPos3=1650;
GoalPos2=2380;
GoalPos1=1500;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1545;
GoalPos2=2480;
GoalPos1=600;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1146; //1595
GoalPos2=2698; //2349
GoalPos1=581; //2400
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=724; //953
GoalPos2=2783; //2806
GoalPos1=757; //2551
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_POSE_MODULE_POMPE_JAUNE_ARRIERE:
speed=300;
GoalPos3=401;
GoalPos2=2340;
GoalPos1=1094;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=401;
GoalPos2=2250;
GoalPos1=80;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Pompe.write(0);
Fin_action();
break;
case AX12_POSE_MODULE_POMPE_BLEU_ARRIERE:
speed=300;
GoalPos3=384;
GoalPos2=2774;
GoalPos1=1094;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=330;
GoalPos2=2506;
GoalPos1=1587;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Pompe.write(0);
Fin_action();
break;
case AX12_REMISE_PLACE_BRAS_POMPE_ARRIERE:
speed=1023;
GoalPos3=450; //449
GoalPos2=2800; //2833
GoalPos1=1500; //2762
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=765;
GoalPos2=2796;
GoalPos1=880;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1200;
GoalPos2=2750;
GoalPos1=600;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1480;
GoalPos2=2550;
GoalPos1=600;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
GoalPos3=1650;
GoalPos2=2380;
GoalPos1=600;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_REPOS_BRAS_POMPE_ARRIERE:
speed=1023;
GoalPos3=1209;
GoalPos2=2693;
GoalPos1=1500;
mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
Fin_action();
break;
case AX12_INITIALISATION :
speed = 1023;
GoalPos4=2200;
GoalPos5=1522;
GoalPos6=2528;
GoalPos7=513;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=2379;
GoalPos5=1522;
GoalPos6=2528;
GoalPos7=513;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_PREPARATION_PRISE :
speed = 1023;
GoalPos4=970;
GoalPos5=2060;
GoalPos6=1891;
GoalPos7=1150;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_STOCKAGE_HAUT :
speed = 1023;
GoalPos4=970;
GoalPos5=2060;
GoalPos6=2520;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1587;
GoalPos5=2032;
GoalPos6=2520;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=2003;
GoalPos5=1810;
GoalPos6=2520;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=2041;
GoalPos5=2018;
GoalPos6=2300;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=2041;
GoalPos5=2018;
GoalPos6=2200;
GoalPos7=750;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_STOCKAGE_BAS :
speed = 1023;
GoalPos4=970;
GoalPos5=2060;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1587;
GoalPos5=2032;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1510;
GoalPos5=1126;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1580;
GoalPos5=660;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_DEPOSER :
speed = 1023;
GoalPos4=1580;
GoalPos5=660;
GoalPos6=2200;
GoalPos7=800;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_PREPARATION_DEPOT_HAUT :
speed = 1023;
GoalPos4=1970;
GoalPos5=2140;
GoalPos6=2000;
GoalPos7=850;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1994;
GoalPos5=2110;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1977;
GoalPos5=1713;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
GoalPos4=1580;
GoalPos5=660;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_POUSSER_MODULE :
speed = 1023;
GoalPos4=1880;
GoalPos5=440;
GoalPos6=2460;
GoalPos7=550;
mvtBrasAvantModule(4, speed, GoalPos4, 5, speed, GoalPos5, 6, speed, GoalPos6, 7, speed, GoalPos7);
Fin_action();
break;
case AX12_DESCENDRE_ASPIRATEUR :
speed = 1023;
GoalPos9=1700;
GoalPos10=2200;
GoalPos15=2400;
mvtBrasAspirateur(9, speed, GoalPos9, 10, speed, GoalPos10, 15, speed, GoalPos15);
Fin_action();
break;
case AX12_MONTER_ASPIRATEUR :
speed = 1023;
GoalPos9=540;
GoalPos10=2370;
GoalPos15=2400;
mvtBrasAspirateur(9, speed, GoalPos9, 10, speed, GoalPos10, 15, speed, GoalPos15);
Fin_action();
break;
case AX12_FUNNY_ACTION:
speed = 1023;
GoalPos9=540;
GoalPos10=2370;
GoalPos15=1250;
mvtBrasAspirateur(9, speed, GoalPos9, 10, speed, GoalPos10, 15, speed, GoalPos15);
wait(1);
speed = 1023;
GoalPos9=540;
GoalPos10=2370;
GoalPos15=2400;
mvtBrasAspirateur(9, speed, GoalPos9, 10, speed, GoalPos10, 15, speed, GoalPos15);
Fin_action();
break;
case AX12_DEFAUT :
break;
}
}
unsigned char mvtBrasAvantPompe(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)
{
char TabPompeBras[15];
unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
Timer timeout;
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)
TabPompeBras[5] = ID2;
TabPompeBras[6] = GPosition2_1;
TabPompeBras[7] = GPosition2_1>>8;
TabPompeBras[8] = GSpeed2;
TabPompeBras[9] = GSpeed2>>8;
TabPompeBras[10] = ID3;
TabPompeBras[11] = GPosition3_1;
TabPompeBras[12] = GPosition3_1>>8;
TabPompeBras[13] = GSpeed3;
TabPompeBras[14] = GSpeed3>>8 ;
BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
wait(TIME);
timeout.start();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_HautBrasPompe->Get_Position()*10)>GPosition3*110/100)
|| ((unsigned short)(Ax12_HautBrasPompe->Get_Position()*10)<GPosition3*90/100))) {
Ax12_HautBrasPompe->Set_Secure_Goal(GPosition3/10) ;
wait(TIME);
}
//timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_MilieuBrasPompe->Get_Position()*10)>GPosition2*110/100)
|| ((unsigned short)(Ax12_MilieuBrasPompe->Get_Position()*10)<GPosition2*90/100))) {
Ax12_MilieuBrasPompe->Set_Secure_Goal(GPosition2/10) ;
wait(TIME);
}
//timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_BasBrasPompe->Get_Position()*10)>GPosition1*110/100)
|| ((unsigned short)(Ax12_BasBrasPompe->Get_Position()*10)<GPosition1*90/100))) {
Ax12_BasBrasPompe->Set_Secure_Goal(GPosition1/10) ;
wait(TIME);
}
timeout.reset();
}
unsigned char mvtBrasAvantModule(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)
{
char TabBrasAvant[20];
unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1;
Timer timeout;
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);
TabBrasAvant[0] = ID1;
TabBrasAvant[1] = GPosition1_1;
TabBrasAvant[2] = GPosition1_1>>8;
TabBrasAvant[3] = GSpeed1;
TabBrasAvant[4] = GSpeed1>>8; /// (haut, milieu, bas)
TabBrasAvant[5] = ID2;
TabBrasAvant[6] = GPosition2_1;
TabBrasAvant[7] = GPosition2_1>>8;
TabBrasAvant[8] = GSpeed2;
TabBrasAvant[9] = GSpeed2>>8;
TabBrasAvant[10] = ID3;
TabBrasAvant[11] = GPosition3_1;
TabBrasAvant[12] = GPosition3_1>>8;
TabBrasAvant[13] = GSpeed3;
TabBrasAvant[14] = GSpeed3>>8 ;
TabBrasAvant[15] = ID4;
TabBrasAvant[16] = GPosition4_1;
TabBrasAvant[17] = GPosition4_1>>8;
TabBrasAvant[18] = GSpeed4;
TabBrasAvant[19] = GSpeed4>>8 ;
BrasModuleAx12->multiple_goal_and_speed(4,TabBrasAvant) ;
wait(TIME);
timeout.start();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_HautBrasModuleAvant->Get_Position()*10)>GPosition1*105/100)
|| ((unsigned short)(Ax12_HautBrasModuleAvant->Get_Position()*10)<GPosition1*95/100))) {
Ax12_HautBrasModuleAvant->Set_Secure_Goal(GPosition1/10) ;
wait(TIME);
}
timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_MilieuBrasModuleAvant->Get_Position()*10)>GPosition2*105/100)
|| ((unsigned short)(Ax12_MilieuBrasModuleAvant->Get_Position()*10)<GPosition2*95/100))) {
Ax12_MilieuBrasModuleAvant->Set_Secure_Goal(GPosition2/10) ;
wait(TIME);
}
timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_DevantPinceGauche->Get_Position()*10)>GPosition3*105/100)
|| ((unsigned short)(Ax12_DevantPinceGauche->Get_Position()*10)<GPosition3*95/100))) {
Ax12_DevantPinceGauche->Set_Secure_Goal(GPosition3/10) ;
wait(TIME);
}
timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_DevantPinceDroite->Get_Position()*10)>GPosition4*105/100)
|| ((unsigned short)(Ax12_DevantPinceDroite->Get_Position()*10)<GPosition4*95/100))) {
Ax12_DevantPinceDroite->Set_Secure_Goal(GPosition4/10) ;
wait(TIME);
}
}
unsigned char mvtBrasAspirateur(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)
{
char TabAspirateurBras[15];
unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
Timer timeout;
GPosition1_1=((unsigned long)GPosition1*341/1000);
GPosition2_1=((unsigned long)GPosition2*341/1000);
GPosition3_1=((unsigned long)GPosition3*341/1000);
TabAspirateurBras[0] = ID1;
TabAspirateurBras[1] = GPosition1_1;
TabAspirateurBras[2] = GPosition1_1>>8;
TabAspirateurBras[3] = GSpeed1;
TabAspirateurBras[4] = GSpeed1>>8;
TabAspirateurBras[5] = ID2;
TabAspirateurBras[6] = GPosition2_1;
TabAspirateurBras[7] = GPosition2_1>>8;
TabAspirateurBras[8] = GSpeed2;
TabAspirateurBras[9] = GSpeed2>>8;
TabAspirateurBras[10] = ID3;
TabAspirateurBras[11] = GPosition3_1;
TabAspirateurBras[12] = GPosition3_1>>8;
TabAspirateurBras[13] = GSpeed3;
TabAspirateurBras[14] = GSpeed3>>8 ;
BrasAspirateurAx12->multiple_goal_and_speed(3,TabAspirateurBras) ;
wait(TIME);
timeout.start();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_HautBrasAspirateur->Get_Position()*10)>GPosition1*105/100)
|| ((unsigned short)(Ax12_HautBrasAspirateur->Get_Position()*10)<GPosition1*95/100))) {
Ax12_HautBrasAspirateur->Set_Secure_Goal(GPosition1/10) ;
wait(TIME);
}
timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_BasBrasAspirateur->Get_Position()*10)>GPosition2*105/100)
|| ((unsigned short)(Ax12_BasBrasAspirateur->Get_Position()*10)<GPosition2*95/100))) {
Ax12_BasBrasAspirateur->Set_Secure_Goal(GPosition2/10) ;
wait(TIME);
}
timeout.reset();
while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_FunnyAction->Get_Position()*10)>GPosition3*105/100)
|| ((unsigned short)(Ax12_FunnyAction->Get_Position()*10)<GPosition3*95/100))) {
Ax12_FunnyAction->Set_Secure_Goal(GPosition3/10) ;
wait(TIME);
}
}
/****************************************************************************************/
/* FUNCTION NAME: Fin_action */
/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
/****************************************************************************************/
void Fin_action(void){
CANMessage msgTx=CANMessage();
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.id = SERVO_AX12_END;
msgTx.len = 1;
msgTx.data[0] = 0x96;
can.write(msgTx);
}