CRAC Team
AX12 gros robot
Fork of
command_AX12_gros_robot
by 
CRAC Team
main.cpp
- Committer:
- SquirrelGod
- Date:
- 2017-04-18
- Revision:
- 2:99b1cb0d9f5e
- Parent:
- 1:b3ff77670606
- Child:
- 3:6cbd7d2c78f4
File content as of revision 2:99b1cb0d9f5e:
#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;
///////////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]= {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};
//////////////////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 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 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
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 */
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 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);
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);
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);
}
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);
}
/****************************************************************************************/
/* 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);
}