main.cpp

00001 #include "mbed.h"
00002 #include "AX12.h"
00003 
00004 Timer t;
00005 
00006 Serial pc(USBTX, USBRX); // tx, rx
00007 int Key=0,mode=0;
00008 
00009 // This function is called when a character goes into the RX buffer.
00010 void rxCallback()
00011 {
00012     Key=pc.getc();
00013 }
00014 
00015 
00016 void menu()
00017 {
00018     pc.printf("------------------------------------------\n\n\r");
00019     pc.printf("-                   MENU                 -\n\n\r");
00020     pc.printf("------------------------------------------\n\n\r");
00021     pc.printf("- S pour scanner l'ID AX12 et la vitesse -\n\n\r");
00022     pc.printf("- V pour changer la vitesse de l'AX12    -\n\n\r");
00023     pc.printf("- I pour changer l'ID de l'AX12          -\n\n\r");
00024     pc.printf("- T pour faire un mouvement de test      -\n\n\r");
00025     pc.printf("- R pour revenir au parametre usine      -\n\n\r");
00026     pc.printf("- M pour revenir au menu                 -\n\n\r");
00027     pc.printf("- A pour lire info d'un AX12             -\n\n\r");
00028     pc.printf("------------------------------------------\n\n\r");
00029 }
00030 
00031 void ax12read_all_info(unsigned char start,unsigned char longueur)
00032 {
00033     AX12 *ptr_myAX12;
00034     unsigned char numero_ID;
00035     unsigned int i=0;
00036     signed char status;
00037     unsigned long vitesse,vitesseNew;
00038     pc.printf("\rNumero de l'Id de l'AX12: ");
00039     pc.scanf("%d",&numero_ID);
00040     pc.printf("%d \n\n\r",numero_ID);
00041     pc.printf("\rVitesse actuelle de l'AX12: ");
00042     pc.scanf("%d",&vitesse);
00043     pc.printf("%d \n\n\r",vitesse);
00044     for(i=0; i<=0; i++) {
00045         //vitesse=2000000/(i+1);
00046         ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
00047         status = -1;                                          // Reset status variable
00048         status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
00049         if(1) { //status == numero_ID)
00050             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
00051             pc.printf("readinfo \n\n\r");
00052             ptr_myAX12->read_all_info(start,longueur);
00053             wait(1.);
00054         } else
00055             pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
00056         delete ptr_myAX12;
00057     }
00058 }
00059 
00060 
00061 void lecture()
00062 {
00063 
00064 }
00065 
00066 void ax12reset()
00067 {
00068     AX12 *ptr_myAX12;
00069     unsigned char numero_ID;
00070     unsigned int i=0;
00071     char status=-1;
00072     unsigned long vitesse,vitesseNew;
00073     pc.printf("\rNumero de l'Id de l'AX12: ");
00074     pc.scanf("%d",&numero_ID);
00075     pc.printf("%d \n\n\r",numero_ID);
00076     pc.printf("\rVitesse actuelle de l'AX12: ");
00077     pc.scanf("%d",&vitesse);
00078     pc.printf("%d \n\n\r",vitesse);
00079     for(i=0; i<=0; i++) {
00080         //vitesse=2000000/(i+1);
00081         ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
00082         status = -1;                                          // Reset status variable
00083         status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
00084         if(1) { //status == numero_ID)
00085             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
00086             pc.printf("reset \n\n\r");
00087             ptr_myAX12->reset();
00088             wait(1.);
00089         } else
00090             pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
00091         delete ptr_myAX12;
00092     }
00093 }
00094 
00095 void change_ident()
00096 {
00097     AX12 *ptr_myAX12;
00098     unsigned int numero_ID,ID_new;
00099     char status=-1;
00100     unsigned long vitesse;
00101     pc.printf("\rNumero de l'Id de l'AX12: ");
00102     pc.scanf("%d",&numero_ID);
00103     pc.printf("%d \n\n\r",numero_ID);
00104     pc.printf("\rVitesse actuelle de l'AX12: ");
00105     pc.scanf("%d",&vitesse);
00106     pc.printf("%d \n\n\r",vitesse);
00107     ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
00108     status = -1;                                          // Reset status variable
00109     status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
00110     if(1) { //status == numero_ID)
00111         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
00112         pc.printf("\rNouvelle ID de l'AX12: ");
00113         pc.scanf("%d",&ID_new);
00114         status = -1;                                          // Reset status variable
00115         status = ptr_myAX12->Set_ID(numero_ID,ID_new);                   // See if the AX12 exists at this address
00116         delete ptr_myAX12;
00117         ptr_myAX12 = new AX12(p9, p10, ID_new,vitesse);                  // Create objects
00118         status = -1;                                          // Reset status variable
00119         status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
00120         pc.printf("Changement de ID");
00121         if(status == ID_new) {
00122             pc.printf(" reussi \n\r");    // If it does print address and position to the PC
00123         } else
00124             pc.printf(" rate \n\r");    // If it does print address and position to the PC
00125         delete ptr_myAX12;
00126     } else {
00127         pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
00128 
00129 
00130     }
00131 
00132 
00133 }
00134 
00135 void vitesse()
00136 {
00137     AX12 *ptr_myAX12;
00138     unsigned int numero_ID,baud;
00139     char status=-1;
00140     unsigned int vitesse,vitesseNew;
00141     pc.printf("\rNumero de l'Id de l'AX12: ");
00142     pc.scanf("%d",&numero_ID);
00143     pc.printf("%d \n\n\r",numero_ID);
00144     pc.printf("\rVitesse actuelle de l'AX12: ");
00145     pc.scanf("%d",&vitesse);
00146     pc.printf("%d \n\n\r",vitesse);
00147     ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse);                  // Create objects
00148     status = -1;                                          // Reset status variable
00149     status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
00150     if(status == numero_ID) {
00151         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
00152         pc.printf("Nouvelle Vitesse de l'AX12: ");
00153         pc.scanf("%d",&vitesseNew);
00154         baud=(2000000/vitesseNew)-1;
00155         pc.printf("\n\r %d \n\r",baud);
00156         status = -1;                                          // Reset status variable
00157         status = ptr_myAX12->Set_Baud(baud);
00158 
00159         // See if the AX12 exists at this address
00160         delete ptr_myAX12;
00161         ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesseNew);                  // Create objects
00162         status = -1;                                          // Reset status variable
00163         status = ptr_myAX12->Get_ID();                   // See if the AX12 exists at this address
00164         pc.printf("Changement de vitesse");
00165         if(status == numero_ID) {
00166             pc.printf(" reussi \n\r");    // If it does print address and position to the PC
00167         } else
00168             pc.printf(" rate \n\r");    // If it does print address and position to the PC
00169         delete ptr_myAX12;
00170     } else {
00171         pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
00172 
00173 
00174     }
00175 
00176 
00177 }
00178 
00179 void test()
00180 {
00181     AX12 *bas_myAX12,*milieu_myAX12, *haut_myAX12;
00182     unsigned char numero_ID;
00183     int i=0;
00184     char status=-1;
00185     unsigned long vitesse=1023,vitesseNew,position=1;
00186     bas_myAX12 = new AX12(p9, p10, 1,1000000);                  // Create objects
00187     milieu_myAX12 = new AX12(p9, p10, 2,1000000);                  // Create objects
00188     haut_myAX12 = new AX12(p9, p10, 3,1000000);                  // Create objects
00189     bas_myAX12->Set_Goal_speed(1023);//wait(0.2);
00190     milieu_myAX12->Set_Goal_speed(1023);//wait(0.2);
00191     haut_myAX12->Set_Goal_speed(1023);//wait(0.2);
00192 
00193     do 
00194     {
00195         pc.printf("numero de mouvement");
00196         pc.scanf("%d",&position);
00197 
00198 
00199         switch(position) 
00200         {
00201              case 0:
00202                 bas_myAX12->Set_Secure_Goal(150);
00203                 milieu_myAX12->Set_Secure_Goal(150);
00204                 haut_myAX12->Set_Secure_Goal(150);
00205                 break;
00206                 
00207             case 1:
00208                 bas_myAX12->Set_Secure_Goal(150);
00209                 haut_myAX12->Set_Secure_Goal(150+55);
00210                 milieu_myAX12->Set_Secure_Goal(150-47);
00211                 //position=2;
00212                 break;
00213             
00214             case 2:
00215                 haut_myAX12->Set_Secure_Goal(150+65);
00216                 milieu_myAX12->Set_Secure_Goal(150-60);
00217                 bas_myAX12->Set_Secure_Goal(60);
00218                 //position=3;
00219                 break;
00220                 
00221             case 3:
00222                 milieu_myAX12->Set_Secure_Goal(150-90);
00223                 bas_myAX12->Set_Secure_Goal(150-45);
00224                 haut_myAX12->Set_Secure_Goal(150+45);
00225                 //position=4;
00226                 break;
00227                 
00228             case 4:
00229                 bas_myAX12->Set_Secure_Goal(150-60);
00230                 
00231                 for (i=0; i<5; i++)
00232                 {
00233                 haut_myAX12->Set_Secure_Goal((150+45)-(i*14)); //150    4.5
00234                 milieu_myAX12->Set_Secure_Goal((150-90)+(i*8)); //150-45   4.5
00235                 bas_myAX12->Set_Secure_Goal((150-60)+(i*9));  //150-40     2
00236                 }
00237                 //position=5;
00238                 break;
00239                 
00240             case 5:
00241                 bas_myAX12->Set_Secure_Goal(150);
00242                 milieu_myAX12->Set_Secure_Goal(150-80);
00243                 haut_myAX12->Set_Secure_Goal(150);
00244                 //position=1;
00245                 break;   
00246                 
00247 
00248         }
00249         //wait(0.2);
00250     }while(pc.readable()==0);
00251  
00252  }
00253 
00254 void scan()
00255 {
00256     AX12 *myAX12[100];
00257     int status=0,cpt=0;
00258     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};//
00259 // Print msg to PC
00260     pc.printf("Scanning chain for AX12+ Servos...\n\n\r");
00261     // See which addresses contain AX12+ servos
00262     for(baud=7; baud<11; baud++) {
00263         baudrate=2000000/(tabbaud[baud]+1);
00264         find_servo=0;
00265         pc.printf("\r\n");
00266         pc.printf("\rcheck servo in %d baudrate\r\n",baudrate);
00267         for (int  i = 0; i < 254; i++) {
00268             myAX12[cpt] = new AX12(p9, p10, i,baudrate);                  // Create objects
00269             pc.printf("\r%03d",i);
00270             status = -1;                                          // Reset status variable
00271             status = myAX12[cpt]->Get_ID();                   // See if the AX12 exists at this address
00272             if(status == i) {
00273                 pc.printf("\rServo found at address %d and baudrate %d\r\n",i,baudrate);    // If it does print address and position to the PC
00274                 find_servo=find_servo++;
00275                 cpt++;
00276             } else {
00277                 delete myAX12[cpt];
00278                 /*if(i%16==0)
00279                     pc.printf("\r");
00280                 */
00281             }
00282         }
00283         if(find_servo==0)
00284             pc.printf("\rno AX12 in this baudrate=%d\n\r",baudrate);
00285         //wait(2.);
00286     }
00287 
00288 }
00289 
00290 void scan_all()
00291 {
00292     AX12 *myAX12[15];
00293     int status=0,cpt=0,nombre_servo;
00294     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};//
00295 // Print msg to PC
00296     pc.printf("Scanning chain for AX12+ Servos not more than 15 servos ...\n\n\r");
00297     pc.printf("combien de servo à chercher? ");
00298     pc.attach(NULL, Serial::RxIrq);
00299     pc.scanf("%d",&nombre_servo);
00300     pc.attach(&rxCallback, Serial::RxIrq);// See which addresses contain AX12+ servos
00301     for(baud=0; baud<=255; baud++) {
00302         if((Key=='q')||(Key=='Q')) {
00303             Key=0;
00304             break;
00305         }
00306         baudrate=2000000/(baud+1);
00307         find_servo=0;
00308         pc.printf("\r\n");
00309         for (int  i = 0; i < 253; i++) {
00310             myAX12[cpt] = new AX12(p9, p10, i,baudrate);                  // Create objects
00311             pc.printf("\r%03d",i);
00312             status = -1;                                          // Reset status variable
00313             status = myAX12[cpt]->Get_ID();                   // See if the AX12 exists at this address
00314             if(status == i) {
00315                 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
00316                 find_servo=find_servo++;
00317                 cpt++;
00318 
00319             } else {
00320                 delete myAX12[cpt];
00321                 /*if(i%16==0)
00322                     pc.printf("\r");
00323                 */
00324             }
00325             wait(0.001);
00326         }
00327         if(cpt>=nombre_servo)
00328             break;
00329         if(find_servo==0)
00330             pc.printf("\r %03d no AX12 in this baudrate=%d\n\r",baud,baudrate);
00331 
00332     }
00333 
00334 }
00335 
00336 
00337 
00338 int main()
00339 {
00340     int NewID=0, OldID=0;
00341     t.start();
00342     menu();
00343     pc.attach(&rxCallback, Serial::RxIrq);
00344     while(1) {
00345         switch(Key) {
00346             case 's':
00347             case 'S':
00348                 Key=0;
00349                 scan_all();
00350                 menu();
00351                 break;
00352 
00353             case 'l':
00354             case 'L':
00355                 Key=0;
00356                 lecture();
00357                 menu();
00358                 break;
00359             case 'm':
00360             case 'M':
00361                 Key=0;
00362                 menu();
00363                 break;
00364 
00365             case 'v':
00366             case 'V':
00367                 Key=0;
00368                 pc.attach(NULL, Serial::RxIrq);
00369                 vitesse();
00370                 pc.attach(&rxCallback, Serial::RxIrq);
00371                 menu();
00372                 break;
00373 
00374             case 'i':
00375             case 'I':
00376                 Key=0;
00377                 pc.attach(NULL, Serial::RxIrq);
00378                 change_ident();
00379                 pc.attach(&rxCallback, Serial::RxIrq);
00380                 menu();
00381                 break;
00382             case 't':
00383             case 'T':
00384                 Key=0;
00385                 pc.attach(NULL, Serial::RxIrq);
00386                 test();
00387                 pc.attach(&rxCallback, Serial::RxIrq);
00388                 break;
00389 
00390             case 'r':
00391             case 'R':
00392                 Key=0;
00393                 pc.attach(NULL, Serial::RxIrq);
00394                 ax12reset();
00395                 pc.attach(&rxCallback, Serial::RxIrq);
00396                 break;
00397 
00398             case 'a':
00399             case 'A':
00400                 Key=0;
00401                 pc.attach(NULL, Serial::RxIrq);
00402                 ax12read_all_info(0,8);
00403                 ax12read_all_info(10,10);
00404                 ax12read_all_info(20,10);
00405                 ax12read_all_info(30,10);
00406                 ax12read_all_info(40,10);
00407 
00408                 pc.attach(&rxCallback, Serial::RxIrq);
00409                 break;
00410 
00411             default:
00412                 break;
00413 
00414         }
00415     }
00416 }
00417 
00418 // Print msg to PC
00419 /*        pc.printf("\nEnter AX12+ ID to Alter...  ");
00420         while (Key != 0x0D){
00421             Key=pc.getc();                                  //Get the keypad pressed
00422             if((Key>=0x30)&&(Key<=0x39)){                   //See if it's a number (0-9)
00423                 OldID=OldID*10+(Key-48);                    //Strip off ASCII code, convert to int & index
00424                 pc.putc(Key);
00425             }
00426             else if (Key == 0x20){                          // Space was pressed
00427                 pc.printf("\n\rMoving sevros...");
00428                 while(1){
00429                     Key=pc.getc();                                  //Get the keypad pressed
00430                     if((Key>=0x30)&&(Key<=0x39)){                   //See if it's a number (0-9)
00431                         pc.putc(Key);
00432                         myAX12[(Key-48)]->Set_Goal(60);
00433                         wait(0.5);
00434                         myAX12[(Key-48)]->Set_Goal(240);
00435                         wait(0.5);
00436                     }
00437                 }
00438             }
00439         }
00440         Key = 0;                                            // Reset Key
00441         pc.printf("\n\rEnter new AX12+ ID...  ");
00442         while (Key != 0x0D){
00443             Key=pc.getc();                                  //Get the keypad pressed
00444             if((Key>=0x30)&&(Key<=0x39)){                   //See if it's a number (0-9)
00445                 NewID=NewID*10+(Key-48);                    //Strip off ASCII code, convert to int & index
00446                 pc.putc(Key);
00447             }
00448         }
00449         pc.printf("\n\rChanging AX12+ No. %d ID from %d to %d...\n\r",OldID,OldID,NewID);
00450         myAX12[OldID]->Set_ID(OldID,NewID);
00451     }
00452 }
00453 */