File content as of revision 14:5374abce265d:

#include "mbed.h"

//AX12 myax12 (PC_4, PC_5, 1,115200);
Serial myserial_ax12(PC_4,PC_5,115200);                                         // Modifier le baud rate en fonction de votre utilisation des moteurs
Serial pc(USBTX,USBRX,115200);
//Serial zigBee(PC_1,PC_0,115200);


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] = {};
int position_1;
int position_2;
int 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;

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


//Fonction d'interruption sur réception de caractère sur liaison série
void Rx_Irq_fonction(void)
{
    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_1 = tata[5]+((uint16_t)tata[6]<<8);
        }
        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_2 = tata[5]+((uint16_t)tata[6]<<8);

        }
        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_3 = tata[5]+((uint16_t)tata[6]<<8);

        }
        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++;
}


int main()
{
    //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 qu'il s'arrete
    uint8_t car_recu=0;

    uint8_t bp_lu;
    
    //pour regarder les 3 moteurs les uns à la suite des autres
    int etat=1;
    int fm_etat=1;
    int position_1_old;
    int position_2_old;
    int position_3_old;
    pc.printf("test lecture AX12\n\r");

    SetMode(1, 1);
    wait(0.1);
    SetMode(2, 1);
    wait(0.1);
    SetMode(3, 1);

    wait(0.1);

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

    myserial_ax12.attach(&Rx_Irq_fonction, Serial::RxIrq);
    ticker_1ms.attach(&fonction_1ms,0.001);
    etat=2;
    while(1) {
        
        //commande des moteurs (pour les tests)
        time_t seconds = time(NULL);
        bp_lu = bp.read();
        if(pc.readable()) {
            car_recu=pc.getc();
            switch(car_recu) {
            case 's' :
                SetCRSpeed(1,0);
                SetCRSpeed(2,0);
                SetCRSpeed(3,0);
                break;
            case 'a' :
                SetCRSpeed(1,0.05);
                wait(0.1);
                SetCRSpeed(2,0.05);
                wait(0.1);
                SetCRSpeed(3,0.05);
            
        }
        }

        



        //on regarde la position des moteurs
        switch(etat) {
            case 2 :
                if(fm_etat) {
                    position_1_old = position_1;
                    for (int i=0; i<=9; i++){
                        pc.printf("%x ", tata[i]);
                        }
                    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
                    }
                    if(position_1_old> position_1) {
                            tour_1++;
                        }
                        
                    fm_etat=0;
                }

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

            case 3 :
            
                if(fm_etat) {
                    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
                        
                    }
                    if(position_2_old > position_2) {
                            tour_2++;
                        }
                    fm_etat=0;
                }

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

            case 4 :
                if(fm_etat) {
                    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]);
                        
                    }
                    if(position_3_old > position_3) {
                            tour_3++;
                        }
                    fm_etat=0;
                }

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

            case 5 :
                
                pc.printf("\n\rposition moteur 1  = %d :", position_1);
                pc.printf("\n\rtour moteur 1  = %d :", tour_1);
                pc.printf("\n\rposition moteur 2  = %d :", position_2);
                pc.printf("\n\rtour moteur 2  = %d :", tour_2);
                pc.printf("\n\rposition moteur 3  = %d :", position_3);
                pc.printf("\n\rtour moteur 3  = %d :", tour_3);
                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);

}