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();
 
        }
        
         
    }
}