config AX12
Fork of configure_ax12_test_bras_module by
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?
User | Revision | Line number | New 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 | */ |