clement lignie / configure_ax12_test_bras_module

config AX12

Fork of configure_ax12_test_bras_module by CRAC Team

main.cpp@2:91b6646ea994, 2017-02-01 (annotated)

Committer:
clementlignie
Date:
Wed Feb 01 13:42:30 2017 +0000
Revision:
2:91b6646ea994
Parent:
1:fce3323d874f 
config AX12

Who changed what in which revision?

UserRevisionLine numberNew contents of line
slowness 0:c03cffe402df 1 #include "mbed.h"
slowness 0:c03cffe402df 2 #include "AX12.h"
slowness 0:c03cffe402df 3
slowness 0:c03cffe402df 4 Timer t;
slowness 0:c03cffe402df 5
slowness 0:c03cffe402df 6 Serial pc(USBTX, USBRX); // tx, rx
slowness 0:c03cffe402df 7 int Key=0,mode=0;
slowness 0:c03cffe402df 8
slowness 0:c03cffe402df 9 // This function is called when a character goes into the RX buffer.
slowness 1:fce3323d874f 10 void rxCallback()
slowness 0:c03cffe402df 11 {
slowness 1:fce3323d874f 12 Key=pc.getc();
slowness 0:c03cffe402df 13 }
slowness 1:fce3323d874f 14
slowness 1:fce3323d874f 15
slowness 0:c03cffe402df 16 void menu()
slowness 0:c03cffe402df 17 {
slowness 0:c03cffe402df 18 pc.printf("------------------------------------------\n\n\r");
slowness 0:c03cffe402df 19 pc.printf("- MENU -\n\n\r");
slowness 0:c03cffe402df 20 pc.printf("------------------------------------------\n\n\r");
slowness 0:c03cffe402df 21 pc.printf("- S pour scanner l'ID AX12 et la vitesse -\n\n\r");
slowness 0:c03cffe402df 22 pc.printf("- V pour changer la vitesse de l'AX12 -\n\n\r");
slowness 0:c03cffe402df 23 pc.printf("- I pour changer l'ID de l'AX12 -\n\n\r");
slowness 0:c03cffe402df 24 pc.printf("- T pour faire un mouvement de test -\n\n\r");
slowness 0:c03cffe402df 25 pc.printf("- R pour revenir au parametre usine -\n\n\r");
slowness 0:c03cffe402df 26 pc.printf("- M pour revenir au menu -\n\n\r");
slowness 0:c03cffe402df 27 pc.printf("- A pour lire info d'un AX12 -\n\n\r");
slowness 0:c03cffe402df 28 pc.printf("------------------------------------------\n\n\r");
slowness 0:c03cffe402df 29 }
slowness 0:c03cffe402df 30
slowness 0:c03cffe402df 31 void ax12read_all_info(unsigned char start,unsigned char longueur)
slowness 0:c03cffe402df 32 {
slowness 0:c03cffe402df 33 AX12 *ptr_myAX12;
slowness 0:c03cffe402df 34 unsigned char numero_ID;
slowness 0:c03cffe402df 35 unsigned int i=0;
slowness 0:c03cffe402df 36 signed char status;
slowness 0:c03cffe402df 37 unsigned long vitesse,vitesseNew;
slowness 0:c03cffe402df 38 pc.printf("\rNumero de l'Id de l'AX12: ");
slowness 0:c03cffe402df 39 pc.scanf("%d",&numero_ID);
slowness 0:c03cffe402df 40 pc.printf("%d \n\n\r",numero_ID);
slowness 0:c03cffe402df 41 pc.printf("\rVitesse actuelle de l'AX12: ");
slowness 0:c03cffe402df 42 pc.scanf("%d",&vitesse);
slowness 0:c03cffe402df 43 pc.printf("%d \n\n\r",vitesse);
slowness 1:fce3323d874f 44 for(i=0; i<=0; i++) {
slowness 0:c03cffe402df 45 //vitesse=2000000/(i+1);
slowness 0:c03cffe402df 46 ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
slowness 0:c03cffe402df 47 status = -1; // Reset status variable
slowness 0:c03cffe402df 48 status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
slowness 1:fce3323d874f 49 if(1) { //status == numero_ID)
slowness 1:fce3323d874f 50 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
slowness 0:c03cffe402df 51 pc.printf("readinfo \n\n\r");
slowness 0:c03cffe402df 52 ptr_myAX12->read_all_info(start,longueur);
slowness 0:c03cffe402df 53 wait(1.);
slowness 1:fce3323d874f 54 } else
slowness 0:c03cffe402df 55 pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
slowness 0:c03cffe402df 56 delete ptr_myAX12;
slowness 1:fce3323d874f 57 }
slowness 0:c03cffe402df 58 }
slowness 0:c03cffe402df 59
slowness 0:c03cffe402df 60
slowness 0:c03cffe402df 61 void lecture()
slowness 0:c03cffe402df 62 {
slowness 0:c03cffe402df 63
slowness 0:c03cffe402df 64 }
slowness 0:c03cffe402df 65
slowness 0:c03cffe402df 66 void ax12reset()
slowness 0:c03cffe402df 67 {
slowness 0:c03cffe402df 68 AX12 *ptr_myAX12;
slowness 0:c03cffe402df 69 unsigned char numero_ID;
slowness 0:c03cffe402df 70 unsigned int i=0;
slowness 0:c03cffe402df 71 char status=-1;
slowness 0:c03cffe402df 72 unsigned long vitesse,vitesseNew;
slowness 0:c03cffe402df 73 pc.printf("\rNumero de l'Id de l'AX12: ");
slowness 0:c03cffe402df 74 pc.scanf("%d",&numero_ID);
slowness 0:c03cffe402df 75 pc.printf("%d \n\n\r",numero_ID);
slowness 0:c03cffe402df 76 pc.printf("\rVitesse actuelle de l'AX12: ");
slowness 0:c03cffe402df 77 pc.scanf("%d",&vitesse);
slowness 0:c03cffe402df 78 pc.printf("%d \n\n\r",vitesse);
slowness 1:fce3323d874f 79 for(i=0; i<=0; i++) {
slowness 0:c03cffe402df 80 //vitesse=2000000/(i+1);
slowness 0:c03cffe402df 81 ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
slowness 0:c03cffe402df 82 status = -1; // Reset status variable
slowness 0:c03cffe402df 83 status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
slowness 1:fce3323d874f 84 if(1) { //status == numero_ID)
slowness 1:fce3323d874f 85 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
slowness 0:c03cffe402df 86 pc.printf("reset \n\n\r");
slowness 0:c03cffe402df 87 ptr_myAX12->reset();
slowness 0:c03cffe402df 88 wait(1.);
slowness 1:fce3323d874f 89 } else
slowness 0:c03cffe402df 90 pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
slowness 0:c03cffe402df 91 delete ptr_myAX12;
slowness 1:fce3323d874f 92 }
slowness 0:c03cffe402df 93 }
slowness 1:fce3323d874f 94
slowness 0:c03cffe402df 95 void change_ident()
slowness 0:c03cffe402df 96 {
slowness 0:c03cffe402df 97 AX12 *ptr_myAX12;
slowness 0:c03cffe402df 98 unsigned int numero_ID,ID_new;
slowness 0:c03cffe402df 99 char status=-1;
slowness 0:c03cffe402df 100 unsigned long vitesse;
slowness 0:c03cffe402df 101 pc.printf("\rNumero de l'Id de l'AX12: ");
slowness 0:c03cffe402df 102 pc.scanf("%d",&numero_ID);
slowness 0:c03cffe402df 103 pc.printf("%d \n\n\r",numero_ID);
slowness 0:c03cffe402df 104 pc.printf("\rVitesse actuelle de l'AX12: ");
slowness 0:c03cffe402df 105 pc.scanf("%d",&vitesse);
slowness 0:c03cffe402df 106 pc.printf("%d \n\n\r",vitesse);
slowness 0:c03cffe402df 107 ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
slowness 0:c03cffe402df 108 status = -1; // Reset status variable
slowness 0:c03cffe402df 109 status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
slowness 1:fce3323d874f 110 if(1) { //status == numero_ID)
slowness 1:fce3323d874f 111 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
slowness 0:c03cffe402df 112 pc.printf("\rNouvelle ID de l'AX12: ");
slowness 0:c03cffe402df 113 pc.scanf("%d",&ID_new);
slowness 0:c03cffe402df 114 status = -1; // Reset status variable
slowness 0:c03cffe402df 115 status = ptr_myAX12->Set_ID(numero_ID,ID_new); // See if the AX12 exists at this address
slowness 0:c03cffe402df 116 delete ptr_myAX12;
slowness 0:c03cffe402df 117 ptr_myAX12 = new AX12(p9, p10, ID_new,vitesse); // Create objects
slowness 0:c03cffe402df 118 status = -1; // Reset status variable
slowness 0:c03cffe402df 119 status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
slowness 0:c03cffe402df 120 pc.printf("Changement de ID");
slowness 1:fce3323d874f 121 if(status == ID_new) {
slowness 1:fce3323d874f 122 pc.printf(" reussi \n\r"); // If it does print address and position to the PC
slowness 1:fce3323d874f 123 } else
slowness 1:fce3323d874f 124 pc.printf(" rate \n\r"); // If it does print address and position to the PC
slowness 1:fce3323d874f 125 delete ptr_myAX12;
slowness 1:fce3323d874f 126 } else {
slowness 1:fce3323d874f 127 pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
slowness 1:fce3323d874f 128
slowness 1:fce3323d874f 129
slowness 0:c03cffe402df 130 }
slowness 1:fce3323d874f 131
slowness 1:fce3323d874f 132
slowness 0:c03cffe402df 133 }
slowness 0:c03cffe402df 134
slowness 0:c03cffe402df 135 void vitesse()
slowness 0:c03cffe402df 136 {
slowness 0:c03cffe402df 137 AX12 *ptr_myAX12;
slowness 0:c03cffe402df 138 unsigned int numero_ID,baud;
slowness 0:c03cffe402df 139 char status=-1;
slowness 0:c03cffe402df 140 unsigned int vitesse,vitesseNew;
slowness 0:c03cffe402df 141 pc.printf("\rNumero de l'Id de l'AX12: ");
slowness 0:c03cffe402df 142 pc.scanf("%d",&numero_ID);
slowness 0:c03cffe402df 143 pc.printf("%d \n\n\r",numero_ID);
slowness 0:c03cffe402df 144 pc.printf("\rVitesse actuelle de l'AX12: ");
slowness 0:c03cffe402df 145 pc.scanf("%d",&vitesse);
slowness 0:c03cffe402df 146 pc.printf("%d \n\n\r",vitesse);
slowness 0:c03cffe402df 147 ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
slowness 0:c03cffe402df 148 status = -1; // Reset status variable
slowness 0:c03cffe402df 149 status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
slowness 1:fce3323d874f 150 if(status == numero_ID) {
slowness 1:fce3323d874f 151 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
slowness 0:c03cffe402df 152 pc.printf("Nouvelle Vitesse de l'AX12: ");
slowness 0:c03cffe402df 153 pc.scanf("%d",&vitesseNew);
slowness 0:c03cffe402df 154 baud=(2000000/vitesseNew)-1;
slowness 0:c03cffe402df 155 pc.printf("\n\r %d \n\r",baud);
slowness 0:c03cffe402df 156 status = -1; // Reset status variable
slowness 1:fce3323d874f 157 status = ptr_myAX12->Set_Baud(baud);
slowness 1:fce3323d874f 158
slowness 1:fce3323d874f 159 // See if the AX12 exists at this address
slowness 0:c03cffe402df 160 delete ptr_myAX12;
slowness 0:c03cffe402df 161 ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesseNew); // Create objects
slowness 0:c03cffe402df 162 status = -1; // Reset status variable
slowness 0:c03cffe402df 163 status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
slowness 0:c03cffe402df 164 pc.printf("Changement de vitesse");
slowness 1:fce3323d874f 165 if(status == numero_ID) {
slowness 1:fce3323d874f 166 pc.printf(" reussi \n\r"); // If it does print address and position to the PC
slowness 1:fce3323d874f 167 } else
slowness 1:fce3323d874f 168 pc.printf(" rate \n\r"); // If it does print address and position to the PC
slowness 1:fce3323d874f 169 delete ptr_myAX12;
slowness 1:fce3323d874f 170 } else {
slowness 1:fce3323d874f 171 pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
slowness 1:fce3323d874f 172
slowness 1:fce3323d874f 173
slowness 0:c03cffe402df 174 }
slowness 1:fce3323d874f 175
slowness 1:fce3323d874f 176
slowness 0:c03cffe402df 177 }
slowness 0:c03cffe402df 178
slowness 0:c03cffe402df 179 void test()
slowness 0:c03cffe402df 180 {
slowness 1:fce3323d874f 181 AX12 *bas_myAX12,*milieu_myAX12, *haut_myAX12;
slowness 0:c03cffe402df 182 unsigned char numero_ID;
clementlignie 2:91b6646ea994 183 int i=0;
slowness 0:c03cffe402df 184 char status=-1;
clementlignie 2:91b6646ea994 185 unsigned long vitesse=1023,vitesseNew,position=1;
clementlignie 2:91b6646ea994 186 bas_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects
clementlignie 2:91b6646ea994 187 milieu_myAX12 = new AX12(p9, p10, 2,1000000); // Create objects
slowness 1:fce3323d874f 188 haut_myAX12 = new AX12(p9, p10, 3,1000000); // Create objects
slowness 1:fce3323d874f 189 bas_myAX12->Set_Goal_speed(1023);//wait(0.2);
slowness 1:fce3323d874f 190 milieu_myAX12->Set_Goal_speed(1023);//wait(0.2);
slowness 1:fce3323d874f 191 haut_myAX12->Set_Goal_speed(1023);//wait(0.2);
slowness 1:fce3323d874f 192
slowness 1:fce3323d874f 193 do
slowness 0:c03cffe402df 194 {
slowness 1:fce3323d874f 195 pc.printf("numero de mouvement");
slowness 1:fce3323d874f 196 pc.scanf("%d",&position);
slowness 1:fce3323d874f 197
slowness 1:fce3323d874f 198
slowness 1:fce3323d874f 199 switch(position)
slowness 0:c03cffe402df 200 {
clementlignie 2:91b6646ea994 201 case 0:
clementlignie 2:91b6646ea994 202 bas_myAX12->Set_Secure_Goal(150);
clementlignie 2:91b6646ea994 203 milieu_myAX12->Set_Secure_Goal(150);
clementlignie 2:91b6646ea994 204 haut_myAX12->Set_Secure_Goal(150);
clementlignie 2:91b6646ea994 205 break;
clementlignie 2:91b6646ea994 206
slowness 1:fce3323d874f 207 case 1:
slowness 1:fce3323d874f 208 bas_myAX12->Set_Secure_Goal(150);
clementlignie 2:91b6646ea994 209 haut_myAX12->Set_Secure_Goal(150+55);
clementlignie 2:91b6646ea994 210 milieu_myAX12->Set_Secure_Goal(150-47);
clementlignie 2:91b6646ea994 211 //position=2;
slowness 1:fce3323d874f 212 break;
slowness 1:fce3323d874f 213
slowness 1:fce3323d874f 214 case 2:
clementlignie 2:91b6646ea994 215 haut_myAX12->Set_Secure_Goal(150+65);
clementlignie 2:91b6646ea994 216 milieu_myAX12->Set_Secure_Goal(150-60);
slowness 1:fce3323d874f 217 bas_myAX12->Set_Secure_Goal(60);
clementlignie 2:91b6646ea994 218 //position=3;
slowness 1:fce3323d874f 219 break;
slowness 1:fce3323d874f 220
slowness 1:fce3323d874f 221 case 3:
slowness 1:fce3323d874f 222 milieu_myAX12->Set_Secure_Goal(150-90);
clementlignie 2:91b6646ea994 223 bas_myAX12->Set_Secure_Goal(150-45);
slowness 1:fce3323d874f 224 haut_myAX12->Set_Secure_Goal(150+45);
clementlignie 2:91b6646ea994 225 //position=4;
slowness 1:fce3323d874f 226 break;
slowness 1:fce3323d874f 227
slowness 1:fce3323d874f 228 case 4:
clementlignie 2:91b6646ea994 229 bas_myAX12->Set_Secure_Goal(150-60);
clementlignie 2:91b6646ea994 230
clementlignie 2:91b6646ea994 231 for (i=0; i<5; i++)
clementlignie 2:91b6646ea994 232 {
clementlignie 2:91b6646ea994 233 haut_myAX12->Set_Secure_Goal((150+45)-(i*14)); //150 4.5
clementlignie 2:91b6646ea994 234 milieu_myAX12->Set_Secure_Goal((150-90)+(i*8)); //150-45 4.5
clementlignie 2:91b6646ea994 235 bas_myAX12->Set_Secure_Goal((150-60)+(i*9)); //150-40 2
clementlignie 2:91b6646ea994 236 }
clementlignie 2:91b6646ea994 237 //position=5;
slowness 1:fce3323d874f 238 break;
slowness 1:fce3323d874f 239
slowness 1:fce3323d874f 240 case 5:
clementlignie 2:91b6646ea994 241 bas_myAX12->Set_Secure_Goal(150);
clementlignie 2:91b6646ea994 242 milieu_myAX12->Set_Secure_Goal(150-80);
clementlignie 2:91b6646ea994 243 haut_myAX12->Set_Secure_Goal(150);
clementlignie 2:91b6646ea994 244 //position=1;
clementlignie 2:91b6646ea994 245 break;
clementlignie 2:91b6646ea994 246
slowness 1:fce3323d874f 247
slowness 0:c03cffe402df 248 }
slowness 1:fce3323d874f 249 //wait(0.2);
slowness 1:fce3323d874f 250 }while(pc.readable()==0);
slowness 1:fce3323d874f 251
slowness 1:fce3323d874f 252 }
slowness 0:c03cffe402df 253
slowness 0:c03cffe402df 254 void scan()
slowness 0:c03cffe402df 255 {
slowness 0:c03cffe402df 256 AX12 *myAX12[100];
slowness 0:c03cffe402df 257 int status=0,cpt=0;
slowness 1:fce3323d874f 258 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};//
slowness 0:c03cffe402df 259 // Print msg to PC
slowness 1:fce3323d874f 260 pc.printf("Scanning chain for AX12+ Servos...\n\n\r");
slowness 1:fce3323d874f 261 // See which addresses contain AX12+ servos
slowness 1:fce3323d874f 262 for(baud=7; baud<11; baud++) {
slowness 1:fce3323d874f 263 baudrate=2000000/(tabbaud[baud]+1);
slowness 1:fce3323d874f 264 find_servo=0;
slowness 1:fce3323d874f 265 pc.printf("\r\n");
slowness 1:fce3323d874f 266 pc.printf("\rcheck servo in %d baudrate\r\n",baudrate);
slowness 1:fce3323d874f 267 for (int i = 0; i < 254; i++) {
slowness 1:fce3323d874f 268 myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects
slowness 1:fce3323d874f 269 pc.printf("\r%03d",i);
slowness 1:fce3323d874f 270 status = -1; // Reset status variable
slowness 1:fce3323d874f 271 status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address
slowness 1:fce3323d874f 272 if(status == i) {
slowness 1:fce3323d874f 273 pc.printf("\rServo found at address %d and baudrate %d\r\n",i,baudrate); // If it does print address and position to the PC
slowness 1:fce3323d874f 274 find_servo=find_servo++;
slowness 1:fce3323d874f 275 cpt++;
slowness 1:fce3323d874f 276 } else {
slowness 1:fce3323d874f 277 delete myAX12[cpt];
slowness 1:fce3323d874f 278 /*if(i%16==0)
slowness 1:fce3323d874f 279 pc.printf("\r");
slowness 1:fce3323d874f 280 */
slowness 0:c03cffe402df 281 }
slowness 0:c03cffe402df 282 }
slowness 1:fce3323d874f 283 if(find_servo==0)
slowness 1:fce3323d874f 284 pc.printf("\rno AX12 in this baudrate=%d\n\r",baudrate);
slowness 1:fce3323d874f 285 //wait(2.);
slowness 1:fce3323d874f 286 }
slowness 1:fce3323d874f 287
slowness 0:c03cffe402df 288 }
slowness 0:c03cffe402df 289
slowness 0:c03cffe402df 290 void scan_all()
slowness 0:c03cffe402df 291 {
slowness 0:c03cffe402df 292 AX12 *myAX12[15];
slowness 0:c03cffe402df 293 int status=0,cpt=0,nombre_servo;
slowness 1:fce3323d874f 294 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};//
slowness 0:c03cffe402df 295 // Print msg to PC
slowness 1:fce3323d874f 296 pc.printf("Scanning chain for AX12+ Servos not more than 15 servos ...\n\n\r");
slowness 1:fce3323d874f 297 pc.printf("combien de servo à chercher? ");
slowness 1:fce3323d874f 298 pc.attach(NULL, Serial::RxIrq);
slowness 1:fce3323d874f 299 pc.scanf("%d",&nombre_servo);
slowness 1:fce3323d874f 300 pc.attach(&rxCallback, Serial::RxIrq);// See which addresses contain AX12+ servos
slowness 1:fce3323d874f 301 for(baud=0; baud<=255; baud++) {
slowness 1:fce3323d874f 302 if((Key=='q')||(Key=='Q')) {
slowness 1:fce3323d874f 303 Key=0;
slowness 1:fce3323d874f 304 break;
slowness 1:fce3323d874f 305 }
slowness 1:fce3323d874f 306 baudrate=2000000/(baud+1);
slowness 1:fce3323d874f 307 find_servo=0;
slowness 1:fce3323d874f 308 pc.printf("\r\n");
slowness 1:fce3323d874f 309 for (int i = 0; i < 253; i++) {
slowness 1:fce3323d874f 310 myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects
slowness 1:fce3323d874f 311 pc.printf("\r%03d",i);
slowness 1:fce3323d874f 312 status = -1; // Reset status variable
slowness 1:fce3323d874f 313 status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address
slowness 1:fce3323d874f 314 if(status == i) {
slowness 1:fce3323d874f 315 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
slowness 1:fce3323d874f 316 find_servo=find_servo++;
slowness 1:fce3323d874f 317 cpt++;
slowness 1:fce3323d874f 318
slowness 1:fce3323d874f 319 } else {
slowness 1:fce3323d874f 320 delete myAX12[cpt];
slowness 1:fce3323d874f 321 /*if(i%16==0)
slowness 1:fce3323d874f 322 pc.printf("\r");
slowness 1:fce3323d874f 323 */
slowness 0:c03cffe402df 324 }
slowness 1:fce3323d874f 325 wait(0.001);
slowness 0:c03cffe402df 326 }
slowness 1:fce3323d874f 327 if(cpt>=nombre_servo)
slowness 1:fce3323d874f 328 break;
slowness 1:fce3323d874f 329 if(find_servo==0)
slowness 1:fce3323d874f 330 pc.printf("\r %03d no AX12 in this baudrate=%d\n\r",baud,baudrate);
slowness 1:fce3323d874f 331
slowness 1:fce3323d874f 332 }
slowness 1:fce3323d874f 333
slowness 0:c03cffe402df 334 }
slowness 0:c03cffe402df 335
slowness 1:fce3323d874f 336
slowness 1:fce3323d874f 337
slowness 1:fce3323d874f 338 int main()
slowness 1:fce3323d874f 339 {
slowness 0:c03cffe402df 340 int NewID=0, OldID=0;
slowness 0:c03cffe402df 341 t.start();
slowness 0:c03cffe402df 342 menu();
slowness 0:c03cffe402df 343 pc.attach(&rxCallback, Serial::RxIrq);
slowness 1:fce3323d874f 344 while(1) {
slowness 1:fce3323d874f 345 switch(Key) {
slowness 0:c03cffe402df 346 case 's':
slowness 1:fce3323d874f 347 case 'S':
slowness 1:fce3323d874f 348 Key=0;
slowness 1:fce3323d874f 349 scan_all();
slowness 1:fce3323d874f 350 menu();
slowness 1:fce3323d874f 351 break;
slowness 1:fce3323d874f 352
slowness 0:c03cffe402df 353 case 'l':
slowness 1:fce3323d874f 354 case 'L':
slowness 1:fce3323d874f 355 Key=0;
slowness 1:fce3323d874f 356 lecture();
slowness 1:fce3323d874f 357 menu();
slowness 1:fce3323d874f 358 break;
slowness 1:fce3323d874f 359 case 'm':
slowness 1:fce3323d874f 360 case 'M':
slowness 1:fce3323d874f 361 Key=0;
slowness 1:fce3323d874f 362 menu();
slowness 1:fce3323d874f 363 break;
slowness 1:fce3323d874f 364
slowness 1:fce3323d874f 365 case 'v':
slowness 1:fce3323d874f 366 case 'V':
slowness 1:fce3323d874f 367 Key=0;
slowness 1:fce3323d874f 368 pc.attach(NULL, Serial::RxIrq);
slowness 1:fce3323d874f 369 vitesse();
slowness 1:fce3323d874f 370 pc.attach(&rxCallback, Serial::RxIrq);
slowness 1:fce3323d874f 371 menu();
slowness 1:fce3323d874f 372 break;
slowness 1:fce3323d874f 373
slowness 1:fce3323d874f 374 case 'i':
slowness 1:fce3323d874f 375 case 'I':
slowness 1:fce3323d874f 376 Key=0;
slowness 1:fce3323d874f 377 pc.attach(NULL, Serial::RxIrq);
slowness 1:fce3323d874f 378 change_ident();
slowness 1:fce3323d874f 379 pc.attach(&rxCallback, Serial::RxIrq);
slowness 1:fce3323d874f 380 menu();
slowness 1:fce3323d874f 381 break;
slowness 1:fce3323d874f 382 case 't':
slowness 1:fce3323d874f 383 case 'T':
slowness 1:fce3323d874f 384 Key=0;
slowness 1:fce3323d874f 385 pc.attach(NULL, Serial::RxIrq);
slowness 1:fce3323d874f 386 test();
slowness 1:fce3323d874f 387 pc.attach(&rxCallback, Serial::RxIrq);
slowness 1:fce3323d874f 388 break;
slowness 1:fce3323d874f 389
slowness 1:fce3323d874f 390 case 'r':
slowness 1:fce3323d874f 391 case 'R':
slowness 1:fce3323d874f 392 Key=0;
slowness 1:fce3323d874f 393 pc.attach(NULL, Serial::RxIrq);
slowness 1:fce3323d874f 394 ax12reset();
slowness 1:fce3323d874f 395 pc.attach(&rxCallback, Serial::RxIrq);
slowness 1:fce3323d874f 396 break;
slowness 1:fce3323d874f 397
slowness 1:fce3323d874f 398 case 'a':
slowness 1:fce3323d874f 399 case 'A':
slowness 1:fce3323d874f 400 Key=0;
slowness 1:fce3323d874f 401 pc.attach(NULL, Serial::RxIrq);
slowness 1:fce3323d874f 402 ax12read_all_info(0,8);
slowness 1:fce3323d874f 403 ax12read_all_info(10,10);
slowness 1:fce3323d874f 404 ax12read_all_info(20,10);
slowness 1:fce3323d874f 405 ax12read_all_info(30,10);
slowness 1:fce3323d874f 406 ax12read_all_info(40,10);
slowness 1:fce3323d874f 407
slowness 1:fce3323d874f 408 pc.attach(&rxCallback, Serial::RxIrq);
slowness 1:fce3323d874f 409 break;
slowness 1:fce3323d874f 410
slowness 1:fce3323d874f 411 default:
slowness 1:fce3323d874f 412 break;
slowness 1:fce3323d874f 413
slowness 0:c03cffe402df 414 }
slowness 0:c03cffe402df 415 }
slowness 0:c03cffe402df 416 }
slowness 1:fce3323d874f 417
slowness 1:fce3323d874f 418 // Print msg to PC
slowness 0:c03cffe402df 419 /* pc.printf("\nEnter AX12+ ID to Alter... ");
slowness 0:c03cffe402df 420 while (Key != 0x0D){
slowness 0:c03cffe402df 421 Key=pc.getc(); //Get the keypad pressed
slowness 0:c03cffe402df 422 if((Key>=0x30)&&(Key<=0x39)){ //See if it's a number (0-9)
slowness 0:c03cffe402df 423 OldID=OldID*10+(Key-48); //Strip off ASCII code, convert to int & index
slowness 0:c03cffe402df 424 pc.putc(Key);
slowness 0:c03cffe402df 425 }
slowness 0:c03cffe402df 426 else if (Key == 0x20){ // Space was pressed
slowness 0:c03cffe402df 427 pc.printf("\n\rMoving sevros...");
slowness 0:c03cffe402df 428 while(1){
slowness 0:c03cffe402df 429 Key=pc.getc(); //Get the keypad pressed
slowness 0:c03cffe402df 430 if((Key>=0x30)&&(Key<=0x39)){ //See if it's a number (0-9)
slowness 0:c03cffe402df 431 pc.putc(Key);
slowness 0:c03cffe402df 432 myAX12[(Key-48)]->Set_Goal(60);
slowness 0:c03cffe402df 433 wait(0.5);
slowness 0:c03cffe402df 434 myAX12[(Key-48)]->Set_Goal(240);
slowness 0:c03cffe402df 435 wait(0.5);
slowness 0:c03cffe402df 436 }
slowness 0:c03cffe402df 437 }
slowness 0:c03cffe402df 438 }
slowness 0:c03cffe402df 439 }
slowness 0:c03cffe402df 440 Key = 0; // Reset Key
slowness 0:c03cffe402df 441 pc.printf("\n\rEnter new AX12+ ID... ");
slowness 0:c03cffe402df 442 while (Key != 0x0D){
slowness 0:c03cffe402df 443 Key=pc.getc(); //Get the keypad pressed
slowness 0:c03cffe402df 444 if((Key>=0x30)&&(Key<=0x39)){ //See if it's a number (0-9)
slowness 0:c03cffe402df 445 NewID=NewID*10+(Key-48); //Strip off ASCII code, convert to int & index
slowness 0:c03cffe402df 446 pc.putc(Key);
slowness 0:c03cffe402df 447 }
slowness 0:c03cffe402df 448 }
slowness 0:c03cffe402df 449 pc.printf("\n\rChanging AX12+ No. %d ID from %d to %d...\n\r",OldID,OldID,NewID);
slowness 0:c03cffe402df 450 myAX12[OldID]->Set_ID(OldID,NewID);
slowness 0:c03cffe402df 451 }
slowness 0:c03cffe402df 452 }
slowness 0:c03cffe402df 453 */