test des capteurs/actionneurs petit robot
Fork of mbed_tes_cpt by
main.cpp
- Committer:
- ClementBreteau
- Date:
- 2017-05-11
- Revision:
- 1:f3f702086a30
- Parent:
- 0:7737d7573e3b
- Child:
- 2:9d280856a536
File content as of revision 1:f3f702086a30:
#include "mbed.h" #include "AX12.h" #include "cmsis.h" #define SIZE_FIFO 20 #define TIME 1 #define T_MOT 0.00005 /* DigitalIn IO1(p23); DigitalIn IO2(p24); DigitalIn IO3(p25); DigitalIn IO4(p26); AnalogIn A_in1(p15); AnalogIn A_in2(p16); AnalogIn A_in3(p17); AnalogIn A_in4(p18); AnalogIn A_in5(p19); AnalogIn A_in6(p20); PwmOut IRL_1(p21); PwmOut IRL_2(p22); */ AnalogIn cptGauche(p20); AnalogIn cptDroit(p19); DigitalIn pressionGauche(p23); DigitalIn pressionDroit(p24); AnalogIn telemetre(p15); DigitalIn jack(p25); PwmOut motGauche(p21); PwmOut motDroit(p22); DigitalOut led(LED1); CAN can1(p30,p29); // Rx&Tx pour le CAN CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN 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 AX12 *AX12_17, *AX12_14, *multiple_AX12; AX12 *AX12_14_2, *AX12_11, *multiple_AX12_2; AX12 *AX12_4, *AX12_18, *multiple_AX12_3; int printCapteurs = 1; static char TAB1[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03, 0x11, 0x50, 0x02, 0xFF, 0x03}; static char TAB2[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03, 0x11, 0x00, 0x00, 0xFF, 0x03}; static char TAB3[10]= {0x0E, 0x00, 0x00, 0xFF, 0x03, 0x0B, 0x50, 0x02, 0xFF, 0x03}; static char TAB4[10]= {0x0E, 0x50, 0x02, 0xFF, 0x03, 0x0B, 0x00, 0x00, 0xFF, 0x03}; static char TAB5[10]= {0x04, 0x00, 0x00, 0xFF, 0x03, 0x12, 0x50, 0x02, 0xFF, 0x03}; static char TAB6[10]= {0x04, 0x50, 0x02, 0xFF, 0x03, 0x12, 0x00, 0x00, 0xFF, 0x03}; Timer t; Ticker flipper; void canRx_ISR (void) { if (can1.read(msgRxBuffer[FIFO_ecriture])) { /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset(); else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/ FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; } } /*********************************************************************************************************/ /* 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; can1.write(msgTx); wait_us(200); } /*********************************************************************************************************/ /* FUNCTION NAME: AX1 */ /* DESCRIPTION : bouge les AX12 */ /*********************************************************************************************************/ void AX1(void){ multiple_AX12->multiple_goal_and_speed(2,TAB1); wait(TIME); multiple_AX12->multiple_goal_and_speed(2,TAB2); wait(TIME); } /*********************************************************************************************************/ /* FUNCTION NAME: AX2 */ /* DESCRIPTION : bouge les AX12 */ /*********************************************************************************************************/ void AX2(void){ multiple_AX12_2->multiple_goal_and_speed(2,TAB3); wait(TIME); multiple_AX12_2->multiple_goal_and_speed(2,TAB4); wait(TIME); } /*********************************************************************************************************/ /* FUNCTION NAME: AX3 */ /* DESCRIPTION : bouge les AX12 */ /*********************************************************************************************************/ void AX3(void){ multiple_AX12_3->multiple_goal_and_speed(2,TAB5); wait(TIME); multiple_AX12_3->multiple_goal_and_speed(2,TAB6); wait(TIME); } /*********************************************************************************************************/ /* FUNCTION NAME: moteur */ /* DESCRIPTION : bouge les moteurs */ /*********************************************************************************************************/ void moteur(void){ motGauche.write(0.5f); motDroit.write(0.5f); } /****************************************************************************************/ /* 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 0x63: SendRawId(0x73); break; case 0x10: printCapteurs = 0; break; case 0x11: printCapteurs = 1; break; } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; } } int main() { can1.frequency(1000000); // fréquence de travail 1Mbit/s can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN CANMessage msgTx=CANMessage(); AX12_14 = new AX12(p9, p10, 14, 1000000); // tx, rx AX12_17 = new AX12(p9, p10, 17, 1000000); multiple_AX12 = new AX12(p9,p10,0xFE,1000000); AX12_14_2 = new AX12(p13, p14, 14, 1000000); AX12_11 = new AX12(p13, p14, 11, 1000000); multiple_AX12_2 = new AX12(p13,p14,0xFE,1000000); AX12_4= new AX12(p28, p27, 14, 1000000); AX12_18= new AX12(p28, p27, 11, 1000000); multiple_AX12_3= new AX12(p28,p27,0xFE,1000000); motGauche.period(T_MOT); motDroit.period(T_MOT); motGauche.write(0.0); motDroit.write(0.0); while(1) { led = !led; canProcessRx(); if (printCapteurs){ motGauche.write(0); motDroit.write(0); msgTx.id=0x21; msgTx.format=CANStandard; msgTx.type=CANData; msgTx.len=7; msgTx.data[0]=(unsigned char)cptGauche.read(); msgTx.data[1]=(unsigned char)cptDroit.read(); msgTx.data[2]=(unsigned char)pressionGauche.read(); msgTx.data[3]=(unsigned char)pressionDroit.read(); msgTx.data[4]=(unsigned char)jack.read(); msgTx.data[5]=(unsigned char)(telemetre.read_u16()>>8); msgTx.data[6]=(unsigned char)telemetre.read_u16(); can1.write(msgTx); wait(0.2); }else{ moteur(); AX1(); AX2(); AX3(); } } }