config AX12
Fork of configure_ax12_test_bras_module by
main.cpp
- Committer:
- clementlignie
- Date:
- 2017-02-01
- Revision:
- 2:91b6646ea994
- Parent:
- 1:fce3323d874f
File content as of revision 2:91b6646ea994:
#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; int i=0; char status=-1; unsigned long vitesse=1023,vitesseNew,position=1; bas_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects milieu_myAX12 = new AX12(p9, p10, 2,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 0: bas_myAX12->Set_Secure_Goal(150); milieu_myAX12->Set_Secure_Goal(150); haut_myAX12->Set_Secure_Goal(150); break; case 1: bas_myAX12->Set_Secure_Goal(150); haut_myAX12->Set_Secure_Goal(150+55); milieu_myAX12->Set_Secure_Goal(150-47); //position=2; break; case 2: haut_myAX12->Set_Secure_Goal(150+65); milieu_myAX12->Set_Secure_Goal(150-60); bas_myAX12->Set_Secure_Goal(60); //position=3; break; case 3: milieu_myAX12->Set_Secure_Goal(150-90); bas_myAX12->Set_Secure_Goal(150-45); haut_myAX12->Set_Secure_Goal(150+45); //position=4; break; case 4: bas_myAX12->Set_Secure_Goal(150-60); for (i=0; i<5; i++) { haut_myAX12->Set_Secure_Goal((150+45)-(i*14)); //150 4.5 milieu_myAX12->Set_Secure_Goal((150-90)+(i*8)); //150-45 4.5 bas_myAX12->Set_Secure_Goal((150-60)+(i*9)); //150-40 2 } //position=5; break; case 5: bas_myAX12->Set_Secure_Goal(150); milieu_myAX12->Set_Secure_Goal(150-80); haut_myAX12->Set_Secure_Goal(150); //position=1; 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); } } */