test des capteurs/actionneurs petit robot
Dependencies: mbed
main.cpp@0:7737d7573e3b, 2017-05-10 (annotated)
- Committer:
- ClementBreteau
- Date:
- Wed May 10 16:11:35 2017 +0000
- Revision:
- 0:7737d7573e3b
test des capteurs 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 | 0:7737d7573e3b | 2 | |
ClementBreteau | 0:7737d7573e3b | 3 | #define SIZE_FIFO 20 |
ClementBreteau | 0:7737d7573e3b | 4 | |
ClementBreteau | 0:7737d7573e3b | 5 | /* |
ClementBreteau | 0:7737d7573e3b | 6 | DigitalIn IO1(p23); |
ClementBreteau | 0:7737d7573e3b | 7 | DigitalIn IO2(p24); |
ClementBreteau | 0:7737d7573e3b | 8 | DigitalIn IO3(p25); |
ClementBreteau | 0:7737d7573e3b | 9 | DigitalIn IO4(p26); |
ClementBreteau | 0:7737d7573e3b | 10 | |
ClementBreteau | 0:7737d7573e3b | 11 | AnalogIn A_in1(p15); |
ClementBreteau | 0:7737d7573e3b | 12 | AnalogIn A_in2(p16); |
ClementBreteau | 0:7737d7573e3b | 13 | AnalogIn A_in3(p17); |
ClementBreteau | 0:7737d7573e3b | 14 | AnalogIn A_in4(p18); |
ClementBreteau | 0:7737d7573e3b | 15 | AnalogIn A_in5(p19); |
ClementBreteau | 0:7737d7573e3b | 16 | AnalogIn A_in6(p20); |
ClementBreteau | 0:7737d7573e3b | 17 | |
ClementBreteau | 0:7737d7573e3b | 18 | PwmOut IRL_1(p21); |
ClementBreteau | 0:7737d7573e3b | 19 | PwmOut IRL_2(p22); |
ClementBreteau | 0:7737d7573e3b | 20 | */ |
ClementBreteau | 0:7737d7573e3b | 21 | |
ClementBreteau | 0:7737d7573e3b | 22 | AnalogIn cptGauche(p20); |
ClementBreteau | 0:7737d7573e3b | 23 | AnalogIn cptDroit(p19); |
ClementBreteau | 0:7737d7573e3b | 24 | |
ClementBreteau | 0:7737d7573e3b | 25 | DigitalIn pressionGauche(p23); |
ClementBreteau | 0:7737d7573e3b | 26 | DigitalIn pressionDroit(p24); |
ClementBreteau | 0:7737d7573e3b | 27 | |
ClementBreteau | 0:7737d7573e3b | 28 | PwmOut moteurGauche(p21); |
ClementBreteau | 0:7737d7573e3b | 29 | PwmOut moteurDroit(p22); |
ClementBreteau | 0:7737d7573e3b | 30 | |
ClementBreteau | 0:7737d7573e3b | 31 | AnalogIn telemetre(p15); |
ClementBreteau | 0:7737d7573e3b | 32 | |
ClementBreteau | 0:7737d7573e3b | 33 | DigitalIn jack(p25); |
ClementBreteau | 0:7737d7573e3b | 34 | |
ClementBreteau | 0:7737d7573e3b | 35 | |
ClementBreteau | 0:7737d7573e3b | 36 | |
ClementBreteau | 0:7737d7573e3b | 37 | DigitalOut led(LED1); |
ClementBreteau | 0:7737d7573e3b | 38 | |
ClementBreteau | 0:7737d7573e3b | 39 | CAN can1(p30,p29); // Rx&Tx pour le CAN |
ClementBreteau | 0:7737d7573e3b | 40 | CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN |
ClementBreteau | 0:7737d7573e3b | 41 | unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN |
ClementBreteau | 0:7737d7573e3b | 42 | |
ClementBreteau | 0:7737d7573e3b | 43 | signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN |
ClementBreteau | 0:7737d7573e3b | 44 | int printCapteurs = 0; |
ClementBreteau | 0:7737d7573e3b | 45 | |
ClementBreteau | 0:7737d7573e3b | 46 | void canRx_ISR (void) |
ClementBreteau | 0:7737d7573e3b | 47 | { |
ClementBreteau | 0:7737d7573e3b | 48 | if (can1.read(msgRxBuffer[FIFO_ecriture])) { |
ClementBreteau | 0:7737d7573e3b | 49 | /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset(); |
ClementBreteau | 0:7737d7573e3b | 50 | else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/ |
ClementBreteau | 0:7737d7573e3b | 51 | FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; |
ClementBreteau | 0:7737d7573e3b | 52 | } |
ClementBreteau | 0:7737d7573e3b | 53 | } |
ClementBreteau | 0:7737d7573e3b | 54 | |
ClementBreteau | 0:7737d7573e3b | 55 | |
ClementBreteau | 0:7737d7573e3b | 56 | |
ClementBreteau | 0:7737d7573e3b | 57 | /*********************************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 58 | /* FUNCTION NAME: SendRawId */ |
ClementBreteau | 0:7737d7573e3b | 59 | /* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */ |
ClementBreteau | 0:7737d7573e3b | 60 | /*********************************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 61 | void SendRawId (unsigned short id) |
ClementBreteau | 0:7737d7573e3b | 62 | { |
ClementBreteau | 0:7737d7573e3b | 63 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 0:7737d7573e3b | 64 | msgTx.id=id; |
ClementBreteau | 0:7737d7573e3b | 65 | msgTx.len=0; |
ClementBreteau | 0:7737d7573e3b | 66 | can1.write(msgTx); |
ClementBreteau | 0:7737d7573e3b | 67 | wait_us(200); |
ClementBreteau | 0:7737d7573e3b | 68 | } |
ClementBreteau | 0:7737d7573e3b | 69 | |
ClementBreteau | 0:7737d7573e3b | 70 | /****************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 71 | /* FUNCTION NAME: canProcessRx */ |
ClementBreteau | 0:7737d7573e3b | 72 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
ClementBreteau | 0:7737d7573e3b | 73 | /****************************************************************************************/ |
ClementBreteau | 0:7737d7573e3b | 74 | void canProcessRx(void){ |
ClementBreteau | 0:7737d7573e3b | 75 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
ClementBreteau | 0:7737d7573e3b | 76 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 0:7737d7573e3b | 77 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
ClementBreteau | 0:7737d7573e3b | 78 | if(FIFO_occupation<0) |
ClementBreteau | 0:7737d7573e3b | 79 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
ClementBreteau | 0:7737d7573e3b | 80 | if(FIFO_max_occupation<FIFO_occupation) |
ClementBreteau | 0:7737d7573e3b | 81 | FIFO_max_occupation=FIFO_occupation; |
ClementBreteau | 0:7737d7573e3b | 82 | if(FIFO_occupation!=0) { |
ClementBreteau | 0:7737d7573e3b | 83 | |
ClementBreteau | 0:7737d7573e3b | 84 | switch(msgRxBuffer[FIFO_lecture].id) { |
ClementBreteau | 0:7737d7573e3b | 85 | case 0x63: |
ClementBreteau | 0:7737d7573e3b | 86 | SendRawId(0x73); |
ClementBreteau | 0:7737d7573e3b | 87 | break; |
ClementBreteau | 0:7737d7573e3b | 88 | case 0x10: |
ClementBreteau | 0:7737d7573e3b | 89 | printCapteurs = 0; |
ClementBreteau | 0:7737d7573e3b | 90 | break; |
ClementBreteau | 0:7737d7573e3b | 91 | case 0x11: |
ClementBreteau | 0:7737d7573e3b | 92 | printCapteurs = 1; |
ClementBreteau | 0:7737d7573e3b | 93 | break; |
ClementBreteau | 0:7737d7573e3b | 94 | |
ClementBreteau | 0:7737d7573e3b | 95 | |
ClementBreteau | 0:7737d7573e3b | 96 | } |
ClementBreteau | 0:7737d7573e3b | 97 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
ClementBreteau | 0:7737d7573e3b | 98 | } |
ClementBreteau | 0:7737d7573e3b | 99 | } |
ClementBreteau | 0:7737d7573e3b | 100 | |
ClementBreteau | 0:7737d7573e3b | 101 | int main() { |
ClementBreteau | 0:7737d7573e3b | 102 | can1.frequency(1000000); // fréquence de travail 1Mbit/s |
ClementBreteau | 0:7737d7573e3b | 103 | can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN |
ClementBreteau | 0:7737d7573e3b | 104 | |
ClementBreteau | 0:7737d7573e3b | 105 | CANMessage msgTx=CANMessage(); |
ClementBreteau | 0:7737d7573e3b | 106 | |
ClementBreteau | 0:7737d7573e3b | 107 | while(1) { |
ClementBreteau | 0:7737d7573e3b | 108 | led = !led; |
ClementBreteau | 0:7737d7573e3b | 109 | |
ClementBreteau | 0:7737d7573e3b | 110 | canProcessRx(); |
ClementBreteau | 0:7737d7573e3b | 111 | if (printCapteurs){ |
ClementBreteau | 0:7737d7573e3b | 112 | msgTx.id=0x21; |
ClementBreteau | 0:7737d7573e3b | 113 | msgTx.format=CANStandard; |
ClementBreteau | 0:7737d7573e3b | 114 | msgTx.type=CANData; |
ClementBreteau | 0:7737d7573e3b | 115 | msgTx.len=7; |
ClementBreteau | 0:7737d7573e3b | 116 | |
ClementBreteau | 0:7737d7573e3b | 117 | msgTx.data[0]=(unsigned char)cptGauche.read(); |
ClementBreteau | 0:7737d7573e3b | 118 | msgTx.data[1]=(unsigned char)cptDroit.read(); |
ClementBreteau | 0:7737d7573e3b | 119 | msgTx.data[2]=(unsigned char)pressionGauche.read(); |
ClementBreteau | 0:7737d7573e3b | 120 | msgTx.data[3]=(unsigned char)pressionDroit.read(); |
ClementBreteau | 0:7737d7573e3b | 121 | msgTx.data[4]=(unsigned char)jack.read(); |
ClementBreteau | 0:7737d7573e3b | 122 | msgTx.data[5]=(unsigned char)(telemetre.read_u16()>>8); |
ClementBreteau | 0:7737d7573e3b | 123 | msgTx.data[6]=(unsigned char)telemetre.read_u16(); |
ClementBreteau | 0:7737d7573e3b | 124 | |
ClementBreteau | 0:7737d7573e3b | 125 | can1.write(msgTx); |
ClementBreteau | 0:7737d7573e3b | 126 | |
ClementBreteau | 0:7737d7573e3b | 127 | } |
ClementBreteau | 0:7737d7573e3b | 128 | wait(0.5); |
ClementBreteau | 0:7737d7573e3b | 129 | } |
ClementBreteau | 0:7737d7573e3b | 130 | } |
ClementBreteau | 0:7737d7573e3b | 131 |