Programme d'utilisation des AX12 et de l'MX12 V3
Fork of Utilisatio_MX12_V2 by
Actionneur.cpp
- Committer:
- R66Y
- Date:
- 2017-05-20
- Revision:
- 4:0972025782f3
- Parent:
- 3:1bb26049bdd1
File content as of revision 4:0972025782f3:
#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 *un_myMX12, *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; AX12 *TrappeLanceur, *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; TrappeLanceur = new AX12(p9, p10, 1, 1000000); // MX12 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); TrappeLanceur->Set_Goal_speed(vitesse); // MX12 HautBrasPompe->Set_Goal_speed(vitesse); MilieuBrasPompe->Set_Goal_speed(vitesse); BasBrasPompe->Set_Goal_speed(vitesse); TrappeLanceur->Set_Mode(0); // MX12 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() ); pc.printf("Bas: %lf \n\r ", TrappeLanceur->Get_Position() ); // MX12 } /****************************************************************************************/ /* 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: // 10 (10) GetPositionAx12(); break; case AX12_VIDER_FUSEE_POMPE: // 8 (10) 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: // 11 (10) 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: // 9 (10) 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 : // 0 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 : // 1 (10) Preparation_prise(choix_bras); if (action == 0){ Fin_action(); action ++; } break; case AX12_STOCKAGE_HAUT : // 2 (10) Stockage_haut(choix_bras); etat_ax12 = AX12_DEFAUT; Fin_action(); break; case AX12_STOCKAGE_BAS : // 3 (10) Stockage_bas(choix_bras); etat_ax12 = AX12_DEFAUT; Fin_action(); break; case AX12_DEPOSER : // 4 (10) Deposer(choix_bras); etat_ax12 = AX12_DEFAUT; Fin_action(); break; case AX12_PREPARATION_DEPOT_BAS : // 5 (10) Preparation_depot_bas(choix_bras); etat_ax12 = AX12_DEFAUT; Fin_action(); break; case AX12_PREPARATION_DEPOT_HAUT : // 6 (10) Preparation_depot_haut(choix_bras); etat_ax12 = AX12_DEFAUT; Fin_action(); break; case AX12_POUSSER_MODULE : // 7 (10) Pousser_module(choix_bras); etat_ax12 = AX12_DEFAUT; Fin_action(); break; case BOUGER_MX12 : // 12 (10) bouger_MX12(choix_bras); // Le choix du bras détermine le sens de rotation de l'MX12 : 0 position initiale | 1 tourne à droite | 2 tourne à gauche. etat_ax12 = AX12_DEFAUT; Fin_action(); break; case AX12_DEFAUT : // 20 (10) 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){ TrappeLanceur->Set_Secure_Goal(600); // Initialise la position de l'MX12 à 180° 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: bouger_MX12 */ /* DESCRIPTION : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge */ /****************************************************************************************/ void bouger_MX12(unsigned char choix){ if( choix == 1) // Tourne à droite { TrappeLanceur->Set_Secure_Goal(0); // tourner droite wait(TIME); } else if(choix == 2) // Tourne à gauche { TrappeLanceur->Set_Secure_Goal(1200); // tourner gauche wait(TIME); } else if (choix == 0) { TrappeLanceur->Set_Secure_Goal(600); // position initiale 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); }