CRAC Team
code ax12 petit robot 12/05/2017
Fork of
command_AX12_petit_robot_V3
by 
CRAC Team
Diff: main.cpp
- Revision:
- 2:99b1cb0d9f5e
- Parent:
- 1:b3ff77670606
- Child:
- 3:6cbd7d2c78f4
--- a/main.cpp Tue Mar 21 14:46:54 2017 +0000
+++ b/main.cpp Tue Apr 18 16:02:40 2017 +0000
@@ -1,42 +1,139 @@
#include "mbed.h"
#include "AX12.h"
#include "cmsis.h"
+#include "ident_crac.h"
+#define SIZE_FIFO 14
+#define TIME 0.8
+
/* DECLARATION VARIABLES */
+CAN can1(p30,p29);
+CANMessage msgRxBuffer[SIZE_FIFO];
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
+
+unsigned char action = 0, choix = 0;
+short vitesse=700;
+float angle=0.0;
+float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
+AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *six_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
+
+
+ /* PROTOTYPES DE FONCTIONS ET POINTEURS */
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fonction de traitement des messages CAN */
+/****************************************************************************************/
+void canProcessRx(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: canRx_ISR */
+/* DESCRIPTION : Interruption en réception sur le CAN */
+/****************************************************************************************/
+void canRx_ISR (void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendRawId */
+/* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
+/****************************************************************************************/
+void SendRawId (unsigned short id);
+
+/****************************************************************************************/
+/* FUNCTION NAME: prendre_modules */
+/* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */
+/****************************************************************************************/
+void Prendre_modules(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Poser_modules */
+/* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */
+/****************************************************************************************/
+void Poser_modules(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Funny_action */
+/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
+/****************************************************************************************/
+void Funny_Action(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_modules */
+/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
+/****************************************************************************************/
+void Tourner_modules(void);
+
Timer t;
Ticker flipper;
-static char TAB1[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
- 0x12,0xBC, 0x02, 0xFF, 0x00,
- 0x04,0xE9, 0x00, 0xFF, 0x00,
- 0x07,0x4D, 0x01, 0xFF, 0x00,
- 0x0F,0x90, 0x01, 0xFF, 0x00};
+
+///////////TABLEAU PINCE DROITE ///////////////////////////
+static char TAB1[25]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte
+ 0x04,0x00, 0x02, 0xFF, 0x03,
+ 0x07,0x4D, 0x01, 0xFF, 0x03,
+ 0x0F,0x90, 0x01, 0xFF, 0x03};
-static char TAB2[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
- 0x12,0x25, 0x02, 0xFF, 0x00,
- 0x04,0x15, 0x02, 0xFF, 0x00,
- 0x07,0x4D, 0x01, 0xFF, 0x00,
- 0x0F,0x90, 0x01, 0xFF, 0x00};
+static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
+ 0x04,0xC0, 0x00, 0xFF, 0x03,
+ 0x07,0x4D, 0x01, 0xFF, 0x03,
+ 0x0F,0x90, 0x01, 0xFF, 0x03};
+
+static char TAB3[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee
+ 0x04,0x15, 0x02, 0xFF, 0x03,
+ 0x07,0xC0, 0x00, 0xFF, 0x03,
+ 0x0F,0x05, 0x02, 0xFF, 0x03};
+
+static char TAB4[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement
+ 0x04,0x30, 0x01, 0xFF, 0x03,
+ 0x07,0xC0, 0x00, 0xFF, 0x03,
+ 0x0F,0x05, 0x02, 0xFF, 0x03};
+
+static char TAB5[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
+ 0x04,0xC0, 0x00, 0xFF, 0x03,
+ 0x07,0xC0, 0x00, 0xFF, 0x03,
+ 0x0F,0x05, 0x02, 0xFF, 0x03};
+
+static char TAB6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin
+ 0x04,0x4D, 0x01, 0xFF, 0x03,
+ 0x07,0x4D, 0x01, 0xFF, 0x03,
+ 0x0F,0x90, 0x01, 0xFF, 0x03};
+
-static char TAB3[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
- 0x12,0x2C, 0x01, 0xFF, 0x00,
- 0x04,0x15, 0x02, 0xFF, 0x00,
- 0x07,0x4D, 0x01, 0xFF, 0x00,
- 0x0F,0x90, 0x01, 0xFF, 0x00};
+//////////////////TABLEAU PINCE GAUCHE /////////////////////////////
+static char TAB1125]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte
+ 0x04,0x00, 0x02, 0xFF, 0x03,
+ 0x07,0x4D, 0x01, 0xFF, 0x03,
+ 0x0F,0x90, 0x01, 0xFF, 0x03};
+
+static char TAB12[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
+ 0x04,0xC0, 0x00, 0xFF, 0x03,
+ 0x07,0x4D, 0x01, 0xFF, 0x03,
+ 0x0F,0x90, 0x01, 0xFF, 0x03};
-static char TAB4[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
- 0x12,0x2C, 0x01, 0xFF, 0x00,
- 0x04,0x4D, 0x01, 0xFF, 0x00,
- 0x07,0x4D, 0x01, 0xFF, 0x00,
- 0x0F,0x90, 0x01, 0xFF, 0x00};
+static char TAB13[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee
+ 0x04,0x15, 0x02, 0xFF, 0x03,
+ 0x07,0xC0, 0x00, 0xFF, 0x03,
+ 0x0F,0x05, 0x02, 0xFF, 0x03};
+
+static char TAB14[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement
+ 0x04,0x30, 0x01, 0xFF, 0x03,
+ 0x07,0xC0, 0x00, 0xFF, 0x03,
+ 0x0F,0x05, 0x02, 0xFF, 0x03};
-static char TAB5[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
- 0x12,0x2C, 0x01, 0xFF, 0x00,
- 0x04,0x4D, 0x01, 0xFF, 0x00,
- 0x07,0x6E, 0x01, 0xFF, 0x00,
- 0x0F,0x90, 0x01, 0xFF, 0x00};
+static char TAB15[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte
+ 0x04,0xC0, 0x00, 0xFF, 0x03,
+ 0x07,0xC0, 0x00, 0xFF, 0x03,
+ 0x0F,0x05, 0x02, 0xFF, 0x03};
+
+static char TAB1 6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin
+ 0x04,0x4D, 0x01, 0xFF, 0x03,
+ 0x07,0x4D, 0x01, 0xFF, 0x03,
+ 0x0F,0x90, 0x01, 0xFF, 0x03};
-
-
+
/* ANGLE */
/* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
@@ -61,126 +158,223 @@
6 = 0x06 | 15 = 0x0F
7 = 0x07 | 16 = 0x10
8 = 0x08 | 17 = 0x11 */
-
- /*static char Tab1[5]={ 0x09,
- 0x5E,
- 0x01,
- 0x70,
- 0x01};*/
-
- /*static char Tab2[5]={ 0x05,
- 0x96,
- 0x01,
- 0x85,
- 0x05};*/
-float angle=0.0;
-float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
-
-
- /* DECLARATION PROTOTYPES & POINTEURS */
-
-short vitesse=700; // Vitesse angulaire (0-1023)
-AX12 *dixhuit_myAX12, *quatre_myAX12,*six_myAX12, *sept_myAX12, *quinze_myAX12, *multiple_AX12;
-void Init_AX12(void);
-void Lecture(void);
-void Flip(void);
-
-
- /* FONCTIONS */
+
+ /* MAIN */
-void Init_AX12() // Initialisation des différents paramétres
+int main()
{
-
- dixhuit_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- quatre_myAX12-> Set_Goal_speed(vitesse);
+ 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 TrameTx = CANMessage();
+ TrameTx.format=CANStandard;
+ TrameTx.type=CANData;
+
+ /*
+ deux_myAX12 = new AX12(p9, p10, 2, 1000000); //main tourneuse
+ quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); // main tourneuse
+ */
+
+ trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince droite
+ treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince droite
+ quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince droite
+ huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince droite
+
+ quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince gauche
+ sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince gauche
+ quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince gauche
+ dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince gauche
+
+ six_myAX12 = new AX12(p28, p27, 6, 1000000); //funny action
+
+ multiple_myAX12 = new AX12(p13,p14,0xFE,1000000);
+ multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000);
+
+
+ while(true) {
+ canProcessRx();//Traitement des trames CAN en attente
+
+ if (action == 0){
+ choix = 1;
+ Prendre_modules(choix);
+ choix = 2;
+ Prendre_modules(choix);
+ wait(2);
+ /*
+ choix = 1;
+ Poser_modules(choix);
+ choix = 2;
+ Poser_modules(choix);
+ wait(2);
+
+ TrameTx.id = 0x092;
+ TrameTx.len = 1;
+ TrameTx.data[0] = 0;
+ can1.write(TrameTx);
+ */
+
+ action = 2;
+ }
+ }
+}
+
+ /* FONCTIONS */
+
+/****************************************************************************************/
+/* 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 SERVO_AX12_PROCESS :
+ action = 1;
+ break;
+
+ case CHECK_ACTIONNEURS:
+ SendRawId (ALIVE_ACTIONNEURS);
+ break;
+
+ }
+
+ FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: canRx_ISR */
+/* DESCRIPTION : Interruption en réception sur le CAN */
+/****************************************************************************************/
+void canRx_ISR (void)
+{
+ if (can1.read(msgRxBuffer[FIFO_ecriture])) {
+ if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
+ else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendRawId */
+/* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
+/****************************************************************************************/
+void SendRawId (unsigned short id)
+{
+ CANMessage msgTx=CANMessage();
+ msgTx.id=id;
+ msgTx.len=0;
+ can1.write(msgTx);
+}
+ void Init_AX12() // Initialisation des différents paramétres
+{
+ deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
+ quatre_myAX12-> Set_Goal_speed(vitesse);
six_myAX12-> Set_Goal_speed(vitesse);
sept_myAX12-> Set_Goal_speed(vitesse);
+ quatorze_myAX12-> Set_Goal_speed(vitesse);
quinze_myAX12-> Set_Goal_speed(vitesse);
+ dixhuit_myAX12-> Set_Goal_speed(vitesse);
+ huit_myAX12-> Set_Goal_speed(vitesse);
+ trois_myAX12-> Set_Goal_speed(vitesse);
+ quatorze_myAX12-> Set_Goal_speed(vitesse);
+ treize_myAX12-> Set_Goal_speed(vitesse);
- dixhuit_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
+ deux_myAX12-> Set_Mode(0);
quatre_myAX12-> Set_Mode(0);
six_myAX12-> Set_Mode(0);
sept_myAX12-> Set_Mode(0);
+ quatorze_myAX12-> Set_Mode(0);
quinze_myAX12-> Set_Mode(0);
-}
-
-/*void Lecture()
-{
- angle= socle_myAX12-> Get_Position();
- test_socle = socle_myAX12 -> read_and_test(angle,TAB);
-
- angle= bas_myAX12-> Get_Position();
- test_bas = bas_myAX12-> read_and_test(angle,TAB);
-
- angle= milieu_myAX12-> Get_Position();
- test_milieu = milieu_myAX12-> read_and_test(angle,TAB);
-
- angle= haut_myAX12-> Get_Position();
- test_haut = haut_myAX12-> read_and_test(angle,TAB);
-
- angle= ventouse_myAX12-> Get_Position();
- test_ventouse = ventouse_myAX12-> read_and_test(angle,TAB);
-
- valeur_test=test_bas+test_milieu+test_haut+test_socle+test_ventouse;
+ dixhuit_myAX12-> Set_Mode(0);
+ huit_myAX12-> Set_Mode(0);
+ trois_myAX12-> Set_Mode(0);
+ quatorze_myAX12-> Set_Mode(0);
+ treize_myAX12-> Set_Mode(0);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: prendre_modules */
+/* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */
+/****************************************************************************************/
+void Prendre_modules(unsigned char choix){
+ if (choix == 1){
+ //multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+ //wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ 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(5,TAB6);
+ wait(TIME);
+ }
- #ifdef AX12_DEBUG
- printf("\ntest_socle = %f",test_socle);
- #endif
- #ifdef AX12_DEBUG
- printf("\ntest_bas = %f",test_bas);
- #endif
- #ifdef AX12_DEBUG
- printf("\ntest_milieu = %f",test_milieu);
- #endif
- #ifdef AX12_DEBUG
- printf("\ntest_haut = %f",test_haut);
- #endif
- #ifdef AX12_DEBUG
- printf("\ntest_ventouse = %f",test_ventouse);
- #endif
- #ifdef AX12_DEBUG
- printf("\nvaleur_test = %f",valeur_test);
- #endif
-}*/
-
- /* PROGRAMME PRINCIPAL*/
+ else if (choix == 2){
+ //multiple2_myAX12->multiple_goal_and_speed(4,TAB11);
+ //wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB15);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB13);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(4,TAB14);
+ wait(TIME);
+ multiple2_myAX12->multiple_goal_and_speed(5,TAB16);
+ wait(TIME);
+ }
+
+ SendRawId(0x123);
+}
-int main()
-{
- // flipper.attach(&Flip, 2.0);
- dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000);
- quatre_myAX12 = new AX12(p13, p14, 4, 1000000);
- six_myAX12 = new AX12(p13, p14, 6, 1000000);
- sept_myAX12 = new AX12(p13, p14, 7, 1000000);
- quinze_myAX12 = new AX12(p13, p14, 15, 1000000);
- multiple_AX12 = new AX12(p13,p14,0xFE,1000000); // Création objet : data pin 9&10, ID Broadcast, baud
-
- multiple_AX12->multiple_goal_and_speed(5,TAB1);
- wait(2);
- multiple_AX12->multiple_goal_and_speed(5,TAB2);
- wait(2);
- multiple_AX12->multiple_goal_and_speed(5,TAB3);
- wait(2);
- multiple_AX12->multiple_goal_and_speed(5,TAB4);
- wait(2);
- multiple_AX12->multiple_goal_and_speed(5,TAB5);
-
-
- //six_myAX12 -> Set_Secure_Goal(200);
- /*quatorze_myAX12 -> Set_Secure_Goal(260);
- gauche_myAX12 -> Set_Secure_Goal(280);
- quatorze_myAX12 -> Set_Secure_Goal(300); //avoir ax12 14 a 90°*/
-
- /* while(valeur_test<4)
- {
- Lecture();
- }*/
-
-
-
-
+/****************************************************************************************/
+/* FUNCTION NAME: Poser_modules */
+/* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */
+/****************************************************************************************/
+void Poser_modules(unsigned char choix){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB4);
+ wait(2);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ wait(2);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+ wait(2);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Funny_action */
+/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */
+/****************************************************************************************/
+void Funny_Action(void){
+ six_myAX12 -> Set_Secure_Goal(150);
+ wait(0.5);
+ six_myAX12 -> Set_Secure_Goal(60);
+ wait(0.5);
+ six_myAX12 -> Set_Secure_Goal(150);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_modules */
+/* DESCRIPTION : Fonction qui permet de deployer la main tourneuse */
+/****************************************************************************************/
+void Tourner_modules(void){
+ multiple2_myAX12 -> multiple_goal_and_speed(2,TAB11);
+ wait(1.5);
+ multiple2_myAX12 -> multiple_goal_and_speed(2,TAB12);
+}
-}
+