File content as of revision 1:fce3323d874f:

#include "mbed.h"
#include "AX12.h"

Timer t;

Serial pc(USBTX, USBRX); // tx, rx
int Key=0,mode=0;

// This function is called when a character goes into the RX buffer.
void rxCallback()
{
    Key=pc.getc();
}


void menu()
{
    pc.printf("------------------------------------------\n\n\r");
    pc.printf("-                   MENU                 -\n\n\r");
    pc.printf("------------------------------------------\n\n\r");
    pc.printf("- S pour scanner l'ID AX12 et la vitesse -\n\n\r");
    pc.printf("- V pour changer la vitesse de l'AX12    -\n\n\r");
    pc.printf("- I pour changer l'ID de l'AX12          -\n\n\r");
    pc.printf("- T pour faire un mouvement de test      -\n\n\r");
    pc.printf("- R pour revenir au parametre usine      -\n\n\r");
    pc.printf("- M pour revenir au menu                 -\n\n\r");
    pc.printf("- A pour lire info d'un AX12             -\n\n\r");
    pc.printf("------------------------------------------\n\n\r");
}

void ax12read_all_info(unsigned char start,unsigned char longueur)
{
    AX12 *ptr_myAX12;
    unsigned char numero_ID;
    unsigned int i=0;
    signed char status;
    unsigned long vitesse,vitesseNew;
    pc.printf("\rNumero de l'Id de l'AX12: ");
    pc.scanf("%d",&numero_ID);
    pc.printf("%d \n\n\r",numero_ID);
    pc.printf("\rVitesse actuelle de l'AX12: ");
    pc.scanf("%d",&vitesse);
    pc.printf("%d \n\n\r",vitesse);
    for(i=0; i<=0; i++) {
        //vitesse=2000000/(i+1);
        ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
        status = -1;                                          // Reset status variable
        status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
        if(1) { //status == numero_ID)
            pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse);    // If it does print address and position to the PC
            pc.printf("readinfo \n\n\r");
            ptr_myAX12->read_all_info(start,longueur);
            wait(1.);
        } else
            pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
        delete ptr_myAX12;
    }
}


void lecture()
{

}

void ax12reset()
{
    AX12 *ptr_myAX12;
    unsigned char numero_ID;
    unsigned int i=0;
    char status=-1;
    unsigned long vitesse,vitesseNew;
    pc.printf("\rNumero de l'Id de l'AX12: ");
    pc.scanf("%d",&numero_ID);
    pc.printf("%d \n\n\r",numero_ID);
    pc.printf("\rVitesse actuelle de l'AX12: ");
    pc.scanf("%d",&vitesse);
    pc.printf("%d \n\n\r",vitesse);
    for(i=0; i<=0; i++) {
        //vitesse=2000000/(i+1);
        ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
        status = -1;                                          // Reset status variable
        status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
        if(1) { //status == numero_ID)
            pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse);    // If it does print address and position to the PC
            pc.printf("reset \n\n\r");
            ptr_myAX12->reset();
            wait(1.);
        } else
            pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
        delete ptr_myAX12;
    }
}

void change_ident()
{
    AX12 *ptr_myAX12;
    unsigned int numero_ID,ID_new;
    char status=-1;
    unsigned long vitesse;
    pc.printf("\rNumero de l'Id de l'AX12: ");
    pc.scanf("%d",&numero_ID);
    pc.printf("%d \n\n\r",numero_ID);
    pc.printf("\rVitesse actuelle de l'AX12: ");
    pc.scanf("%d",&vitesse);
    pc.printf("%d \n\n\r",vitesse);
    ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
    status = -1;                                          // Reset status variable
    status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
    if(1) { //status == numero_ID)
        pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse);    // If it does print address and position to the PC
        pc.printf("\rNouvelle ID de l'AX12: ");
        pc.scanf("%d",&ID_new);
        status = -1;                                          // Reset status variable
        status = ptr_myAX12->Set_ID(numero_ID,ID_new);                   // See if the AX12 exists at this address
        delete ptr_myAX12;
        ptr_myAX12 = new AX12(p9, p10, ID_new,vitesse);                  // Create objects
        status = -1;                                          // Reset status variable
        status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
        pc.printf("Changement de ID");
        if(status == ID_new) {
            pc.printf(" reussi \n\r");    // If it does print address and position to the PC
        } else
            pc.printf(" rate \n\r");    // If it does print address and position to the PC
        delete ptr_myAX12;
    } else {
        pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);


    }


}

void vitesse()
{
    AX12 *ptr_myAX12;
    unsigned int numero_ID,baud;
    char status=-1;
    unsigned int vitesse,vitesseNew;
    pc.printf("\rNumero de l'Id de l'AX12: ");
    pc.scanf("%d",&numero_ID);
    pc.printf("%d \n\n\r",numero_ID);
    pc.printf("\rVitesse actuelle de l'AX12: ");
    pc.scanf("%d",&vitesse);
    pc.printf("%d \n\n\r",vitesse);
    ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
    status = -1;                                          // Reset status variable
    status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
    if(status == numero_ID) {
        pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse);    // If it does print address and position to the PC
        pc.printf("Nouvelle Vitesse de l'AX12: ");
        pc.scanf("%d",&vitesseNew);
        baud=(2000000/vitesseNew)-1;
        pc.printf("\n\r %d \n\r",baud);
        status = -1;                                          // Reset status variable
        status = ptr_myAX12->Set_Baud(baud);

        // See if the AX12 exists at this address
        delete ptr_myAX12;
        ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesseNew);                  // Create objects
        status = -1;                                          // Reset status variable
        status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
        pc.printf("Changement de vitesse");
        if(status == numero_ID) {
            pc.printf(" reussi \n\r");    // If it does print address and position to the PC
        } else
            pc.printf(" rate \n\r");    // If it does print address and position to the PC
        delete ptr_myAX12;
    } else {
        pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);


    }


}

void test()
{
    AX12 *bas_myAX12,*milieu_myAX12, *haut_myAX12;
    unsigned char numero_ID;
    char status=-1;
    unsigned long vitesse=1023,vitesseNew,position;
    bas_myAX12 = new AX12(p9, p10, 2,1000000);                  // Create objects
    milieu_myAX12 = new AX12(p9, p10, 1,1000000);                  // Create objects
    haut_myAX12 = new AX12(p9, p10, 3,1000000);                  // Create objects
    bas_myAX12->Set_Goal_speed(1023);//wait(0.2);
    milieu_myAX12->Set_Goal_speed(1023);//wait(0.2);
    haut_myAX12->Set_Goal_speed(1023);//wait(0.2);

    do 
    {
        pc.printf("numero de mouvement");
        pc.scanf("%d",&position);


        switch(position) 
        {
            case 1:
                bas_myAX12->Set_Secure_Goal(150);
                milieu_myAX12->Set_Secure_Goal(150-60);
                haut_myAX12->Set_Secure_Goal(150+60);
                break;
            
            case 2:
                bas_myAX12->Set_Secure_Goal(60);
                break;
                
            case 3:
                milieu_myAX12->Set_Secure_Goal(150-90);
                bas_myAX12->Set_Secure_Goal(60+45);
                haut_myAX12->Set_Secure_Goal(150+45);
                break;
                
            case 4:
                haut_myAX12->Set_Secure_Goal(150);
                milieu_myAX12->Set_Secure_Goal(105);
                bas_myAX12->Set_Secure_Goal(110);
                break;
                
            case 5:
                bas_myAX12->Set_Secure_Goal(105);
                haut_myAX12->Set_Secure_Goal(105);
                milieu_myAX12->Set_Secure_Goal(150);
                break;

        }
        //wait(0.2);
    }while(pc.readable()==0);
 
 }

void scan()
{
    AX12 *myAX12[100];
    int status=0,cpt=0;
    long baud=0,baudrate=0,find_servo=0,tabbaud[11]= {1,2,3,4,7,9,16,34,64,103,208}; //{207,103,34,16,9,7,4,3,2,1,0};//
// Print msg to PC
    pc.printf("Scanning chain for AX12+ Servos...\n\n\r");
    // See which addresses contain AX12+ servos
    for(baud=7; baud<11; baud++) {
        baudrate=2000000/(tabbaud[baud]+1);
        find_servo=0;
        pc.printf("\r\n");
        pc.printf("\rcheck servo in %d baudrate\r\n",baudrate);
        for (int  i = 0; i < 254; i++) {
            myAX12[cpt] = new AX12(p9, p10, i,baudrate);                  // Create objects
            pc.printf("\r%03d",i);
            status = -1;                                          // Reset status variable
            status = myAX12[cpt]->Get_ID();                   // See if the AX12 exists at this address
            if(status == i) {
                pc.printf("\rServo found at address %d and baudrate %d\r\n",i,baudrate);    // If it does print address and position to the PC
                find_servo=find_servo++;
                cpt++;
            } else {
                delete myAX12[cpt];
                /*if(i%16==0)
                    pc.printf("\r");
                */
            }
        }
        if(find_servo==0)
            pc.printf("\rno AX12 in this baudrate=%d\n\r",baudrate);
        //wait(2.);
    }

}

void scan_all()
{
    AX12 *myAX12[15];
    int status=0,cpt=0,nombre_servo;
    long baud=0,baudrate=0,find_servo=0,tabbaud[11]= {64,1,2,3,4,7,9,16,34,103,208}; //{207,103,34,16,9,7,4,3,2,1,0};//
// Print msg to PC
    pc.printf("Scanning chain for AX12+ Servos not more than 15 servos ...\n\n\r");
    pc.printf("combien de servo à chercher? ");
    pc.attach(NULL, Serial::RxIrq);
    pc.scanf("%d",&nombre_servo);
    pc.attach(&rxCallback, Serial::RxIrq);// See which addresses contain AX12+ servos
    for(baud=0; baud<=255; baud++) {
        if((Key=='q')||(Key=='Q')) {
            Key=0;
            break;
        }
        baudrate=2000000/(baud+1);
        find_servo=0;
        pc.printf("\r\n");
        for (int  i = 0; i < 253; i++) {
            myAX12[cpt] = new AX12(p9, p10, i,baudrate);                  // Create objects
            pc.printf("\r%03d",i);
            status = -1;                                          // Reset status variable
            status = myAX12[cpt]->Get_ID();                   // See if the AX12 exists at this address
            if(status == i) {
                pc.printf("\r Servo found at address %d and baudrate %d\n\r",i,baudrate);    // If it does print address and position to the PC
                find_servo=find_servo++;
                cpt++;

            } else {
                delete myAX12[cpt];
                /*if(i%16==0)
                    pc.printf("\r");
                */
            }
            wait(0.001);
        }
        if(cpt>=nombre_servo)
            break;
        if(find_servo==0)
            pc.printf("\r %03d no AX12 in this baudrate=%d\n\r",baud,baudrate);

    }

}



int main()
{
    int NewID=0, OldID=0;
    t.start();
    menu();
    pc.attach(&rxCallback, Serial::RxIrq);
    while(1) {
        switch(Key) {
            case 's':
            case 'S':
                Key=0;
                scan_all();
                menu();
                break;

            case 'l':
            case 'L':
                Key=0;
                lecture();
                menu();
                break;
            case 'm':
            case 'M':
                Key=0;
                menu();
                break;

            case 'v':
            case 'V':
                Key=0;
                pc.attach(NULL, Serial::RxIrq);
                vitesse();
                pc.attach(&rxCallback, Serial::RxIrq);
                menu();
                break;

            case 'i':
            case 'I':
                Key=0;
                pc.attach(NULL, Serial::RxIrq);
                change_ident();
                pc.attach(&rxCallback, Serial::RxIrq);
                menu();
                break;
            case 't':
            case 'T':
                Key=0;
                pc.attach(NULL, Serial::RxIrq);
                test();
                pc.attach(&rxCallback, Serial::RxIrq);
                break;

            case 'r':
            case 'R':
                Key=0;
                pc.attach(NULL, Serial::RxIrq);
                ax12reset();
                pc.attach(&rxCallback, Serial::RxIrq);
                break;

            case 'a':
            case 'A':
                Key=0;
                pc.attach(NULL, Serial::RxIrq);
                ax12read_all_info(0,8);
                ax12read_all_info(10,10);
                ax12read_all_info(20,10);
                ax12read_all_info(30,10);
                ax12read_all_info(40,10);

                pc.attach(&rxCallback, Serial::RxIrq);
                break;

            default:
                break;

        }
    }
}

// Print msg to PC
/*        pc.printf("\nEnter AX12+ ID to Alter...  ");
        while (Key != 0x0D){
            Key=pc.getc();                                  //Get the keypad pressed
            if((Key>=0x30)&&(Key<=0x39)){                   //See if it's a number (0-9)
                OldID=OldID*10+(Key-48);                    //Strip off ASCII code, convert to int & index
                pc.putc(Key);
            }
            else if (Key == 0x20){                          // Space was pressed
                pc.printf("\n\rMoving sevros...");
                while(1){
                    Key=pc.getc();                                  //Get the keypad pressed
                    if((Key>=0x30)&&(Key<=0x39)){                   //See if it's a number (0-9)
                        pc.putc(Key);
                        myAX12[(Key-48)]->Set_Goal(60);
                        wait(0.5);
                        myAX12[(Key-48)]->Set_Goal(240);
                        wait(0.5);
                    }
                }
            }
        }
        Key = 0;                                            // Reset Key
        pc.printf("\n\rEnter new AX12+ ID...  ");
        while (Key != 0x0D){
            Key=pc.getc();                                  //Get the keypad pressed
            if((Key>=0x30)&&(Key<=0x39)){                   //See if it's a number (0-9)
                NewID=NewID*10+(Key-48);                    //Strip off ASCII code, convert to int & index
                pc.putc(Key);
            }
        }
        pc.printf("\n\rChanging AX12+ No. %d ID from %d to %d...\n\r",OldID,OldID,NewID);
        myAX12[OldID]->Set_ID(OldID,NewID);
    }
}
*/