CRAC Team
code ax12 petit robot 12/05/2017
Fork of
command_AX12_petit_robot_V3
by 
CRAC Team
Diff: main.cpp
- Revision:
- 7:ad4a19e26b84
- Parent:
- 6:c239c7abd7fe
--- a/main.cpp Thu May 11 14:21:57 2017 +0000
+++ b/main.cpp Fri May 12 14:35:09 2017 +0000
@@ -1,5 +1,7 @@
#include "mbed.h"
#include "AX12.h"
+#include "actionneurs.h"
+#include "global.h"
#include "ident_crac.h"
@@ -13,29 +15,12 @@
#define AX12_POUSSER_MODULE 7
#define AX12_DEFAUT 20
-#define TIME 0.8
-#define SIZE_FIFO 25
-
- /* 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
+
+#define SIZE_FIFO 25
+
extern "C" void mbed_reset();//Pour pouvoir reset la carte
-unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0, action_precedente = 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 *un_myAX12, *sept_myAX12, *huit_myAX12, *seize_myAX12, *multiple_myAX12; //Pince centrale
-AX12 *un_myAX12_2, *deux_myAX12_2, *trois_myAX12_2, *multiple_myAX12_2; //Bras de gauche
-
-
- /* PROTOTYPES DE FONCTIONS ET POINTEURS */
-
-
-
/****************************************************************************************/
/* FUNCTION NAME: canProcessRx */
/* DESCRIPTION : Fonction de traitement des messages CAN */
@@ -65,193 +50,21 @@
/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
/****************************************************************************************/
void AX12_automate(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_position */
-/* DESCRIPTION : Fonction qui place les bras en position verticale */
-/****************************************************************************************/
-void Initialisation_position(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise */
-/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
-/****************************************************************************************/
-void Preparation_prise(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut */
-/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
-/****************************************************************************************/
-void Stockage_haut(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas */
-/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
-/****************************************************************************************/
-void Stockage_bas(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer */
-/* DESCRIPTION : Fonction qui permet de déposer un module */
-/****************************************************************************************/
-void Deposer(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_bas */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
-/****************************************************************************************/
-void Preparation_depot_bas(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_haut */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
-/****************************************************************************************/
-void Preparation_depot_haut(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module */
-/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
-/****************************************************************************************/
-void Pousser_module(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_gauche */
-/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
-/****************************************************************************************/
-void Initialisation_gauche(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise_gauche */
-/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
-/****************************************************************************************/
-void Preparation_prise_gauche(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Prendre_module_gauche */
-/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
-/****************************************************************************************/
-void Prendre_module_gauche(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Tourner_module_gauche */
-/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
-/****************************************************************************************/
-void Tourner_module_gauche(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparatio_module_gauche */
-/* DESCRIPTION : Fonction qui prepare le tournante */
-/****************************************************************************************/
-void Preparation_module_gauche(void);
-
-Timer t;
-Ticker flipper;
-
-////////////////////// TABLEAU PINCE CENTRALE ///////////////////////////
-static char TAB1[25]= {0x01,0x05, 0x02, 0xFF, 0x00, ///Position initiale
- 0x10,0x00, 0x02, 0xFF, 0x00,
- 0x07,0x00, 0x01, 0xFF, 0x00,
- 0x08,0x00, 0x03, 0xFF, 0x00};
+
+
+ /* 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
-static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise
- 0x10,0x50, 0x01, 0xFF, 0x03,
- 0x07,0xF4, 0x01, 0xFF, 0x03,
- 0x08,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee)
- 0x10,0x50, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
-static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air)
- 0x10,0x50, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
- 0x10,0xF4, 0x01, 0xFF, 0x00,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB6[25]= {0x01,0xB0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x10,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0x00, 0x01, 0xFF, 0x03,
- 0x08,0x00, 0x03, 0xFF, 0x03};
-
-static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air)
- 0x10,0xB0, 0x00, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x00,
- 0x08,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas
- 0x10,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x00,
- 0x08,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer
- 0x10,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0xD0, 0x00, 0xFF, 0x00,
- 0x08,0x35, 0x03, 0xFF, 0x00};
-
-static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
- 0x10,0x00, 0x01, 0xFF, 0x00,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module
- 0x10,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x00,
- 0x08,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale
- 0x10,0x00, 0x02, 0xFF, 0x03,
- 0x07,0xF4, 0x01, 0xFF, 0x03,
- 0x08,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer
- 0x10,0xA0, 0x02, 0xFF, 0x03,
- 0x07,0xD0, 0x00, 0xFF, 0x00,
- 0x08,0x35, 0x03, 0xFF, 0x00};
-
-////////////////////// TABLEAU BRAS GAUCHE ///////////////////////////
-static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale
- 0x02,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner
- 0x02,0x40, 0x03, 0xFF, 0x03};
-
-static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module
- 0x02,0xE5, 0x02, 0xFF, 0x03};
-
-
-
-
- /* 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 */
-
-
+unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 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;
+
/* MAIN */
@@ -260,45 +73,10 @@
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
- //Pince centrale
- un_myAX12 = new AX12(p9, p10, 1, 1000000);
- sept_myAX12 = new AX12(p9, p10, 7, 1000000);
- huit_myAX12 = new AX12(p9, p10, 8, 1000000);
- seize_myAX12 = new AX12(p9, p10, 16, 1000000);
- multiple_myAX12 = new AX12(p9,p10,0xFE,1000000);
-
- //Bras de gauche
- un_myAX12_2 = new AX12(p13, p14, 1, 1000000);
- deux_myAX12_2 = new AX12(p13, p14, 2, 1000000);
- trois_myAX12_2 = new AX12(p13, p14, 3, 1000000);
- multiple_myAX12_2 = new AX12(p13,p14,0xFE,1000000);
-
- //Demonstration pince centrale
- /*
- Initialisation_position();
- wait(1);
- Preparation_prise();
- wait(1);
- Stockage_haut();
- wait(1);
- Preparation_prise();
- wait(1);
- Stockage_bas();
- wait(1);
- Pousser_module();
- wait(1);
- Preparation_depot_bas();
- wait(1);
- Deposer();
- wait(1);
- Preparation_depot_haut();
- wait(1);
- Deposer();
- wait(1);
- */
+ declarationAX12();
//Demonstration bras gauche
- Initialisation_gauche();
+ /*Initialisation_gauche();
wait(1);
Preparation_prise_gauche();
wait(1);
@@ -314,10 +92,12 @@
wait(1);
Preparation_module_gauche();
wait(1);
- Tourner_module_gauche();
-
+ Tourner_module_gauche();*/
+ float position =0;
while(true) {
//AX12_automate();
+ canProcessRx();
+
}
}
@@ -348,6 +128,7 @@
case CHECK_AX12:
SendRawId(ALIVE_AX12);
flag = 1;
+ mbed_reset();
break;
case SERVO_AX12_ACTION :
@@ -359,6 +140,47 @@
msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
can1.write(msgTx);
break;
+ case 0x123:
+ SendRawId(ALIVE_AX12);
+ if(msgRxBuffer[FIFO_lecture].data[0] == 0){
+ getPosiotionCentrale();
+ }else if(msgRxBuffer[FIFO_lecture].data[0] == 1){
+ getPosiotionGauche();
+ }else{
+ getPosiotionDroite();
+ }
+ break;
+ case 0x456:
+ //Demonstration pince centrale
+ switch(msgRxBuffer[FIFO_lecture].data[0]){
+ case 0:
+ Initialisation_position();
+ break;
+ case 1:
+ Preparation_prise();
+ break;
+ case 2:
+ Stockage_haut();
+ break;
+ case 3:
+ Stockage_bas();
+ break;
+ case 4:
+ Pousser_module();
+ break;
+ case 5:
+ Preparation_depot_bas();
+ break;
+ case 6:
+ Deposer();
+ break;
+ case 7:
+ Preparation_depot_haut();
+ break;
+
+ }
+ wait(1);
+ break;
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
@@ -372,7 +194,7 @@
void canRx_ISR (void)
{
if (can1.read(msgRxBuffer[FIFO_ecriture])) {
- if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
+ if(msgRxBuffer[FIFO_ecriture].id==0x65) mbed_reset();
else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
}
}
@@ -388,163 +210,8 @@
msgTx.len=0;
can1.write(msgTx);
}
- void Init_AX12() // Initialisation des différents paramétres
-{
- un_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- sept_myAX12-> Set_Goal_speed(vitesse);
- huit_myAX12-> Set_Goal_speed(vitesse);
- seize_myAX12-> Set_Goal_speed(vitesse);
-
- un_myAX12-> Set_Mode(0);
- sept_myAX12-> Set_Mode(0);
- huit_myAX12-> Set_Mode(0);
- seize_myAX12-> Set_Mode(0);
-}
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_position */
-/* DESCRIPTION : Fonction qui place les bras en position verticale */
-/****************************************************************************************/
-void Initialisation_position(void){
- multiple_myAX12->multiple_goal_and_speed(4,TAB12);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB1);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise */
-/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
-/****************************************************************************************/
-void Preparation_prise(void){
- if (action_precedente == 0){
- multiple_myAX12->multiple_goal_and_speed(4,TAB12);
- wait(TIME);
- action_precedente = 1;
- }
- multiple_myAX12->multiple_goal_and_speed(4,TAB2);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut */
-/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
-/****************************************************************************************/
-void Stockage_haut(void){
- 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);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas */
-/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
-/****************************************************************************************/
-void Stockage_bas(void){
- multiple_myAX12->multiple_goal_and_speed(4,TAB3);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB7);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer */
-/* DESCRIPTION : Fonction qui permet de déposer le module */
-/****************************************************************************************/
-void Deposer(void){
- multiple_myAX12->multiple_goal_and_speed(4,TAB9);
- wait(TIME/5);
- multiple_myAX12->multiple_goal_and_speed(4,TAB8);
- wait(TIME/5);
- multiple_myAX12->multiple_goal_and_speed(4,TAB13);
- wait(TIME/5);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_bas */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
-/****************************************************************************************/
-void Preparation_depot_bas(void){
- multiple_myAX12->multiple_goal_and_speed(4,TAB8);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_haut */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
-/****************************************************************************************/
-void Preparation_depot_haut(void){
- 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);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module */
-/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
-/****************************************************************************************/
-void Pousser_module(void){
- multiple_myAX12->multiple_goal_and_speed(4,TAB11);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_gauche */
-/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
-/****************************************************************************************/
-void Initialisation_gauche(void){
- trois_myAX12_2->Set_Secure_Goal(235);
- multiple_myAX12_2->multiple_goal_and_speed(4,TAB22);
- wait(TIME);
- multiple_myAX12_2->multiple_goal_and_speed(4,TAB21);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise_gauche */
-/* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */
-/****************************************************************************************/
-void Preparation_prise_gauche(void){
- trois_myAX12_2->Set_Secure_Goal(120);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Prendre_module_gauche */
-/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
-/****************************************************************************************/
-void Prendre_module_gauche(void){
- trois_myAX12_2->Set_Secure_Goal(160);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparatio_module_gauche */
-/* DESCRIPTION : Fonction qui prepare le tournante */
-/****************************************************************************************/
-void Preparation_module_gauche(void){
- multiple_myAX12_2->multiple_goal_and_speed(4,TAB22);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Tourner_module_gauche */
-/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
-/****************************************************************************************/
-void Tourner_module_gauche(void){
- multiple_myAX12_2->multiple_goal_and_speed(4,TAB23);
- wait(TIME);
-}
-
/****************************************************************************************/
/* FUNCTION NAME: Fin_action */