test des capteurs/actionneurs petit robot
Fork of mbed_tes_cpt by
CAN.cpp
- Committer:
- matthieuvignon
- Date:
- 2017-05-25
- Revision:
- 5:7e1c328c5d50
- Parent:
- 3:43843ab8af47
File content as of revision 5:7e1c328c5d50:
#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) { #ifdef ARRIERE case ACKNOWLEDGE_GLOBAL_JACK: jack.fall(NULL); EnvoieJack=0; TimeJack.stop(); break; #endif #ifdef AVANT case CHECK_CARTE_AVANT: EtatCarteAvant = 1; Cote = 1; //Avant //ACK de reception des actions a effectuer msgTx.id = 0x73; msgTx.len = 0; can.write(msgTx); wait_us(50); break; #endif #ifdef ARRIERE case CHECK_CARTE_ARRIERE: EtatCarteArriere = 1; Cote = 2; //Arriere //ACK de reception des actions a effectuer msgTx.id = 0x74; msgTx.len = 0; can.write(msgTx); wait_us(50); break; #endif case GLOBAL_GAME_END: EtatGameEnd = 1; break; case GLOBAL_FUNNY_ACTION: EtatFunnyAction = msgRxBuffer[FIFO_lecture].data[0]; Cote = msgRxBuffer[FIFO_lecture].data[1]; break; case ELECTROVANNE: EtatElectroVanne = msgRxBuffer[FIFO_lecture].data[0]; Cote = msgRxBuffer[FIFO_lecture].data[1]; break; case POMPE: ActionPompe = 1; EtatPompe = msgRxBuffer[FIFO_lecture].data[0]; Cote = msgRxBuffer[FIFO_lecture].data[1]; msgTx.id = ACKNOWLEDGE_POMPES; msgTx.len = 1; msgTx.data[0] = POMPE_PWM; can.write(msgTx); wait_us(50); break; case TURBINE: EtatTurbine = msgRxBuffer[FIFO_lecture].data[0]; Cote = msgRxBuffer[FIFO_lecture].data[1]; msgTx.id = SERVO_AX12_ACK; msgTx.len = 1; msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; can.write(msgTx); break; case LANCEUR: EtatLanceur = msgRxBuffer[FIFO_lecture].data[0]; Cote = msgRxBuffer[FIFO_lecture].data[1]; break; case CHECK_AX12: SendRawId(ALIVE_AX12); FlagAx12 = 1; break; case SERVO_AX12_ACTION : ActionAx12=1; EtatAx12 = msgRxBuffer[FIFO_lecture].data[0]; Cote = 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