CRAC Team
Programme d'utilisation servomotors MX12 V1
Diff: main.cpp
- Revision:
- 0:80df663dd15e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 19 14:32:14 2017 +0000
@@ -0,0 +1,391 @@
+/**************************************************************************************************/
+/* cette fonction de controle de l'MX12 est réalisée à partir de la même bibliothèque que les AX12*/
+/**************************************************************************************************/
+
+#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 *un_myMX12;
+
+
+ /* 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 */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: bouger_MX12 */
+/* DESCRIPTION : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge */
+/****************************************************************************************/
+void bouger_MX12(unsigned char choix);
+
+Timer t;
+Ticker flipper;
+
+
+ /* ANGLE */
+
+/* 0 = tourner à droite
+ 600 = position initiale
+ 1200 = tourner à gauche
+ */
+
+ /* NUMERO MX12 : 0x01 */
+
+
+ /* 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); // 0x96 Trie des messages CAN sur les paramètres suivants.
+ CAN2_wrFilter(SERVO_AX12_ACK); // 0x106
+ CAN2_wrFilter(SERVO_AX12_END); // 0x116
+ CAN2_wrFilter(CHECK_AX12); // 0x65
+
+ // Set l'ID du MX12 et la vitesse de communication. Paramètres : tx rx id baud
+ un_myMX12 = new AX12(p9, p10, 1, 1000000); //Trappe de fermeture du lanceur.
+
+ while(true) {
+ AX12_automate();// Trie les actions de l'MX12
+ 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(); // Set la structure d'un message CAN Tx : id, data, taille, type, format
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+
+ if(FIFO_occupation<0)
+ FIFO_occupation=FIFO_occupation+SIZE_FIFO; //Taille du CANRx.
+
+ if(FIFO_max_occupation<FIFO_occupation)
+ FIFO_max_occupation=FIFO_occupation;
+
+ if(FIFO_occupation!=0) {
+
+ switch(msgRxBuffer[FIFO_lecture].id) {
+ case CHECK_AX12: // 0x065
+ SendRawId(ALIVE_AX12);
+ flag = 1;
+ break;
+
+ case SERVO_AX12_ACTION : // 0x96
+ etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
+
+ //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'envoyer une trame vide à un ID */
+/****************************************************************************************/
+void SendRawId (unsigned short id)
+{
+ CANMessage msgTx=CANMessage();
+ msgTx.id=id;
+ msgTx.len=0;
+ can1.write(msgTx);
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation */
+/* DESCRIPTION : Place le MX12 en position initiale : lanceur fermé */
+/****************************************************************************************/
+void Initialisation(){
+ un_myMX12-> Set_Goal_speed(vitesse);
+ un_myMX12-> Set_Mode(0);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: bouger_MX12 */
+/* DESCRIPTION : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge */
+/****************************************************************************************/
+void bouger_MX12(unsigned char choix){
+ AX12 *ptr_myAX12;
+ ptr_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects
+ if( choix == 1) // Tourne à droite
+ {
+ ptr_myAX12->Set_Secure_Goal(0); // tourner droite
+ }
+ else if(choix == 2) // Tourne à gauche
+ {
+ ptr_myAX12->Set_Secure_Goal(1200); // tourner gauche
+ }
+ else if (choix == 0)
+ {
+ ptr_myAX12->Set_Secure_Goal(600); // position initiale
+ }
+}
+
+/****************************************************************************************/
+/* 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); // Envoie le message via le CAN.
+}
+
+/****************************************************************************************/
+/* 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 : // Etat = 0
+ if (flag == 1){ // Décide si il faut initialiser le bras droit ou gauche
+ Initialisation();
+ flag = 2;
+ }
+ break;
+
+ case AX12_PREPARATION_PRISE : // Etat = 1
+ bouger_MX12(msgRxBuffer[FIFO_lecture].data[1]);
+ if (action == 0){
+ Fin_action();
+ 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 de l'MX12 */
+/****************************************************************************************/
+void Check_positionAX12(char* TAB, unsigned char choix){
+ static float TAB_POS_TH[4];
+
+ CANMessage msgTx=CANMessage();
+ msgTx.id=SERVO_AX12_POSITION;
+ msgTx.len=5;
+
+ //PERMET DE VERIFIER LA POSITION De l'MX12
+ TAB_ANGLE1[0] = (unsigned short)(un_myMX12->Get_Position()/0.3);
+
+
+ TAB_POS_TH[0] = (unsigned short) TAB[0] + ((unsigned short)TAB[1]<<8);
+
+ if (choix == 1){
+ if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
+ TAB_POSITION[0] = 1;
+ }
+ else if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
+ TAB_POSITION[0] = 0;
+ }
+ }
+ else if (choix == 2){
+ if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
+ TAB_POSITION[0] = 1;
+ }
+ else if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
+ TAB_POSITION[0] = 0;
+ }
+ }
+
+ msgTx.data[0] = choix;
+ msgTx.data[0] = TAB_POSITION[0];
+ can1.write(msgTx);
+}
+
\ No newline at end of file