Programme d'utilisation des AX12 et de l'MX12 V3. 0C = action de l'MX12. (data0) 0 | 1 | 2 = position & sens de rotation
Dependencies: MX12
Fork of Utilisatio_MX12_V3 by
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