File content as of revision 18:68a8f70707e6:

/* #include <mbed_events.h> */
#include "mbed.h"

#define adrr_queue  2
#define adrr_gauche 1
#define adrr_droite 3

#define hex_queue 0x02
#define hex_gauche 0x01
#define hex_droite 0x03


DigitalOut alivenessLED(LED1);
DigitalOut testLed(LED2);

Serial coach(D1, D0);
/*Serial coach(USBTX, USBRX);*/
Serial myserial_ax12(PC_4,PC_5,115200);                                         // Modifier le baud rate en fonction de votre utilisation des moteurs

const float rapport_vitesse = 0.019383809*0.026315;
const float root3 = 1.7321;
/*a=moteur de queue
AX12 myax12a (PC_4, PC_5, 2,115200);
b : moteur de gauche
AX12 myax12b (PC_4, PC_5, 1,115200);
c : moteur de droite
AX12 myax12c (PC_4, PC_5, 3,115200);*/

int i = 0;
char last_read;
int indice_a_modifie = 1;
char message_0[50] = {};
char message_1[50] = {};
char message_vide = '0';
float front, lat, rot, T_front, T_lat = 0;
int flag_msg = 0;
int flag_a_traite = 0;


DigitalOut my_led1(LED1);
DigitalOut my_led2(LED2);
DigitalOut my_led3(LED3);
DigitalIn bp(USER_BUTTON);

void SetCRSpeed(int ID, float speed);
void SetMode(int ID, int mode);
void SetCWLimit (int ID, int degrees);
void SetCCWLimit (int ID, int degrees);
void Write(int ID, int start, int bytes, char* data);

char tata[10] = {};
uint16_t position_1;
uint16_t position_2;
uint16_t position_3;
int num_moteur = 1;
int vitesse;
char sens;
int temps_ms = 0;
//pour connaitre le nombre de tours
int tour_1 = 0;
int tour_2 = 0;
int tour_3 = 0;

Ticker ticker_1ms;

uint16_t position_1_old;
uint16_t position_2_old;
uint16_t position_3_old;
int16_t delta_position_1;
int16_t delta_position_2;
int16_t delta_position_3;

//Définition des trames de requête
char trame_demande_pos_1[8]= {0xFF,0xFF,hex_queue,0x4,0x2,0x24,2,0xD2};
char trame_demande_pos_2[8]= {0xFF,0xFF,hex_gauche,0x4,0x2,0x24,2,0xD1};
char trame_demande_pos_3[8]= {0xFF,0xFF,hex_droite,0x4,0x2,0x24,2,0xD0};


//Fonction d'interruption sur réception de caractère sur liaison série
void Rx_Irq_fonction(void)
{
    uint16_t position_lu = 0;
    my_led1 = !my_led1;

    static int i_rx=0;
    tata[i_rx]=myserial_ax12.getc();

    if (tata[0]!=0xFF)                                                          // On vérifie que le premier et le deuxième octet sont biens 0xFF
        i_rx=0;
    else i_rx++;
    if((i_rx==2)&&(tata[1] != 0xFF)) i_rx=0;

//Une fois qu'on sait que la trame est "juste" on récupère le reste
    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x01) && i_rx==7) {
        my_led3 = !my_led3;
        i_rx=0;

        if(tata[4] == 0) { //si erreur  est nulle
            //On transforme les données en fonction de ce qu'on lit depuis les moteurs
            position_lu = tata[5]+((uint16_t)tata[6]<<8);
            if((0<= position_lu)&&(position_lu<=4095)) {
                position_1 = position_lu;
            }
        }
        for(int i=0; i<10; i++) {
            tata[i]=0;
        }
    }

    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x02) && i_rx==7) {
        my_led3 = !my_led3;
        i_rx=0;

        if(tata[4]==00) { //si erreur nulle
            //On transforme les données en fonction de ce qu'on lit depuis les moteurs
            position_lu = tata[5]+((uint16_t)tata[6]<<8);
            if((0<= position_lu)&&(position_lu<=4095)) {
                position_2 = position_lu;
            }

        }
        for(int i=0; i<10; i++) {
            tata[i]=0;
        }
    }

    if ((tata[0]==0xFF) && (tata[1]==0xFF) && (tata[2]==0x03) && i_rx==7) {
        my_led3 = !my_led3;
        i_rx=0;

        if(tata[4]==00) { //si erreur nulle
            //On transforme les données en fonction de ce qu'on lit depuis les moteurs
            position_lu = tata[5]+((uint16_t)tata[6]<<8);
            if((0<= position_lu)&&(position_lu<=4095)) {
                position_3 = position_lu;
            }

        }
        for(int i=0; i<10; i++) {
            tata[i]=0;
        }
    }

    if (i_rx>9) {
        for(int i=0; i<10; i++)
            tata[i]=0;
        i_rx=0;
    }
}

void fonction_1ms(void)
{
    temps_ms++;
}



void asser(float Vx, float Vy, float gammaz)
{
    float wa = rapport_vitesse*(Vy + gammaz);
    float wb = rapport_vitesse*( -0.5 * Vy + (root3 / 2) * Vx + gammaz);
    float wc = rapport_vitesse*( -0.5 * Vy - (root3 / 2) * Vx + gammaz);
    /*moteur a */
    SetCRSpeed(adrr_queue, wa);
    wait(0.1)
    /*moteur b */
    SetCRSpeed(adrr_gauche, wb);
    wait(0.1)
    /* moteur c */
    SetCRSpeed(adrr_droite, wc);
    wait(0.1)
}

void send_stat()
{
    coach.printf("S%f,%f,%f,%f,%f \r\n", front, lat, rot, T_front, T_lat );
}

void send_roues()
{
    coach.printf("\n\rR %5d ; %5d ; %5d ; %5d ; %5d ; %5d\r\n", position_1, delta_position_1, position_2, delta_position_2, position_3, delta_position_3);
}

void mise_a_zero_msg_0()
{
    for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
        message_0[i] = message_vide;
    }
}


void mise_a_zero_msg_1()
{
    for(i=0; i<50; i++) { //On réinitialise la chaîne de caractères
        message_1[i] = message_vide;
    }
}

void callback_serial()
{
    last_read = coach.getc();
    if (last_read == 'S') {
        send_stat();
    } else if (last_read == 'R') {
        send_roues();
    } else if (last_read == ';') {
        coach.puts("EOL\n");
        /*coach.puts(message_0);
        coach.puts(message_1); */
        if (flag_msg == 0) {
            message_0[indice_a_modifie] = last_read;
            mise_a_zero_msg_1();
            flag_msg = 1;
        } else {
            message_1[indice_a_modifie] = last_read;
            mise_a_zero_msg_0();
            flag_msg = 0;
        }
        flag_a_traite = 1;
        indice_a_modifie = 0;
    } else {
        if (flag_msg == 0) {
            message_0[indice_a_modifie] = last_read;
        } else {
            message_1[indice_a_modifie] = last_read;
        }
        indice_a_modifie++;
    }
}

void reception_com()
{
    if (flag_a_traite == 1) {
        coach.puts("traitement\n");
        if (flag_msg == 0) {
            sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
        } else {
            sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
        }
        flag_a_traite = 0;
    }
}


int main()
{

    coach.baud(115200);
    coach.puts("slt\n\r");
    coach.attach(&callback_serial, Serial::RxIrq);
    
    //myax12.SetMode(1); //passage en Continuous rotation

    //char trame_cmd_speed_1[9]={0xFF,0xFF,0x01,0x5,0x3,0x20,0x10,0x0,0xC6};
    //char trame_cmd_speed_2[9]={0xFF,0xFF,0x02,0x5,0x3,0x20,0x10,0x0,0xC5};
    //char trame_cmd_speed_3[9]={0xFF,0xFF,0x03,0x5,0x3,0x20,0x10,0x0,0xC4};

    //pour regarder les 3 moteurs les uns à la suite des autres

    int etat=1;
    int fm_etat=1;

    SetMode(adrr_queue , 1);
    wait(0.1);
    SetMode(adrr_droite, 1);
    wait(0.1);
    SetMode(adrr_gauche, 1);

    /*   SetCRSpeed(1,0.05);
       wait(0.1);
       SetCRSpeed(2,0.05);
       wait(0.1);
       SetCRSpeed(3,0.05); */

    char a;

    //On passe le mode retour en 1 : retour seulement sur demande de lecture
    a=1;
    Write(adrr_queue,16,1,&a);
    Write(adrr_droite,16,1,&a);
    Write(adrr_gauche,16,1,&a);


    //On passe le délai de retour à 250µs. C'est bien de le diminuer ensuite.
    a=250;
    Write(adrr_queue,5,1,&a);
    Write(adrr_droite,5,1,&a);
    Write(adrr_gauche,5,1,&a);

    /* On coupe de le couple du moteur pour pouvoir le faire tourner à la main
    a=0;
    Write(2,24,1,&a);
    Write(1,24,1,&a);
    Write(3,24,1,&a); */

    myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
    ticker_1ms.attach(&fonction_1ms,0.001);
    etat=2;

    coach.printf("fin_initialisation");
    position_1 = 0;
    position_2 = 0;
    position_3 = 0;
    position_1_old = 0;
    position_2_old = 0;
    position_3_old = 0;
    delta_position_1 = 0;
    delta_position_2 = 0;
    delta_position_3 = 0;
    
    while (true) {
        //on regarde la position des moteurs
        switch(etat) {
            case 2 :
                if(fm_etat) {
                    position_1_old = position_1;
                    for(int i=0; i<=7; i++) {
                        //On envoi la trame que l'on souhaite

                        myserial_ax12.putc(trame_demande_pos_1[i]);
                        //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures
                    }
                    fm_etat=0;
                }

                if(temps_ms>=3) {
                    etat=3;
                    fm_etat=1;
                }
                break;

            case 3 :

                if(fm_etat) {
                    delta_position_1 = (int16_t)position_1 - (int16_t)position_1_old;
                    if(delta_position_1 > 2048) {
                        delta_position_1 = delta_position_1 - 4096;
                    }

                    if(delta_position_1 < -2048) {
                        delta_position_1 = delta_position_1 + 4096;
                    }
                    position_2_old = position_2;
                    for(int i=0; i<=7; i++) {
                        //On envoi la trame que l'on souhaite
                        myserial_ax12.putc(trame_demande_pos_2[i]);
                        //hypothese marche avant et il ne fait pas plus d'un tour entre deux mesures

                    }
                    fm_etat=0;
                }

                if(temps_ms>=6) {
                    etat=4;
                    fm_etat=1;
                }
                break;

            case 4 :
                if(fm_etat) {
                    delta_position_2 = (int16_t)position_2 - (int16_t)position_2_old;
                    if(delta_position_2 > 2048) {
                        delta_position_2 = delta_position_2 - 4096;
                    }

                    if(delta_position_2 < -2048) {
                        delta_position_2 = delta_position_2 + 4096;
                    }
                    position_3_old = position_3;
                    for(int i=0; i<=7; i++) {
                        //On envoi la trame que l'on souhaite
                        myserial_ax12.putc(trame_demande_pos_3[i]);

                    }
                    fm_etat=0;
                }

                if(temps_ms>=9) {
                    etat=5;
                    fm_etat=1;
                }
                break;

            case 5 :
                if(fm_etat == 1) {
                    delta_position_3 = (int16_t)position_3 - (int16_t)position_3_old;
                    if(delta_position_3 > 2048) {
                        delta_position_3 = delta_position_3 - 4096;
                    }

                    if(delta_position_3 < -2048) {
                        delta_position_3 = delta_position_3 + 4096;
                    }
                    send_roues();
                }

                fm_etat =0;

                if (flag_a_traite == 1) {
                    etat = 6;
                    temps_ms = 0;
                    fm_etat =1;
                }
                break;
                
            case 6 :
                if(fm_etat ==1){
                coach.puts("traitement\n");
                if (flag_msg == 0) {
                    sscanf(message_1,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
                } else {
                    sscanf(message_0,"%f,%f,%f,%f,%f", &front, &lat, &rot, &T_front, &T_lat );
                }
                flag_a_traite = 0;
                temps_ms = 0;
                asser(front, lat, rot);
                fm_etat = 0;
                }
                if(temps_ms>= 5) {

                    etat = 2;
                    fm_etat =1;
                    temps_ms = 0;
                }
                break;
        }
    }

}

void SetCRSpeed(int ID, float speed)
{

    // bit 10     = direction, 0 = CCW, 1=CW
    // bits 9-0   = Speed
    char data[2];

    int goal = (0x3ff * abs(speed));

    // Set direction CW if we have a negative speed
    if (speed < 0) {
        goal |= (0x1 << 10);
    }

    data[0] = goal & 0xff; // bottom 8 bits
    data[1] = goal >> 8;   // top 8 bits

    // write the packet, return the error code
    Write(ID, 0x20, 2, data);
}

void SetMode(int ID, int mode)
{

    if (mode == 1) { // set CR
        SetCWLimit(ID,0);
        SetCCWLimit(ID,0);
        SetCRSpeed(ID,0.0);
    } else {
        SetCWLimit(ID,0);
        SetCCWLimit(ID,360);
        SetCRSpeed(ID,0.0);
    }
}

void SetCWLimit (int ID, int degrees)
{

    char data[2];
    short limit = (4095 * degrees) / 360;

    data[0] = limit & 0xff; // bottom 8 bits
    data[1] = limit >> 8;   // top 8 bits

    Write(ID, 0x06, 2, data);
}

void SetCCWLimit (int ID, int degrees)
{

    char data[2];
    short limit = (4095 * degrees) / 360;

    data[0] = limit & 0xff; // bottom 8 bits
    data[1] = limit >> 8;   // top 8 bits

    Write(ID, 0x08, 2, data);
}

void Write(int ID, int start, int bytes, char* data)
{
    char TxBuf[16];
    char sum = 0;

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;

    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];


    // packet Length
    TxBuf[3] = 3+bytes;
    sum += TxBuf[3];

    // Instruction
    TxBuf[4]=0x03;
    sum += TxBuf[4];

    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];

    // data
    for (char i=0; i<bytes ; i++) {
        TxBuf[6+i] = data[i];
        sum += TxBuf[6+i];

    }

    // checksum
    TxBuf[6+bytes] = 0xFF - sum;

    // Transmit the packet in one burst with no pausing
    for (int i = 0; i < (7 + bytes) ; i++) {
        myserial_ax12.putc(TxBuf[i]);
    }

    wait(0.00002);

}