test des capteurs/actionneurs petit robot
Fork of mbed_tes_cpt by
main.cpp@1:f3f702086a30, 2017-05-11 (annotated)
- Committer:
- ClementBreteau
- Date:
- Thu May 11 11:49:50 2017 +0000
- Revision:
- 1:f3f702086a30
- Parent:
- 0:7737d7573e3b
- Child:
- 2:9d280856a536
test de la carte ? tout faire du petit robot
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ClementBreteau | 0:7737d7573e3b | 1 | #include "mbed.h" |
ClementBreteau | 1:f3f702086a30 | 2 | #include "AX12.h" |
ClementBreteau | 1:f3f702086a30 | 3 | #include "cmsis.h" |
ClementBreteau | 0:7737d7573e3b | 4 | |
ClementBreteau | 0:7737d7573e3b | 5 | #define SIZE_FIFO 20 |
ClementBreteau | 1:f3f702086a30 | 6 | #define TIME 1 |
ClementBreteau | 1:f3f702086a30 | 7 | #define T_MOT 0.00005 |
ClementBreteau | 0:7737d7573e3b | 8 | |
ClementBreteau | 0:7737d7573e3b | 9 | /* |
ClementBreteau | 0:7737d7573e3b | 10 | DigitalIn IO1(p23); |
ClementBreteau | 0:7737d7573e3b | 11 | DigitalIn IO2(p24); |
ClementBreteau | 0:7737d7573e3b | 12 | DigitalIn IO3(p25); |
ClementBreteau | 0:7737d7573e3b | 13 | DigitalIn IO4(p26); |
ClementBreteau | 0:7737d7573e3b | 14 | |
ClementBreteau | 0:7737d7573e3b | 15 | AnalogIn A_in1(p15); |
ClementBreteau | 0:7737d7573e3b | 16 | AnalogIn A_in2(p16); |
ClementBreteau | 0:7737d7573e3b | 17 | AnalogIn A_in3(p17); |
ClementBreteau | 0:7737d7573e3b | 18 | AnalogIn A_in4(p18); |
ClementBreteau | 0:7737d7573e3b | 19 | AnalogIn A_in5(p19); |
ClementBreteau | 0:7737d7573e3b | 20 | AnalogIn A_in6(p20); |
ClementBreteau | 0:7737d7573e3b | 21 | |
ClementBreteau | 0:7737d7573e3b | 22 | PwmOut IRL_1(p21); |
ClementBreteau | 0:7737d7573e3b | 23 | PwmOut IRL_2(p22); |
ClementBreteau | 0:7737d7573e3b | 24 | */ |
ClementBreteau | 0:7737d7573e3b | 25 | |
ClementBreteau | 0:7737d7573e3b | 26 | AnalogIn cptGauche(p20); |
ClementBreteau | 0:7737d7573e3b | 27 | AnalogIn cptDroit(p19); |
ClementBreteau | 0:7737d7573e3b | 28 | |
ClementBreteau | 0:7737d7573e3b | 29 | DigitalIn pressionGauche(p23); |
ClementBreteau | 0:7737d7573e3b | 30 | DigitalIn pressionDroit(p24); |
ClementBreteau | 0:7737d7573e3b | 31 | |
ClementBreteau | 0:7737d7573e3b | 32 | AnalogIn telemetre(p15); |
ClementBreteau | 0:7737d7573e3b | 33 | |
ClementBreteau | 0:7737d7573e3b | 34 | DigitalIn jack(p25); |
ClementBreteau | 0:7737d7573e3b | 35 | |
ClementBreteau | 1:f3f702086a30 | 36 | PwmOut motGauche(p21); |
ClementBreteau | 1:f3f702086a30 | 37 | PwmOut motDroit(p22); |
ClementBreteau | 0:7737d7573e3b | 38 | |
ClementBreteau | 0:7737d7573e3b | 39 | DigitalOut led(LED1); |
ClementBreteau | 0:7737d7573e3b | 40 | |
ClementBreteau | 0:7737d7573e3b | 41 | CAN can1(p30,p29); // Rx&Tx pour le CAN |
ClementBreteau | 0:7737d7573e3b | 42 | CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN |
ClementBreteau | 0:7737d7573e3b | 43 | unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN |
ClementBreteau | 1:f3f702086a30 | 44 | signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN |
ClementBreteau | 0:7737d7573e3b | 45 | |
ClementBreteau | 1:f3f702086a30 | 46 | AX12 *AX12_17, *AX12_14, *multiple_AX12; |
ClementBreteau | 1:f3f702086a30 | 47 | AX12 *AX12_14_2, *AX12_11, *multiple_AX12_2; |
ClementBreteau | 1:f3f702086a30 | 48 | AX12 *AX12_4, *AX12_18, *multiple_AX12_3; |
ClementBreteau | 1:f3f702086a30 | 49 | |
ClementBreteau | 1:f3f702086a30 | 50 | int printCapteurs = 1; |
ClementBreteau | 1:f3f702086a30 | 51 | |
ClementBreteau | 1:f3f702086a30 | 52 | static char TAB1[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03, |
ClementBreteau | 1:f3f702086a30 | 53 | 0x11, 0x50, 0x02, 0xFF, 0x03}; |
ClementBreteau | 1:f3f702086a30 | 54 | |
ClementBreteau | 1:f3f702086a30 | 55 | static char TAB2[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03, |
ClementBreteau | 1:f3f702086a30 | 56 | 0x11, 0x00, 0x00, 0xFF, 0x03}; |
ClementBreteau | 1:f3f702086a30 | 57 | |
ClementBreteau | 1:f3f702086a30 | 58 | static char TAB3[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03, |
ClementBreteau | 1:f3f702086a30 | 59 | 0x0B, 0x50, 0x02, 0xFF, 0x03}; |
ClementBreteau | 1:f3f702086a30 | 60 | |
ClementBreteau | 1:f3f702086a30 | 61 | static char TAB4[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03, |
ClementBreteau | 1:f3f702086a30 | 62 | 0x0B, 0x00, 0x00, 0xFF, 0x03}; |
ClementBreteau | 1:f3f702086a30 | 63 | |
ClementBreteau | 1:f3f702086a30 | 64 | static char TAB5[10]= {0x04, 0x00, 0x00, 0xFF, 0x03, |
ClementBreteau | 1:f3f702086a30 | 65 | 0x12, 0x50, 0x02, 0xFF, 0x03}; |
ClementBreteau | 1:f3f702086a30 | 66 | |
ClementBreteau | 1:f3f702086a30 | 67 | static char TAB6[10]= {0x04, 0x50, 0x02, 0xFF, 0x03, |
ClementBreteau | 1:f3f702086a30 | 68 | 0x12, 0x00, 0x00, 0xFF, 0x03}; |
ClementBreteau | 1:f3f702086a30 | 69 | |
ClementBreteau | 1:f3f702086a30 | 70 | Timer t; |
ClementBreteau | 1:f3f702086a30 | 71 | Ticker flipper; |
ClementBreteau | 1:f3f702086a30 | 72 | |
ClementBreteau | 0:7737d7573e3b | 73 | |
ClementBreteau | 0:7737d7573e3b | 74 | void canRx_ISR (void) |
ClementBreteau | 0:7737d7573e3b | 75 | { |
ClementBreteau | 0:7737d7573e3b | 76 | if (can1.read(msgRxBuffer[FIFO_ecriture])) { |
ClementBreteau | 0:7737d7573e3b | 77 | /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset(); |
ClementBreteau | 0:7737d7573e3b | 78 | else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/ |
ClementBreteau | 0:7737d7573e3b | 79 | FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; |
ClementBreteau | 0:7737d7573e3b | 80 | } |
ClementBreteau | 0:7737d7573e3b | 81 | } |
ClementBreteau | 0:7737d7573e3b | 82 | |
ClementBreteau | 0:7737d7573e3b | 83 | |
ClementBreteau | 0:7737d7573e3b | 84 | |
ClementBreteau | 0:7737d7573e3b | 85 | /*********************************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 86 | /* FUNCTION NAME: SendRawId */ |
ClementBreteau | 0:7737d7573e3b | 87 | /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ |
ClementBreteau | 0:7737d7573e3b | 88 | /*********************************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 89 | void SendRawId (unsigned short id) |
ClementBreteau | 0:7737d7573e3b | 90 | { |
ClementBreteau | 0:7737d7573e3b | 91 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 0:7737d7573e3b | 92 | msgTx.id=id; |
ClementBreteau | 0:7737d7573e3b | 93 | msgTx.len=0; |
ClementBreteau | 0:7737d7573e3b | 94 | can1.write(msgTx); |
ClementBreteau | 0:7737d7573e3b | 95 | wait_us(200); |
ClementBreteau | 0:7737d7573e3b | 96 | } |
ClementBreteau | 0:7737d7573e3b | 97 | |
ClementBreteau | 1:f3f702086a30 | 98 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 99 | /* FUNCTION NAME: AX1 */ |
ClementBreteau | 1:f3f702086a30 | 100 | /* DESCRIPTION : bouge les AX12 */ |
ClementBreteau | 1:f3f702086a30 | 101 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 102 | void AX1(void){ |
ClementBreteau | 1:f3f702086a30 | 103 | multiple_AX12->multiple_goal_and_speed(2,TAB1); |
ClementBreteau | 1:f3f702086a30 | 104 | wait(TIME); |
ClementBreteau | 1:f3f702086a30 | 105 | multiple_AX12->multiple_goal_and_speed(2,TAB2); |
ClementBreteau | 1:f3f702086a30 | 106 | wait(TIME); |
ClementBreteau | 1:f3f702086a30 | 107 | } |
ClementBreteau | 1:f3f702086a30 | 108 | |
ClementBreteau | 1:f3f702086a30 | 109 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 110 | /* FUNCTION NAME: AX2 */ |
ClementBreteau | 1:f3f702086a30 | 111 | /* DESCRIPTION : bouge les AX12 */ |
ClementBreteau | 1:f3f702086a30 | 112 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 113 | void AX2(void){ |
ClementBreteau | 1:f3f702086a30 | 114 | multiple_AX12_2->multiple_goal_and_speed(2,TAB3); |
ClementBreteau | 1:f3f702086a30 | 115 | wait(TIME); |
ClementBreteau | 1:f3f702086a30 | 116 | multiple_AX12_2->multiple_goal_and_speed(2,TAB4); |
ClementBreteau | 1:f3f702086a30 | 117 | wait(TIME); |
ClementBreteau | 1:f3f702086a30 | 118 | } |
ClementBreteau | 1:f3f702086a30 | 119 | |
ClementBreteau | 1:f3f702086a30 | 120 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 121 | /* FUNCTION NAME: AX3 */ |
ClementBreteau | 1:f3f702086a30 | 122 | /* DESCRIPTION : bouge les AX12 */ |
ClementBreteau | 1:f3f702086a30 | 123 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 124 | void AX3(void){ |
ClementBreteau | 1:f3f702086a30 | 125 | multiple_AX12_3->multiple_goal_and_speed(2,TAB5); |
ClementBreteau | 1:f3f702086a30 | 126 | wait(TIME); |
ClementBreteau | 1:f3f702086a30 | 127 | multiple_AX12_3->multiple_goal_and_speed(2,TAB6); |
ClementBreteau | 1:f3f702086a30 | 128 | wait(TIME); |
ClementBreteau | 1:f3f702086a30 | 129 | } |
ClementBreteau | 1:f3f702086a30 | 130 | |
ClementBreteau | 1:f3f702086a30 | 131 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 132 | /* FUNCTION NAME: moteur */ |
ClementBreteau | 1:f3f702086a30 | 133 | /* DESCRIPTION : bouge les moteurs */ |
ClementBreteau | 1:f3f702086a30 | 134 | /*********************************************************************************************************/ |
ClementBreteau | 1:f3f702086a30 | 135 | void moteur(void){ |
ClementBreteau | 1:f3f702086a30 | 136 | motGauche.write(0.5f); |
ClementBreteau | 1:f3f702086a30 | 137 | motDroit.write(0.5f); |
ClementBreteau | 1:f3f702086a30 | 138 | } |
ClementBreteau | 1:f3f702086a30 | 139 | |
ClementBreteau | 1:f3f702086a30 | 140 | |
ClementBreteau | 0:7737d7573e3b | 141 | /****************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 142 | /* FUNCTION NAME: canProcessRx */ |
ClementBreteau | 0:7737d7573e3b | 143 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
ClementBreteau | 0:7737d7573e3b | 144 | /****************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 145 | void canProcessRx(void){ |
ClementBreteau | 0:7737d7573e3b | 146 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
ClementBreteau | 0:7737d7573e3b | 147 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 0:7737d7573e3b | 148 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
ClementBreteau | 0:7737d7573e3b | 149 | if(FIFO_occupation<0) |
ClementBreteau | 0:7737d7573e3b | 150 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
ClementBreteau | 0:7737d7573e3b | 151 | if(FIFO_max_occupation<FIFO_occupation) |
ClementBreteau | 0:7737d7573e3b | 152 | FIFO_max_occupation=FIFO_occupation; |
ClementBreteau | 0:7737d7573e3b | 153 | if(FIFO_occupation!=0) { |
ClementBreteau | 0:7737d7573e3b | 154 | |
ClementBreteau | 0:7737d7573e3b | 155 | switch(msgRxBuffer[FIFO_lecture].id) { |
ClementBreteau | 0:7737d7573e3b | 156 | case 0x63: |
ClementBreteau | 0:7737d7573e3b | 157 | SendRawId(0x73); |
ClementBreteau | 0:7737d7573e3b | 158 | break; |
ClementBreteau | 0:7737d7573e3b | 159 | case 0x10: |
ClementBreteau | 0:7737d7573e3b | 160 | printCapteurs = 0; |
ClementBreteau | 0:7737d7573e3b | 161 | break; |
ClementBreteau | 0:7737d7573e3b | 162 | case 0x11: |
ClementBreteau | 0:7737d7573e3b | 163 | printCapteurs = 1; |
ClementBreteau | 0:7737d7573e3b | 164 | break; |
ClementBreteau | 0:7737d7573e3b | 165 | |
ClementBreteau | 0:7737d7573e3b | 166 | |
ClementBreteau | 0:7737d7573e3b | 167 | } |
ClementBreteau | 0:7737d7573e3b | 168 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
ClementBreteau | 0:7737d7573e3b | 169 | } |
ClementBreteau | 0:7737d7573e3b | 170 | } |
ClementBreteau | 0:7737d7573e3b | 171 | |
ClementBreteau | 0:7737d7573e3b | 172 | int main() { |
ClementBreteau | 0:7737d7573e3b | 173 | can1.frequency(1000000); // fréquence de travail 1Mbit/s |
ClementBreteau | 0:7737d7573e3b | 174 | can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN |
ClementBreteau | 0:7737d7573e3b | 175 | |
ClementBreteau | 0:7737d7573e3b | 176 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 1:f3f702086a30 | 177 | |
ClementBreteau | 1:f3f702086a30 | 178 | |
ClementBreteau | 1:f3f702086a30 | 179 | AX12_14 = new AX12(p9, p10, 14, 1000000); // tx, rx |
ClementBreteau | 1:f3f702086a30 | 180 | AX12_17 = new AX12(p9, p10, 17, 1000000); |
ClementBreteau | 1:f3f702086a30 | 181 | multiple_AX12 = new AX12(p9,p10,0xFE,1000000); |
ClementBreteau | 1:f3f702086a30 | 182 | |
ClementBreteau | 1:f3f702086a30 | 183 | |
ClementBreteau | 1:f3f702086a30 | 184 | AX12_14_2 = new AX12(p13, p14, 14, 1000000); |
ClementBreteau | 1:f3f702086a30 | 185 | AX12_11 = new AX12(p13, p14, 11, 1000000); |
ClementBreteau | 1:f3f702086a30 | 186 | multiple_AX12_2 = new AX12(p13,p14,0xFE,1000000); |
ClementBreteau | 1:f3f702086a30 | 187 | |
ClementBreteau | 1:f3f702086a30 | 188 | AX12_4= new AX12(p28, p27, 14, 1000000); |
ClementBreteau | 1:f3f702086a30 | 189 | AX12_18= new AX12(p28, p27, 11, 1000000); |
ClementBreteau | 1:f3f702086a30 | 190 | multiple_AX12_3= new AX12(p28,p27,0xFE,1000000); |
ClementBreteau | 1:f3f702086a30 | 191 | |
ClementBreteau | 1:f3f702086a30 | 192 | motGauche.period(T_MOT); |
ClementBreteau | 1:f3f702086a30 | 193 | motDroit.period(T_MOT); |
ClementBreteau | 1:f3f702086a30 | 194 | motGauche.write(0.0); |
ClementBreteau | 1:f3f702086a30 | 195 | motDroit.write(0.0); |
ClementBreteau | 1:f3f702086a30 | 196 | |
ClementBreteau | 0:7737d7573e3b | 197 | while(1) { |
ClementBreteau | 0:7737d7573e3b | 198 | led = !led; |
ClementBreteau | 0:7737d7573e3b | 199 | |
ClementBreteau | 0:7737d7573e3b | 200 | canProcessRx(); |
ClementBreteau | 1:f3f702086a30 | 201 | |
ClementBreteau | 0:7737d7573e3b | 202 | if (printCapteurs){ |
ClementBreteau | 1:f3f702086a30 | 203 | |
ClementBreteau | 1:f3f702086a30 | 204 | motGauche.write(0); |
ClementBreteau | 1:f3f702086a30 | 205 | motDroit.write(0); |
ClementBreteau | 1:f3f702086a30 | 206 | |
ClementBreteau | 0:7737d7573e3b | 207 | msgTx.id=0x21; |
ClementBreteau | 0:7737d7573e3b | 208 | msgTx.format=CANStandard; |
ClementBreteau | 0:7737d7573e3b | 209 | msgTx.type=CANData; |
ClementBreteau | 0:7737d7573e3b | 210 | msgTx.len=7; |
ClementBreteau | 0:7737d7573e3b | 211 | |
ClementBreteau | 0:7737d7573e3b | 212 | msgTx.data[0]=(unsigned char)cptGauche.read(); |
ClementBreteau | 1:f3f702086a30 | 213 | msgTx.data[1]=(unsigned char)cptDroit.read(); |
ClementBreteau | 0:7737d7573e3b | 214 | msgTx.data[2]=(unsigned char)pressionGauche.read(); |
ClementBreteau | 0:7737d7573e3b | 215 | msgTx.data[3]=(unsigned char)pressionDroit.read(); |
ClementBreteau | 0:7737d7573e3b | 216 | msgTx.data[4]=(unsigned char)jack.read(); |
ClementBreteau | 0:7737d7573e3b | 217 | msgTx.data[5]=(unsigned char)(telemetre.read_u16()>>8); |
ClementBreteau | 0:7737d7573e3b | 218 | msgTx.data[6]=(unsigned char)telemetre.read_u16(); |
ClementBreteau | 0:7737d7573e3b | 219 | |
ClementBreteau | 0:7737d7573e3b | 220 | can1.write(msgTx); |
ClementBreteau | 1:f3f702086a30 | 221 | wait(0.2); |
ClementBreteau | 1:f3f702086a30 | 222 | }else{ |
ClementBreteau | 1:f3f702086a30 | 223 | moteur(); |
ClementBreteau | 0:7737d7573e3b | 224 | |
ClementBreteau | 1:f3f702086a30 | 225 | AX1(); |
ClementBreteau | 1:f3f702086a30 | 226 | AX2(); |
ClementBreteau | 1:f3f702086a30 | 227 | AX3(); |
ClementBreteau | 1:f3f702086a30 | 228 | |
ClementBreteau | 0:7737d7573e3b | 229 | } |
ClementBreteau | 1:f3f702086a30 | 230 | |
ClementBreteau | 1:f3f702086a30 | 231 | |
ClementBreteau | 0:7737d7573e3b | 232 | } |
ClementBreteau | 0:7737d7573e3b | 233 | } |
ClementBreteau | 0:7737d7573e3b | 234 |