CRAC Team
Programme d'utilisation des AX12 et de l'MX12 V3
Fork of
Utilisatio_MX12_V2
by 
CRAC Team
Diff: Actionneur.cpp
- Revision:
- 2:9d280856a536
- Child:
- 3:1bb26049bdd1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneur.cpp Fri May 19 08:06:49 2017 +0000
@@ -0,0 +1,547 @@
+#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 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;
+
+
+
+ /* 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=700;
+
+ HautBrasPompe = new AX12(p9, p10, 3, 1000000);
+ MilieuBrasPompe = new AX12(p9, p10, 2, 1000000);
+ BasBrasPompe = new AX12(p9, p10, 1, 1000000);
+
+ BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
+
+ HautBrasPompe->Set_Goal_speed(vitesse);
+ MilieuBrasPompe->Set_Goal_speed(vitesse);
+ BasBrasPompe->Set_Goal_speed(vitesse);
+
+ HautBrasPompe->Set_Mode(0);
+ MilieuBrasPompe->Set_Mode(0);
+ BasBrasPompe->Set_Mode(0);
+
+}
+
+void GetPositionAx12(void) {
+
+ pc.printf("\n\r * Bras Pompe * \n\r");
+
+ 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() );
+
+}
+
+
+/****************************************************************************************/
+/* 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){
+
+ unsigned short speed;
+
+ static unsigned char action = 0;
+ unsigned int GoalPos1, GoalPos2, GoalPos3;
+ switch(etat_ax12){
+
+ case AX12_GET_POSITION:
+ GetPositionAx12();
+ 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);
+
+ 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();
+ break;
+
+ case AX12_REMPLIR_BASE_POMPE:
+
+ speed=511;
+
+ 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);
+
+ GoalPos3=1413;
+ GoalPos2=624;
+ GoalPos1=1500;
+ mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
+
+ 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);
+ }
+ break;
+
+ case AX12_PREPARATION_PRISE :
+ Preparation_prise(choix_bras);
+ if (action == 0){
+ Fin_action();
+ action ++;
+ }
+ break;
+
+ case AX12_STOCKAGE_HAUT :
+ Stockage_haut(choix_bras);
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_STOCKAGE_BAS :
+ Stockage_bas(choix_bras);
+ etat_ax12 = AX12_DEFAUT;
+ 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();
+ break;
+
+ case AX12_PREPARATION_DEPOT_HAUT :
+ Preparation_depot_haut(choix_bras);
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_POUSSER_MODULE :
+ Pousser_module(choix_bras);
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_DEFAUT :
+ action = 0;
+ 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)
+{
+ char TabPompeBras[15];
+ unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
+
+ 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);
+
+
+ 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);
+ }
+
+ 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);
+ }
+
+
+
+}
+
+
+
+/****************************************************************************************/
+/* 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);
+ }
+
+ 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);
+ }
+}
+
+/****************************************************************************************/
+/* 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);
+ }
+
+ 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);
+ }
+}
+
+/****************************************************************************************/
+/* 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] = AX12_PREPARATION_PRISE;
+ 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