Programme d'utilisation des AX12 et de l'MX12 V3. 0C = action de l'MX12. (data0) 0 | 1 | 2 = position & sens de rotation
Dependencies: MX12
Fork of Utilisatio_MX12_V3 by
Diff: CAN.cpp
- Revision:
- 2:9d280856a536
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CAN.cpp Fri May 19 08:06:49 2017 +0000 @@ -0,0 +1,191 @@ +#include "all_includes.h" + +extern CANMessage msgRxBuffer[SIZE_FIFO]; +extern CAN can; +extern DigitalOut led2; +extern void GetPositionAx12(void); + +unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN +signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN + +unsigned char FlagAx12 = 0; + + +/*********************************************************************************************************/ +/* FUNCTION NAME: SendRawId */ +/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ +/*********************************************************************************************************/ +void SendRawId (unsigned short id) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=id; + msgTx.len=0; + can.write(msgTx); + wait_us(50); +} + + +/*********************************************************************************************************/ +/* FUNCTION NAME: canRx_ISR */ +/* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */ +/*********************************************************************************************************/ +void canRx_ISR (void) +{ + if (can.read(msgRxBuffer[FIFO_ecriture])) + FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; +} + + + +/****************************************************************************************/ +/* 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 SERVOVANNE: + EtatServoVanne = msgRxBuffer[FIFO_lecture].data[0]; + break; + + case POMPE_DROITE: + ActionPompe = 1; + EtatPompeDroite = msgRxBuffer[FIFO_lecture].data[0]; + break; + + case POMPE_GAUCHE: + ActionPompe = 1; + EtatPompeGauche = msgRxBuffer[FIFO_lecture].data[0]; + break; + + case TURBINE: + EtatTurbine = msgRxBuffer[FIFO_lecture].data[0]; + break; + + case LANCEUR: + EtatLanceur = msgRxBuffer[FIFO_lecture].data[0]; + break; + + case CHECK_AX12: + SendRawId(ALIVE_AX12); + FlagAx12 = 1; + break; + + case SERVO_AX12_ACTION : + + ActionAx12=1; + EtatAx12 = msgRxBuffer[FIFO_lecture].data[0]; + ChoixBras = 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]; + can.write(msgTx); + break; + + case 0x123: + SendRawId(100); + GetPositionAx12(); + break; + + } + action_a_effectuer=1; + FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; + } +} + + + + +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