CRAC Team
/
Utilisatio_MX12_V2
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 2:9d280856a536, committed 2017-05-19
- Comitter:
- matthieuvignon
- Date:
- Fri May 19 08:06:49 2017 +0000
- Parent:
- 1:f3f702086a30
- Child:
- 3:1bb26049bdd1
- Commit message:
- Programme AX12 + verification position
Changed in this revision
--- a/AX12/AX12.h Thu May 11 11:49:50 2017 +0000 +++ b/AX12/AX12.h Fri May 19 08:06:49 2017 +0000 @@ -27,9 +27,9 @@ #include "mbed.h" //#define AX12_WRITE_DEBUG 0 -#define AX12_READ_DEBUG +//#define AX12_READ_DEBUG //#define AX12_TRIGGER_DEBUG 0 -#define AX12_DEBUG 0 +//#define AX12_DEBUG 0 /****** à utiliser pour le debug !! ******/
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneur.h Fri May 19 08:06:49 2017 +0000
@@ -0,0 +1,325 @@
+#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 TOLERANCE_AX12 50
+
+ /* PROTOTYPES DE FONCTIONS ET POINTEURS */
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Fin_action */
+/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
+/****************************************************************************************/
+void Fin_action(void);
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN.cpp Fri May 19 08:06:49 2017 +0000
@@ -0,0 +1,191 @@
+#include "all_includes.h"
+
+extern CANMessage msgRxBuffer[SIZE_FIFO];
+extern CAN can;
+extern DigitalOut led2;
+extern void GetPositionAx12(void);
+
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+unsigned char FlagAx12 = 0;
+
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: SendRawId */
+/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
+/*********************************************************************************************************/
+void SendRawId (unsigned short id)
+{
+ CANMessage msgTx=CANMessage();
+ msgTx.id=id;
+ msgTx.len=0;
+ can.write(msgTx);
+ wait_us(50);
+}
+
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: canRx_ISR */
+/* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
+/*********************************************************************************************************/
+void canRx_ISR (void)
+{
+ if (can.read(msgRxBuffer[FIFO_ecriture]))
+ FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+}
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fonction de traitement des messages CAN */
+/****************************************************************************************/
+void canProcessRx(void){
+ static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+ CANMessage msgTx=CANMessage();
+ FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+ if(FIFO_occupation<0)
+ FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+ if(FIFO_max_occupation<FIFO_occupation)
+ FIFO_max_occupation=FIFO_occupation;
+ 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;
+ break;
+
+ case SERVO_AX12_ACTION :
+
+ 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;
+ msgTx.len = 1;
+ msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+ can.write(msgTx);
+ break;
+
+ case 0x123:
+ SendRawId(100);
+ GetPositionAx12();
+ break;
+
+ }
+ action_a_effectuer=1;
+ FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+ }
+}
+
+
+
+
+void CAN2_wrFilter (uint32_t id) {
+ static int CAN_std_cnt = 0;
+ uint32_t buf0, buf1;
+ int cnt1, cnt2, bound1;
+
+ /* Acceptance Filter Memory full */
+ if (((CAN_std_cnt + 1) >> 1) >= 512)
+ return; /* error: objects full */
+
+ /* Setup Acceptance Filter Configuration
+ Acceptance Filter Mode Register = Off */
+ LPC_CANAF->AFMR = 0x00000001;
+
+ id |= 1 << 13; /* Add controller number(2) */
+ id &= 0x0000F7FF; /* Mask out 16-bits of ID */
+
+ if (CAN_std_cnt == 0) { /* For entering first ID */
+ LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
+ } else if (CAN_std_cnt == 1) { /* For entering second ID */
+ if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
+ LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
+ else
+ LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
+ } else {
+ /* Find where to insert new ID */
+ cnt1 = 0;
+ cnt2 = CAN_std_cnt;
+ bound1 = (CAN_std_cnt - 1) >> 1;
+ while (cnt1 <= bound1) { /* Loop through standard existing IDs */
+ if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id) {
+ cnt2 = cnt1 * 2;
+ break;
+ }
+ if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id) {
+ cnt2 = cnt1 * 2 + 1;
+ break;
+ }
+ cnt1++; /* cnt1 = U32 where to insert new ID */
+ } /* cnt2 = U16 where to insert new ID */
+
+ if (cnt1 > bound1) { /* Adding ID as last entry */
+ if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */
+ LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16);
+ else /* Odd number of IDs exists */
+ LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
+ } else {
+ buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */
+ if ((cnt2 & 0x0001) == 0) /* Insert new mask to even address */
+ buf1 = (id << 16) | (buf0 >> 16);
+ else /* Insert new mask to odd address */
+ buf1 = (buf0 & 0xFFFF0000) | id;
+
+ LPC_CANAF_RAM->mask[cnt1] = buf1; /* Insert mask */
+
+ bound1 = CAN_std_cnt >> 1;
+ /* Move all remaining standard mask entries one place up */
+ while (cnt1 < bound1) {
+ cnt1++;
+ buf1 = LPC_CANAF_RAM->mask[cnt1];
+ LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
+ buf0 = buf1;
+ }
+
+ if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */
+ LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
+ }
+ }
+ CAN_std_cnt++;
+
+ /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
+ buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
+ buf1 = buf0;
+
+ /* Setup acceptance filter pointers */
+ LPC_CANAF->SFF_sa = 0;
+ LPC_CANAF->SFF_GRP_sa = buf0;
+ LPC_CANAF->EFF_sa = buf0;
+ LPC_CANAF->EFF_GRP_sa = buf1;
+ LPC_CANAF->ENDofTable = buf1;
+
+ LPC_CANAF->AFMR = 0x00000000; /* Use acceptance filter */
+} // CAN2_wrFilter
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Turbine.cpp Fri May 19 08:06:49 2017 +0000
@@ -0,0 +1,19 @@
+#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/all_includes.h Fri May 19 08:06:49 2017 +0000 @@ -0,0 +1,29 @@ + +#include "mbed.h" +#include "AX12.h" +#include "ident_crac.h" +#include "cmsis.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 PwmOut PompeDroite; + +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 GetPositionAx12(void); +void SendRawId (unsigned short id); +void initialisation_AX12(void); +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); + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ident_crac.h Fri May 19 08:06:49 2017 +0000 @@ -0,0 +1,146 @@ +#ifndef CRAC_IDENTH +#define CRAC_IDENTH + + + +#define GLOBAL_GAME_END 0x004 // Stop fin du match +#define GLOBAL_START 0x002 // Start +#define GLOBAL_END_INIT_POSITION 0x005 // Fin positionnement robot avant depart +#define GLOBAL_FUNNY_ACTION 0x007 // Funny action start (0: start, 1: stop) + +#define BALISE_STOP 0x003 // Trame stop + +#define BALISE_DANGER 0xA // Trame danger + +#define BALISE_END_DANGER 0xB // Trame fin de danger + + +#define ASSERVISSEMENT_STOP 0x001 // Stop moteur +#define ASSERVISSEMENT_SPEED_DANGER 0x006 // Vitesse de danger +#define ASSERVISSEMENT_XYT 0x020 // Asservissement (x,y,theta) (0 : au choix 1 : avant -1 : arrière) +#define ASSERVISSEMENT_COURBURE 0x021 // Asservissement rayon de courbure (+ gauche, - droite , sens : 1avt , -1arr; enchainement => 1 oui, 0 => non, 2=>derniére instruction de l'enchainement) +#define ASSERVISSEMENT_CONFIG 0x022 // Asservissement paramètre (définir les valeurs de vitesse max et d'eccélération max) +#define ASSERVISSEMENT_ROTATION 0x023 // Asservissement rotation +#define ASSERVISSEMENT_RECALAGE 0x024 // Moteur tout droit (recalage : 0 mouvement seul, 1 x, 2y valeur : coordonnée à laquelle est recalé x/y; enchainement => 1 oui, 0 => non) + +#define ODOMETRIE_BIG_POSITION 0x026 // Odométrie position robot (Position actuel du robot) +#define ODOMETRIE_BIG_VITESSE 0x027 // Odométrie vitesse (Indication sur l'état actuel) +#define ODOMETRIE_SMALL_POSITION 0x028 // Odométrie position robot (Position actuel du robot) +#define ODOMETRIE_SMALL_VITESSE 0x029 // Odométrie vitesse (Indication sur l'état actuel) +#define ACTION_BIG_DEMARRAGE 0x025 // Action de départ du GR (Lancement de la trajectoire de départ du GR) + +#define ASSERVISSEMENT_INFO_CONSIGNE 0x1F0 // Info Consigne et Commande moteur +#define ASSERVISSEMENT_CONFIG_KPP_DROITE 0x1F1 // Config coef KPP_Droit +#define ASSERVISSEMENT_CONFIG_KPI_DROITE 0x1F2 // Config coef KPI_Droit +#define ASSERVISSEMENT_CONFIG_KPD_DROITE 0x1F3 // Config coef KPD_Droit +#define ASSERVISSEMENT_CONFIG_KPP_GAUCHE 0x1F4 // Config coef KPP_Gauche +#define ASSERVISSEMENT_CONFIG_KPI_GAUCHE 0x1F5 // Config coef KPI_Gauche +#define ASSERVISSEMENT_CONFIG_KPD_GAUCHE 0x1F6 // Config coef KPD_Gauche +#define ASSERVISSEMENT_ENABLE 0x1F7 // Activation asservissement (0 : désactivation, 1 : activation) + + +#define RESET_BALISE 0x030 // Reset balise +#define RESET_MOTEUR 0x031 // Reset moteur +#define RESET_IHM 0x032 // Reset écran tactile +#define RESET_ACTIONNEURS 0x033 // Reset actionneurs +#define RESET_POMPES 0x034 // Reset pompes +#define RESET_AX12 0x035 // Reset AX12 +#define RESET_TELEMETRE 0x036 // Reset telemetre + + + +#define RESET_STRAT 0x3A // Reset stratégie + + +#define CHECK_BALISE 0x060 // Check balise +#define CHECK_MOTEUR 0x061 // Check moteur +#define CHECK_IHM 0x062 // Check écran tactile +#define CHECK_ACTIONNEURS 0x063 // Check actionneurs +#define CHECK_POMPES 0x064 // Check pompes +#define CHECK_AX12 0x065 // Check AX12 +#define CHECK_OK_TELEMETRE 0x066 // Check telemetre + + + + +#define ALIVE_BALISE 0x070 // Alive balise +#define ALIVE_MOTEUR 0x071 // Alive moteur +#define ALIVE_IHM 0x072 // Alive écran tactile +#define ALIVE_ACTIONNEURS 0x073 // Alive actionneurs +#define ALIVE_POMPES 0x074 // Alive pompes +#define ALIVE_AX12 0x075 // Alive AX12 +#define ALIVE_TELEMETRE 0x076 // Alive telemetre + + + + +#define ACKNOWLEDGE_BALISE 0x100 // Acknowledge balise +#define ACKNOWLEDGE_MOTEUR 0x101 // Acknowledge moteur +#define ACKNOWLEDGE_IHM 0x102 // Acknowledge ecran tactile +#define ACKNOWLEDGE_ACTIONNEURS 0x103 // Acknowledge actionneurs +#define ACKNOWLEDGE_POMPES 0x104 // Acknowledge pompes +#define ACKNOWLEDGE_TELEMETRE 0x105 // Acknowledge telemetre +#define ACKNOWLEDGE_AX12 0x106 // Ack ax12 +#define ACKNOWLEDGE_STRAT 0x10A // Acknowledge pompes + + +#define INSTRUCTION_END_BALISE 0x110 // Fin instruction balise (Indique que l'instruction est terminée) +#define INSTRUCTION_END_MOTEUR 0x111 // Fin instruction moteur (Indique que l'instruction est terminée) +#define INSTRUCTION_END_IHM 0x112 // Fin instruction ecran tactile (Indique que l'instruction est terminée) +#define INSTRUCTION_END_ACTIONNEURS 0x113 // Fin instruction actionneurs (Indique que l'instruction est terminée) +#define INSTRUCTION_END_AX12 0x116 + +#define ECRAN_CHOICE_STRAT 0x601 // Choix d'une stratégie (n° strat (1-4)) +#define ECRAN_CHOICE_COLOR 0x602 // Couleur (0->Blue;1->Yellow) +#define ECRAN_START_MATCH 0x603 // Match (Indique que l'on souhaite commencer le match) +#define ECRAN_DEMO_BEGIN 0x604 // Demo (Indique que l'on souhaite faire une demo) +#define ECRAN_ACK_STRAT 0x611 // Acknowledge stratégie (si 0 erreur, sinon n°strat) +#define ECRAN_ACK_COLOR 0x612 // Acknowledge couleur (0->Blue;1->Yellow) +#define ECRAN_ACK_START_MATCH 0x613 // Acknowledge Match (Indique que l'on a bien reçu le debut du match) +#define ECRAN_ACK_DEMO 0x614 +#define ECRAN_ALL_CHECK 0x620 // Carte all check (Si provient de carte strat => toutes les cartes sont en ligne, Si provient IHM => forcer le lancement) +#define ECRAN_TIME 0x621 // Time match (Indication de moment cle du temps (10,30,60,70,80,85,90)) +#define ECRAN_PRINTF_1 0x6C0 // Tactile printf (Afficher les 8 permier caractères) +#define ECRAN_PRINTF_2 0x6C1 // Tactile printf (Afficher les 8 second caractères) +#define ECRAN_PRINTF_3 0x6C2 // Tactile printf (Afficher les 8 troisième caractères) +#define ECRAN_PRINTF_4 0x6C3 // Tactile printf (Afficher les 8 quatrième caractères) +#define ECRAN_PRINTF_CLEAR 0x6CF // Tactile printf clear (Permet d'effacer l'ecran) +#define ECRAN_CHOICE_START_ACTION 0x604 // Tactile printf clear (Choisir si il faut lancer le test actionneur) +#define ECRAN_ACK_CHOICE_START_ACTION 0x605 // Tactile printf clear (Ack du test actionneur) + +#define ERROR_OVERFLOW_BALISE 0x040 // Overflow odométrie +#define ERROR_OVERFLOW_MOTEUR 0x041 // Overflow asservissement +#define ERROR_OVERFLOW_IHM 0x042 // Overflow balise +#define ERROR_OVERFLOW_STRAT 0x043 // Overflow stratégie +#define ERROR_BALISE 0x785 // Bug balise +#define ERROR_RTC 0x786 // Bug RTC +#define ERROR_MOTEUR 0x787 // Bug moteur +#define ERROR_TELEMETRIE 0x788 // Bug télémètre +#define ERROR_STRATEGIE 0x789 // Bug stratégie + +#define DEBUG_STRATEGIE_AUTOMATE 0x760 // Etat automate stratégie (Permet de savoir l'etat de l'automate) +#define DEBUG_FAKE_JAKE 0x761 // Fake jack (Permet d'outre passerr le JACk du robot) +#define DEBUG_ASSERV 0x762 // Info debug carte moteur + + +#define SERVO_AX12_ACTION 0x96 //AX12 action +#define SERVO_AX12_ACK 0x106 //AX12 ACK +#define SERVO_AX12_END 0x116 //AX12 END action +#define SERVO_AX12_POSITION 0x126 //AX12_POSITION + +#define SERVO_AX12_SETGOAL 0x090 // AX12 setGoal (Indiquer la nouvelle position de l'AX12 !! Ne bouge pas) +#define SERVO_AX12_PROCESS 0x091 // AX12 processChange (Lancer le déplacement des AX12) +#define SERVO_AX12_DONE 0x092 // AX12 done (Indique q'un AX12 a terminé son déplacement) +#define SERVO_XL320 0x093 // XL320 +#define POMPE_PWM 0x9A // pwm des pompes + +#define POMPE_DROITE 0x400 +#define POMPE_GAUCHE 0x401 +#define LANCEUR 0x402 +#define TURBINE 0x403 +#define SERVOVANNE 0x404 + +#define SERVO_XL320 0x093 // XL320 +#define POMPE_PWM 0x9A + +#endif
--- a/main.cpp Thu May 11 11:49:50 2017 +0000
+++ b/main.cpp Fri May 19 08:06:49 2017 +0000
@@ -1,10 +1,33 @@
-#include "mbed.h"
-#include "AX12.h"
-#include "cmsis.h"
+#include "all_includes.h"
+
+Timer t;
+Ticker flipper;
+
+CAN can(p30,p29); // Rx&Tx pour le CAN
+CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
+
+Serial pc(USBTX, USBRX);
+
+extern void gerer_turbine(unsigned char pwm_turbine);
+
-#define SIZE_FIFO 20
-#define TIME 1
-#define T_MOT 0.00005
+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;
+
/*
DigitalIn IO1(p23);
@@ -23,212 +46,81 @@
PwmOut IRL_2(p22);
*/
-AnalogIn cptGauche(p20);
-AnalogIn cptDroit(p19);
-
-DigitalIn pressionGauche(p23);
-DigitalIn pressionDroit(p24);
-
-AnalogIn telemetre(p15);
-
-DigitalIn jack(p25);
-
-PwmOut motGauche(p21);
-PwmOut motDroit(p22);
-
-DigitalOut led(LED1);
-
-CAN can1(p30,p29); // Rx&Tx pour le CAN
-CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
-unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
-signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
-
-AX12 *AX12_17, *AX12_14, *multiple_AX12;
-AX12 *AX12_14_2, *AX12_11, *multiple_AX12_2;
-AX12 *AX12_4, *AX12_18, *multiple_AX12_3;
-
-int printCapteurs = 1;
-
-static char TAB1[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03,
- 0x11, 0x50, 0x02, 0xFF, 0x03};
-
-static char TAB2[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03,
- 0x11, 0x00, 0x00, 0xFF, 0x03};
-
-static char TAB3[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03,
- 0x0B, 0x50, 0x02, 0xFF, 0x03};
-
-static char TAB4[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03,
- 0x0B, 0x00, 0x00, 0xFF, 0x03};
-
-static char TAB5[10]= {0x04, 0x00, 0x00, 0xFF, 0x03,
- 0x12, 0x50, 0x02, 0xFF, 0x03};
-
-static char TAB6[10]= {0x04, 0x50, 0x02, 0xFF, 0x03,
- 0x12, 0x00, 0x00, 0xFF, 0x03};
-
-Timer t;
-Ticker flipper;
-void canRx_ISR (void)
-{
- if (can1.read(msgRxBuffer[FIFO_ecriture])) {
- /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset();
- else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/
- FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+
+int main() {
+
+ 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();
+
+ PompeDroite.period(0.001);
+ //PompeGauche.period(0.001);
+ MotLanceur.period(0.001);
+ // ServoVanne.period(0.001);
+
+
+ while(1) {
+ led = !led;
+ canProcessRx();//Traitement des trames CAN en attente
+
+
+ if (action_a_effectuer==1) {
+
+ action_a_effectuer=0;
+
+ if (ActionAx12==1){
+ AX12_automate(EtatAx12, ChoixBras);
+ 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);
+ */
+ }
+
}
}
-
-
-/*********************************************************************************************************/
-/* FUNCTION NAME: SendRawId */
-/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
-/*********************************************************************************************************/
-void SendRawId (unsigned short id)
-{
- CANMessage msgTx=CANMessage();
- msgTx.id=id;
- msgTx.len=0;
- can1.write(msgTx);
- wait_us(200);
-}
-
-/*********************************************************************************************************/
-/* FUNCTION NAME: AX1 */
-/* DESCRIPTION : bouge les AX12 */
-/*********************************************************************************************************/
-void AX1(void){
- multiple_AX12->multiple_goal_and_speed(2,TAB1);
- wait(TIME);
- multiple_AX12->multiple_goal_and_speed(2,TAB2);
- wait(TIME);
-}
-
-/*********************************************************************************************************/
-/* FUNCTION NAME: AX2 */
-/* DESCRIPTION : bouge les AX12 */
-/*********************************************************************************************************/
-void AX2(void){
- multiple_AX12_2->multiple_goal_and_speed(2,TAB3);
- wait(TIME);
- multiple_AX12_2->multiple_goal_and_speed(2,TAB4);
- wait(TIME);
-}
-
-/*********************************************************************************************************/
-/* FUNCTION NAME: AX3 */
-/* DESCRIPTION : bouge les AX12 */
-/*********************************************************************************************************/
-void AX3(void){
- multiple_AX12_3->multiple_goal_and_speed(2,TAB5);
- wait(TIME);
- multiple_AX12_3->multiple_goal_and_speed(2,TAB6);
- wait(TIME);
-}
-
-/*********************************************************************************************************/
-/* FUNCTION NAME: moteur */
-/* DESCRIPTION : bouge les moteurs */
-/*********************************************************************************************************/
-void moteur(void){
- motGauche.write(0.5f);
- motDroit.write(0.5f);
-}
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: canProcessRx */
-/* DESCRIPTION : Fonction de traitement des messages CAN */
-/****************************************************************************************/
-void canProcessRx(void){
- static signed char FIFO_occupation=0,FIFO_max_occupation=0;
- CANMessage msgTx=CANMessage();
- FIFO_occupation=FIFO_ecriture-FIFO_lecture;
- if(FIFO_occupation<0)
- FIFO_occupation=FIFO_occupation+SIZE_FIFO;
- if(FIFO_max_occupation<FIFO_occupation)
- FIFO_max_occupation=FIFO_occupation;
- if(FIFO_occupation!=0) {
-
- switch(msgRxBuffer[FIFO_lecture].id) {
- case 0x63:
- SendRawId(0x73);
- break;
- case 0x10:
- printCapteurs = 0;
- break;
- case 0x11:
- printCapteurs = 1;
- break;
-
-
- }
- FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
- }
-}
-
-int main() {
- can1.frequency(1000000); // fréquence de travail 1Mbit/s
- can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
-
- CANMessage msgTx=CANMessage();
-
-
- AX12_14 = new AX12(p9, p10, 14, 1000000); // tx, rx
- AX12_17 = new AX12(p9, p10, 17, 1000000);
- multiple_AX12 = new AX12(p9,p10,0xFE,1000000);
-
-
- AX12_14_2 = new AX12(p13, p14, 14, 1000000);
- AX12_11 = new AX12(p13, p14, 11, 1000000);
- multiple_AX12_2 = new AX12(p13,p14,0xFE,1000000);
-
- AX12_4= new AX12(p28, p27, 14, 1000000);
- AX12_18= new AX12(p28, p27, 11, 1000000);
- multiple_AX12_3= new AX12(p28,p27,0xFE,1000000);
-
- motGauche.period(T_MOT);
- motDroit.period(T_MOT);
- motGauche.write(0.0);
- motDroit.write(0.0);
-
- while(1) {
- led = !led;
-
- canProcessRx();
-
- if (printCapteurs){
-
- motGauche.write(0);
- motDroit.write(0);
-
- msgTx.id=0x21;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=7;
-
- msgTx.data[0]=(unsigned char)cptGauche.read();
- msgTx.data[1]=(unsigned char)cptDroit.read();
- msgTx.data[2]=(unsigned char)pressionGauche.read();
- msgTx.data[3]=(unsigned char)pressionDroit.read();
- msgTx.data[4]=(unsigned char)jack.read();
- msgTx.data[5]=(unsigned char)(telemetre.read_u16()>>8);
- msgTx.data[6]=(unsigned char)telemetre.read_u16();
-
- can1.write(msgTx);
- wait(0.2);
- }else{
- moteur();
-
- AX1();
- AX2();
- AX3();
-
- }
-
-
- }
-}
-