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
CAN.cpp@2:9d280856a536, 2017-05-19 (annotated)
- Committer:
- matthieuvignon
- Date:
- Fri May 19 08:06:49 2017 +0000
- Revision:
- 2:9d280856a536
Programme AX12 + verification position
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
matthieuvignon | 2:9d280856a536 | 1 | #include "all_includes.h" |
matthieuvignon | 2:9d280856a536 | 2 | |
matthieuvignon | 2:9d280856a536 | 3 | extern CANMessage msgRxBuffer[SIZE_FIFO]; |
matthieuvignon | 2:9d280856a536 | 4 | extern CAN can; |
matthieuvignon | 2:9d280856a536 | 5 | extern DigitalOut led2; |
matthieuvignon | 2:9d280856a536 | 6 | extern void GetPositionAx12(void); |
matthieuvignon | 2:9d280856a536 | 7 | |
matthieuvignon | 2:9d280856a536 | 8 | unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN |
matthieuvignon | 2:9d280856a536 | 9 | signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN |
matthieuvignon | 2:9d280856a536 | 10 | |
matthieuvignon | 2:9d280856a536 | 11 | unsigned char FlagAx12 = 0; |
matthieuvignon | 2:9d280856a536 | 12 | |
matthieuvignon | 2:9d280856a536 | 13 | |
matthieuvignon | 2:9d280856a536 | 14 | /*********************************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 15 | /* FUNCTION NAME: SendRawId */ |
matthieuvignon | 2:9d280856a536 | 16 | /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ |
matthieuvignon | 2:9d280856a536 | 17 | /*********************************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 18 | void SendRawId (unsigned short id) |
matthieuvignon | 2:9d280856a536 | 19 | { |
matthieuvignon | 2:9d280856a536 | 20 | CANMessage msgTx=CANMessage(); |
matthieuvignon | 2:9d280856a536 | 21 | msgTx.id=id; |
matthieuvignon | 2:9d280856a536 | 22 | msgTx.len=0; |
matthieuvignon | 2:9d280856a536 | 23 | can.write(msgTx); |
matthieuvignon | 2:9d280856a536 | 24 | wait_us(50); |
matthieuvignon | 2:9d280856a536 | 25 | } |
matthieuvignon | 2:9d280856a536 | 26 | |
matthieuvignon | 2:9d280856a536 | 27 | |
matthieuvignon | 2:9d280856a536 | 28 | /*********************************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 29 | /* FUNCTION NAME: canRx_ISR */ |
matthieuvignon | 2:9d280856a536 | 30 | /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */ |
matthieuvignon | 2:9d280856a536 | 31 | /*********************************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 32 | void canRx_ISR (void) |
matthieuvignon | 2:9d280856a536 | 33 | { |
matthieuvignon | 2:9d280856a536 | 34 | if (can.read(msgRxBuffer[FIFO_ecriture])) |
matthieuvignon | 2:9d280856a536 | 35 | FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; |
matthieuvignon | 2:9d280856a536 | 36 | } |
matthieuvignon | 2:9d280856a536 | 37 | |
matthieuvignon | 2:9d280856a536 | 38 | |
matthieuvignon | 2:9d280856a536 | 39 | |
matthieuvignon | 2:9d280856a536 | 40 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 41 | /* FUNCTION NAME: canProcessRx */ |
matthieuvignon | 2:9d280856a536 | 42 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
matthieuvignon | 2:9d280856a536 | 43 | /****************************************************************************************/ |
matthieuvignon | 2:9d280856a536 | 44 | void canProcessRx(void){ |
matthieuvignon | 2:9d280856a536 | 45 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
matthieuvignon | 2:9d280856a536 | 46 | CANMessage msgTx=CANMessage(); |
matthieuvignon | 2:9d280856a536 | 47 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
matthieuvignon | 2:9d280856a536 | 48 | if(FIFO_occupation<0) |
matthieuvignon | 2:9d280856a536 | 49 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
matthieuvignon | 2:9d280856a536 | 50 | if(FIFO_max_occupation<FIFO_occupation) |
matthieuvignon | 2:9d280856a536 | 51 | FIFO_max_occupation=FIFO_occupation; |
matthieuvignon | 2:9d280856a536 | 52 | if(FIFO_occupation!=0) { |
matthieuvignon | 2:9d280856a536 | 53 | |
matthieuvignon | 2:9d280856a536 | 54 | switch(msgRxBuffer[FIFO_lecture].id) { |
matthieuvignon | 2:9d280856a536 | 55 | |
matthieuvignon | 2:9d280856a536 | 56 | case SERVOVANNE: |
matthieuvignon | 2:9d280856a536 | 57 | EtatServoVanne = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 58 | break; |
matthieuvignon | 2:9d280856a536 | 59 | |
matthieuvignon | 2:9d280856a536 | 60 | case POMPE_DROITE: |
matthieuvignon | 2:9d280856a536 | 61 | ActionPompe = 1; |
matthieuvignon | 2:9d280856a536 | 62 | EtatPompeDroite = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 63 | break; |
matthieuvignon | 2:9d280856a536 | 64 | |
matthieuvignon | 2:9d280856a536 | 65 | case POMPE_GAUCHE: |
matthieuvignon | 2:9d280856a536 | 66 | ActionPompe = 1; |
matthieuvignon | 2:9d280856a536 | 67 | EtatPompeGauche = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 68 | break; |
matthieuvignon | 2:9d280856a536 | 69 | |
matthieuvignon | 2:9d280856a536 | 70 | case TURBINE: |
matthieuvignon | 2:9d280856a536 | 71 | EtatTurbine = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 72 | break; |
matthieuvignon | 2:9d280856a536 | 73 | |
matthieuvignon | 2:9d280856a536 | 74 | case LANCEUR: |
matthieuvignon | 2:9d280856a536 | 75 | EtatLanceur = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 76 | break; |
matthieuvignon | 2:9d280856a536 | 77 | |
matthieuvignon | 2:9d280856a536 | 78 | case CHECK_AX12: |
matthieuvignon | 2:9d280856a536 | 79 | SendRawId(ALIVE_AX12); |
matthieuvignon | 2:9d280856a536 | 80 | FlagAx12 = 1; |
matthieuvignon | 2:9d280856a536 | 81 | break; |
matthieuvignon | 2:9d280856a536 | 82 | |
matthieuvignon | 2:9d280856a536 | 83 | case SERVO_AX12_ACTION : |
matthieuvignon | 2:9d280856a536 | 84 | |
matthieuvignon | 2:9d280856a536 | 85 | ActionAx12=1; |
matthieuvignon | 2:9d280856a536 | 86 | EtatAx12 = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 87 | ChoixBras = msgRxBuffer[FIFO_lecture].data[1]; |
matthieuvignon | 2:9d280856a536 | 88 | |
matthieuvignon | 2:9d280856a536 | 89 | //ACK de reception des actions a effectuer |
matthieuvignon | 2:9d280856a536 | 90 | msgTx.id = SERVO_AX12_ACK; |
matthieuvignon | 2:9d280856a536 | 91 | msgTx.len = 1; |
matthieuvignon | 2:9d280856a536 | 92 | msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; |
matthieuvignon | 2:9d280856a536 | 93 | can.write(msgTx); |
matthieuvignon | 2:9d280856a536 | 94 | break; |
matthieuvignon | 2:9d280856a536 | 95 | |
matthieuvignon | 2:9d280856a536 | 96 | case 0x123: |
matthieuvignon | 2:9d280856a536 | 97 | SendRawId(100); |
matthieuvignon | 2:9d280856a536 | 98 | GetPositionAx12(); |
matthieuvignon | 2:9d280856a536 | 99 | break; |
matthieuvignon | 2:9d280856a536 | 100 | |
matthieuvignon | 2:9d280856a536 | 101 | } |
matthieuvignon | 2:9d280856a536 | 102 | action_a_effectuer=1; |
matthieuvignon | 2:9d280856a536 | 103 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
matthieuvignon | 2:9d280856a536 | 104 | } |
matthieuvignon | 2:9d280856a536 | 105 | } |
matthieuvignon | 2:9d280856a536 | 106 | |
matthieuvignon | 2:9d280856a536 | 107 | |
matthieuvignon | 2:9d280856a536 | 108 | |
matthieuvignon | 2:9d280856a536 | 109 | |
matthieuvignon | 2:9d280856a536 | 110 | void CAN2_wrFilter (uint32_t id) { |
matthieuvignon | 2:9d280856a536 | 111 | static int CAN_std_cnt = 0; |
matthieuvignon | 2:9d280856a536 | 112 | uint32_t buf0, buf1; |
matthieuvignon | 2:9d280856a536 | 113 | int cnt1, cnt2, bound1; |
matthieuvignon | 2:9d280856a536 | 114 | |
matthieuvignon | 2:9d280856a536 | 115 | /* Acceptance Filter Memory full */ |
matthieuvignon | 2:9d280856a536 | 116 | if (((CAN_std_cnt + 1) >> 1) >= 512) |
matthieuvignon | 2:9d280856a536 | 117 | return; /* error: objects full */ |
matthieuvignon | 2:9d280856a536 | 118 | |
matthieuvignon | 2:9d280856a536 | 119 | /* Setup Acceptance Filter Configuration |
matthieuvignon | 2:9d280856a536 | 120 | Acceptance Filter Mode Register = Off */ |
matthieuvignon | 2:9d280856a536 | 121 | LPC_CANAF->AFMR = 0x00000001; |
matthieuvignon | 2:9d280856a536 | 122 | |
matthieuvignon | 2:9d280856a536 | 123 | id |= 1 << 13; /* Add controller number(2) */ |
matthieuvignon | 2:9d280856a536 | 124 | id &= 0x0000F7FF; /* Mask out 16-bits of ID */ |
matthieuvignon | 2:9d280856a536 | 125 | |
matthieuvignon | 2:9d280856a536 | 126 | if (CAN_std_cnt == 0) { /* For entering first ID */ |
matthieuvignon | 2:9d280856a536 | 127 | LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16); |
matthieuvignon | 2:9d280856a536 | 128 | } else if (CAN_std_cnt == 1) { /* For entering second ID */ |
matthieuvignon | 2:9d280856a536 | 129 | if ((LPC_CANAF_RAM->mask[0] >> 16) > id) |
matthieuvignon | 2:9d280856a536 | 130 | LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16); |
matthieuvignon | 2:9d280856a536 | 131 | else |
matthieuvignon | 2:9d280856a536 | 132 | LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id; |
matthieuvignon | 2:9d280856a536 | 133 | } else { |
matthieuvignon | 2:9d280856a536 | 134 | /* Find where to insert new ID */ |
matthieuvignon | 2:9d280856a536 | 135 | cnt1 = 0; |
matthieuvignon | 2:9d280856a536 | 136 | cnt2 = CAN_std_cnt; |
matthieuvignon | 2:9d280856a536 | 137 | bound1 = (CAN_std_cnt - 1) >> 1; |
matthieuvignon | 2:9d280856a536 | 138 | while (cnt1 <= bound1) { /* Loop through standard existing IDs */ |
matthieuvignon | 2:9d280856a536 | 139 | if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id) { |
matthieuvignon | 2:9d280856a536 | 140 | cnt2 = cnt1 * 2; |
matthieuvignon | 2:9d280856a536 | 141 | break; |
matthieuvignon | 2:9d280856a536 | 142 | } |
matthieuvignon | 2:9d280856a536 | 143 | if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id) { |
matthieuvignon | 2:9d280856a536 | 144 | cnt2 = cnt1 * 2 + 1; |
matthieuvignon | 2:9d280856a536 | 145 | break; |
matthieuvignon | 2:9d280856a536 | 146 | } |
matthieuvignon | 2:9d280856a536 | 147 | cnt1++; /* cnt1 = U32 where to insert new ID */ |
matthieuvignon | 2:9d280856a536 | 148 | } /* cnt2 = U16 where to insert new ID */ |
matthieuvignon | 2:9d280856a536 | 149 | |
matthieuvignon | 2:9d280856a536 | 150 | if (cnt1 > bound1) { /* Adding ID as last entry */ |
matthieuvignon | 2:9d280856a536 | 151 | if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */ |
matthieuvignon | 2:9d280856a536 | 152 | LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16); |
matthieuvignon | 2:9d280856a536 | 153 | else /* Odd number of IDs exists */ |
matthieuvignon | 2:9d280856a536 | 154 | LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id; |
matthieuvignon | 2:9d280856a536 | 155 | } else { |
matthieuvignon | 2:9d280856a536 | 156 | buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */ |
matthieuvignon | 2:9d280856a536 | 157 | if ((cnt2 & 0x0001) == 0) /* Insert new mask to even address */ |
matthieuvignon | 2:9d280856a536 | 158 | buf1 = (id << 16) | (buf0 >> 16); |
matthieuvignon | 2:9d280856a536 | 159 | else /* Insert new mask to odd address */ |
matthieuvignon | 2:9d280856a536 | 160 | buf1 = (buf0 & 0xFFFF0000) | id; |
matthieuvignon | 2:9d280856a536 | 161 | |
matthieuvignon | 2:9d280856a536 | 162 | LPC_CANAF_RAM->mask[cnt1] = buf1; /* Insert mask */ |
matthieuvignon | 2:9d280856a536 | 163 | |
matthieuvignon | 2:9d280856a536 | 164 | bound1 = CAN_std_cnt >> 1; |
matthieuvignon | 2:9d280856a536 | 165 | /* Move all remaining standard mask entries one place up */ |
matthieuvignon | 2:9d280856a536 | 166 | while (cnt1 < bound1) { |
matthieuvignon | 2:9d280856a536 | 167 | cnt1++; |
matthieuvignon | 2:9d280856a536 | 168 | buf1 = LPC_CANAF_RAM->mask[cnt1]; |
matthieuvignon | 2:9d280856a536 | 169 | LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16); |
matthieuvignon | 2:9d280856a536 | 170 | buf0 = buf1; |
matthieuvignon | 2:9d280856a536 | 171 | } |
matthieuvignon | 2:9d280856a536 | 172 | |
matthieuvignon | 2:9d280856a536 | 173 | if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */ |
matthieuvignon | 2:9d280856a536 | 174 | LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF); |
matthieuvignon | 2:9d280856a536 | 175 | } |
matthieuvignon | 2:9d280856a536 | 176 | } |
matthieuvignon | 2:9d280856a536 | 177 | CAN_std_cnt++; |
matthieuvignon | 2:9d280856a536 | 178 | |
matthieuvignon | 2:9d280856a536 | 179 | /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */ |
matthieuvignon | 2:9d280856a536 | 180 | buf0 = ((CAN_std_cnt + 1) >> 1) << 2; |
matthieuvignon | 2:9d280856a536 | 181 | buf1 = buf0; |
matthieuvignon | 2:9d280856a536 | 182 | |
matthieuvignon | 2:9d280856a536 | 183 | /* Setup acceptance filter pointers */ |
matthieuvignon | 2:9d280856a536 | 184 | LPC_CANAF->SFF_sa = 0; |
matthieuvignon | 2:9d280856a536 | 185 | LPC_CANAF->SFF_GRP_sa = buf0; |
matthieuvignon | 2:9d280856a536 | 186 | LPC_CANAF->EFF_sa = buf0; |
matthieuvignon | 2:9d280856a536 | 187 | LPC_CANAF->EFF_GRP_sa = buf1; |
matthieuvignon | 2:9d280856a536 | 188 | LPC_CANAF->ENDofTable = buf1; |
matthieuvignon | 2:9d280856a536 | 189 | |
matthieuvignon | 2:9d280856a536 | 190 | LPC_CANAF->AFMR = 0x00000000; /* Use acceptance filter */ |
matthieuvignon | 2:9d280856a536 | 191 | } // CAN2_wrFilter |