CRAC Team
AX12 gros robot
Fork of
command_AX12_gros_robot
by 
CRAC Team
main.cpp
- Committer:
- SquirrelGod
- Date:
- 2017-05-11
- Revision:
- 5:740eb0f606c1
- Parent:
- 4:92d6a49cd2c5
File content as of revision 5:740eb0f606c1:
#include "mbed.h"
#include "AX12.h"
#include "cmsis.h"
#include "ident_crac.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_DEFAUT 20
#define TIME 0.8
#define TOLERANCE_AX12 50
#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
extern "C" void mbed_reset();//Pour pouvoir reset la carte
unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0;
static float TAB_ANGLE1[4], TAB_ANGLE2[4];
static char TAB_POSITION[4];
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, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
/* PROTOTYPES DE FONCTIONS ET POINTEURS */
/****************************************************************************************/
/* FUNCTION NAME: CAN2_wrFilter */
/* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */
/****************************************************************************************/
void CAN2_wrFilter (uint32_t id);
/****************************************************************************************/
/* 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: Fin_action */
/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
/****************************************************************************************/
void Fin_action(void);
/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12 */
/* 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(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);
Timer t;
Ticker flipper;
///////////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};
/* 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
CAN2_wrFilter(SERVO_AX12_ACTION);
CAN2_wrFilter(SERVO_AX12_ACK);
CAN2_wrFilter(SERVO_AX12_END);
CAN2_wrFilter(CHECK_AX12);
trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche
treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche
quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche
huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche
quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite
sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite
quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite
dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite
multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); //pince droite
multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); //pince gauche
while(true) {
AX12_automate();
canProcessRx();//Traitement des trames CAN en attente
}
}
/* 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();
msgTx.format=CANStandard;
msgTx.type=CANData;
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 CHECK_AX12:
SendRawId(ALIVE_AX12);
flag = 1;
break;
case SERVO_AX12_ACTION :
etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
choix_bras = 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];
can1.write(msgTx);
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);
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);
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: 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);
}
}
/****************************************************************************************/
/* 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;
can1.write(msgTx);
}
/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12 */
/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
/****************************************************************************************/
void AX12_automate(void){
switch(etat_ax12){
case AX12_INITIALISATION :
if (flag == 1){
Initialisation_position(1);
//Check_positionAX12(&TAB1[25], 1);
Initialisation_position(2);
//Check_positionAX12(&TAB21[25], 2);
flag = 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;
}
}
/****************************************************************************************/
/* FUNCTION NAME: CAN2_wrFilter */
/* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */
/****************************************************************************************/
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
/****************************************************************************************/
/* 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];
}
can1.write(msgTx);
}