config AX12
Fork of configure_ax12_test_bras_module by
main.cpp@1:fce3323d874f, 2016-11-25 (annotated)
- Committer:
- slowness
- Date:
- Fri Nov 25 17:38:14 2016 +0000
- Revision:
- 1:fce3323d874f
- Parent:
- 0:c03cffe402df
- Child:
- 2:91b6646ea994
mouvement du bras
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; |
slowness | 0:c03cffe402df | 183 | char status=-1; |
slowness | 1:fce3323d874f | 184 | unsigned long vitesse=1023,vitesseNew,position; |
slowness | 1:fce3323d874f | 185 | bas_myAX12 = new AX12(p9, p10, 2,1000000); // Create objects |
slowness | 1:fce3323d874f | 186 | milieu_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects |
slowness | 1:fce3323d874f | 187 | haut_myAX12 = new AX12(p9, p10, 3,1000000); // Create objects |
slowness | 1:fce3323d874f | 188 | bas_myAX12->Set_Goal_speed(1023);//wait(0.2); |
slowness | 1:fce3323d874f | 189 | milieu_myAX12->Set_Goal_speed(1023);//wait(0.2); |
slowness | 1:fce3323d874f | 190 | haut_myAX12->Set_Goal_speed(1023);//wait(0.2); |
slowness | 1:fce3323d874f | 191 | |
slowness | 1:fce3323d874f | 192 | do |
slowness | 0:c03cffe402df | 193 | { |
slowness | 1:fce3323d874f | 194 | pc.printf("numero de mouvement"); |
slowness | 1:fce3323d874f | 195 | pc.scanf("%d",&position); |
slowness | 1:fce3323d874f | 196 | |
slowness | 1:fce3323d874f | 197 | |
slowness | 1:fce3323d874f | 198 | switch(position) |
slowness | 0:c03cffe402df | 199 | { |
slowness | 1:fce3323d874f | 200 | case 1: |
slowness | 1:fce3323d874f | 201 | bas_myAX12->Set_Secure_Goal(150); |
slowness | 1:fce3323d874f | 202 | milieu_myAX12->Set_Secure_Goal(150-60); |
slowness | 1:fce3323d874f | 203 | haut_myAX12->Set_Secure_Goal(150+60); |
slowness | 1:fce3323d874f | 204 | break; |
slowness | 1:fce3323d874f | 205 | |
slowness | 1:fce3323d874f | 206 | case 2: |
slowness | 1:fce3323d874f | 207 | bas_myAX12->Set_Secure_Goal(60); |
slowness | 1:fce3323d874f | 208 | break; |
slowness | 1:fce3323d874f | 209 | |
slowness | 1:fce3323d874f | 210 | case 3: |
slowness | 1:fce3323d874f | 211 | milieu_myAX12->Set_Secure_Goal(150-90); |
slowness | 1:fce3323d874f | 212 | bas_myAX12->Set_Secure_Goal(60+45); |
slowness | 1:fce3323d874f | 213 | haut_myAX12->Set_Secure_Goal(150+45); |
slowness | 1:fce3323d874f | 214 | break; |
slowness | 1:fce3323d874f | 215 | |
slowness | 1:fce3323d874f | 216 | case 4: |
slowness | 1:fce3323d874f | 217 | haut_myAX12->Set_Secure_Goal(150); |
slowness | 1:fce3323d874f | 218 | milieu_myAX12->Set_Secure_Goal(105); |
slowness | 1:fce3323d874f | 219 | bas_myAX12->Set_Secure_Goal(110); |
slowness | 1:fce3323d874f | 220 | break; |
slowness | 1:fce3323d874f | 221 | |
slowness | 1:fce3323d874f | 222 | case 5: |
slowness | 1:fce3323d874f | 223 | bas_myAX12->Set_Secure_Goal(105); |
slowness | 1:fce3323d874f | 224 | haut_myAX12->Set_Secure_Goal(105); |
slowness | 1:fce3323d874f | 225 | milieu_myAX12->Set_Secure_Goal(150); |
slowness | 1:fce3323d874f | 226 | break; |
slowness | 1:fce3323d874f | 227 | |
slowness | 0:c03cffe402df | 228 | } |
slowness | 1:fce3323d874f | 229 | //wait(0.2); |
slowness | 1:fce3323d874f | 230 | }while(pc.readable()==0); |
slowness | 1:fce3323d874f | 231 | |
slowness | 1:fce3323d874f | 232 | } |
slowness | 0:c03cffe402df | 233 | |
slowness | 0:c03cffe402df | 234 | void scan() |
slowness | 0:c03cffe402df | 235 | { |
slowness | 0:c03cffe402df | 236 | AX12 *myAX12[100]; |
slowness | 0:c03cffe402df | 237 | int status=0,cpt=0; |
slowness | 1:fce3323d874f | 238 | 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 | 239 | // Print msg to PC |
slowness | 1:fce3323d874f | 240 | pc.printf("Scanning chain for AX12+ Servos...\n\n\r"); |
slowness | 1:fce3323d874f | 241 | // See which addresses contain AX12+ servos |
slowness | 1:fce3323d874f | 242 | for(baud=7; baud<11; baud++) { |
slowness | 1:fce3323d874f | 243 | baudrate=2000000/(tabbaud[baud]+1); |
slowness | 1:fce3323d874f | 244 | find_servo=0; |
slowness | 1:fce3323d874f | 245 | pc.printf("\r\n"); |
slowness | 1:fce3323d874f | 246 | pc.printf("\rcheck servo in %d baudrate\r\n",baudrate); |
slowness | 1:fce3323d874f | 247 | for (int i = 0; i < 254; i++) { |
slowness | 1:fce3323d874f | 248 | myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects |
slowness | 1:fce3323d874f | 249 | pc.printf("\r%03d",i); |
slowness | 1:fce3323d874f | 250 | status = -1; // Reset status variable |
slowness | 1:fce3323d874f | 251 | status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address |
slowness | 1:fce3323d874f | 252 | if(status == i) { |
slowness | 1:fce3323d874f | 253 | 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 | 254 | find_servo=find_servo++; |
slowness | 1:fce3323d874f | 255 | cpt++; |
slowness | 1:fce3323d874f | 256 | } else { |
slowness | 1:fce3323d874f | 257 | delete myAX12[cpt]; |
slowness | 1:fce3323d874f | 258 | /*if(i%16==0) |
slowness | 1:fce3323d874f | 259 | pc.printf("\r"); |
slowness | 1:fce3323d874f | 260 | */ |
slowness | 0:c03cffe402df | 261 | } |
slowness | 0:c03cffe402df | 262 | } |
slowness | 1:fce3323d874f | 263 | if(find_servo==0) |
slowness | 1:fce3323d874f | 264 | pc.printf("\rno AX12 in this baudrate=%d\n\r",baudrate); |
slowness | 1:fce3323d874f | 265 | //wait(2.); |
slowness | 1:fce3323d874f | 266 | } |
slowness | 1:fce3323d874f | 267 | |
slowness | 0:c03cffe402df | 268 | } |
slowness | 0:c03cffe402df | 269 | |
slowness | 0:c03cffe402df | 270 | void scan_all() |
slowness | 0:c03cffe402df | 271 | { |
slowness | 0:c03cffe402df | 272 | AX12 *myAX12[15]; |
slowness | 0:c03cffe402df | 273 | int status=0,cpt=0,nombre_servo; |
slowness | 1:fce3323d874f | 274 | 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 | 275 | // Print msg to PC |
slowness | 1:fce3323d874f | 276 | pc.printf("Scanning chain for AX12+ Servos not more than 15 servos ...\n\n\r"); |
slowness | 1:fce3323d874f | 277 | pc.printf("combien de servo à chercher? "); |
slowness | 1:fce3323d874f | 278 | pc.attach(NULL, Serial::RxIrq); |
slowness | 1:fce3323d874f | 279 | pc.scanf("%d",&nombre_servo); |
slowness | 1:fce3323d874f | 280 | pc.attach(&rxCallback, Serial::RxIrq);// See which addresses contain AX12+ servos |
slowness | 1:fce3323d874f | 281 | for(baud=0; baud<=255; baud++) { |
slowness | 1:fce3323d874f | 282 | if((Key=='q')||(Key=='Q')) { |
slowness | 1:fce3323d874f | 283 | Key=0; |
slowness | 1:fce3323d874f | 284 | break; |
slowness | 1:fce3323d874f | 285 | } |
slowness | 1:fce3323d874f | 286 | baudrate=2000000/(baud+1); |
slowness | 1:fce3323d874f | 287 | find_servo=0; |
slowness | 1:fce3323d874f | 288 | pc.printf("\r\n"); |
slowness | 1:fce3323d874f | 289 | for (int i = 0; i < 253; i++) { |
slowness | 1:fce3323d874f | 290 | myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects |
slowness | 1:fce3323d874f | 291 | pc.printf("\r%03d",i); |
slowness | 1:fce3323d874f | 292 | status = -1; // Reset status variable |
slowness | 1:fce3323d874f | 293 | status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address |
slowness | 1:fce3323d874f | 294 | if(status == i) { |
slowness | 1:fce3323d874f | 295 | 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 | 296 | find_servo=find_servo++; |
slowness | 1:fce3323d874f | 297 | cpt++; |
slowness | 1:fce3323d874f | 298 | |
slowness | 1:fce3323d874f | 299 | } else { |
slowness | 1:fce3323d874f | 300 | delete myAX12[cpt]; |
slowness | 1:fce3323d874f | 301 | /*if(i%16==0) |
slowness | 1:fce3323d874f | 302 | pc.printf("\r"); |
slowness | 1:fce3323d874f | 303 | */ |
slowness | 0:c03cffe402df | 304 | } |
slowness | 1:fce3323d874f | 305 | wait(0.001); |
slowness | 0:c03cffe402df | 306 | } |
slowness | 1:fce3323d874f | 307 | if(cpt>=nombre_servo) |
slowness | 1:fce3323d874f | 308 | break; |
slowness | 1:fce3323d874f | 309 | if(find_servo==0) |
slowness | 1:fce3323d874f | 310 | pc.printf("\r %03d no AX12 in this baudrate=%d\n\r",baud,baudrate); |
slowness | 1:fce3323d874f | 311 | |
slowness | 1:fce3323d874f | 312 | } |
slowness | 1:fce3323d874f | 313 | |
slowness | 0:c03cffe402df | 314 | } |
slowness | 0:c03cffe402df | 315 | |
slowness | 1:fce3323d874f | 316 | |
slowness | 1:fce3323d874f | 317 | |
slowness | 1:fce3323d874f | 318 | int main() |
slowness | 1:fce3323d874f | 319 | { |
slowness | 0:c03cffe402df | 320 | int NewID=0, OldID=0; |
slowness | 0:c03cffe402df | 321 | t.start(); |
slowness | 0:c03cffe402df | 322 | menu(); |
slowness | 0:c03cffe402df | 323 | pc.attach(&rxCallback, Serial::RxIrq); |
slowness | 1:fce3323d874f | 324 | while(1) { |
slowness | 1:fce3323d874f | 325 | switch(Key) { |
slowness | 0:c03cffe402df | 326 | case 's': |
slowness | 1:fce3323d874f | 327 | case 'S': |
slowness | 1:fce3323d874f | 328 | Key=0; |
slowness | 1:fce3323d874f | 329 | scan_all(); |
slowness | 1:fce3323d874f | 330 | menu(); |
slowness | 1:fce3323d874f | 331 | break; |
slowness | 1:fce3323d874f | 332 | |
slowness | 0:c03cffe402df | 333 | case 'l': |
slowness | 1:fce3323d874f | 334 | case 'L': |
slowness | 1:fce3323d874f | 335 | Key=0; |
slowness | 1:fce3323d874f | 336 | lecture(); |
slowness | 1:fce3323d874f | 337 | menu(); |
slowness | 1:fce3323d874f | 338 | break; |
slowness | 1:fce3323d874f | 339 | case 'm': |
slowness | 1:fce3323d874f | 340 | case 'M': |
slowness | 1:fce3323d874f | 341 | Key=0; |
slowness | 1:fce3323d874f | 342 | menu(); |
slowness | 1:fce3323d874f | 343 | break; |
slowness | 1:fce3323d874f | 344 | |
slowness | 1:fce3323d874f | 345 | case 'v': |
slowness | 1:fce3323d874f | 346 | case 'V': |
slowness | 1:fce3323d874f | 347 | Key=0; |
slowness | 1:fce3323d874f | 348 | pc.attach(NULL, Serial::RxIrq); |
slowness | 1:fce3323d874f | 349 | vitesse(); |
slowness | 1:fce3323d874f | 350 | pc.attach(&rxCallback, Serial::RxIrq); |
slowness | 1:fce3323d874f | 351 | menu(); |
slowness | 1:fce3323d874f | 352 | break; |
slowness | 1:fce3323d874f | 353 | |
slowness | 1:fce3323d874f | 354 | case 'i': |
slowness | 1:fce3323d874f | 355 | case 'I': |
slowness | 1:fce3323d874f | 356 | Key=0; |
slowness | 1:fce3323d874f | 357 | pc.attach(NULL, Serial::RxIrq); |
slowness | 1:fce3323d874f | 358 | change_ident(); |
slowness | 1:fce3323d874f | 359 | pc.attach(&rxCallback, Serial::RxIrq); |
slowness | 1:fce3323d874f | 360 | menu(); |
slowness | 1:fce3323d874f | 361 | break; |
slowness | 1:fce3323d874f | 362 | case 't': |
slowness | 1:fce3323d874f | 363 | case 'T': |
slowness | 1:fce3323d874f | 364 | Key=0; |
slowness | 1:fce3323d874f | 365 | pc.attach(NULL, Serial::RxIrq); |
slowness | 1:fce3323d874f | 366 | test(); |
slowness | 1:fce3323d874f | 367 | pc.attach(&rxCallback, Serial::RxIrq); |
slowness | 1:fce3323d874f | 368 | break; |
slowness | 1:fce3323d874f | 369 | |
slowness | 1:fce3323d874f | 370 | case 'r': |
slowness | 1:fce3323d874f | 371 | case 'R': |
slowness | 1:fce3323d874f | 372 | Key=0; |
slowness | 1:fce3323d874f | 373 | pc.attach(NULL, Serial::RxIrq); |
slowness | 1:fce3323d874f | 374 | ax12reset(); |
slowness | 1:fce3323d874f | 375 | pc.attach(&rxCallback, Serial::RxIrq); |
slowness | 1:fce3323d874f | 376 | break; |
slowness | 1:fce3323d874f | 377 | |
slowness | 1:fce3323d874f | 378 | case 'a': |
slowness | 1:fce3323d874f | 379 | case 'A': |
slowness | 1:fce3323d874f | 380 | Key=0; |
slowness | 1:fce3323d874f | 381 | pc.attach(NULL, Serial::RxIrq); |
slowness | 1:fce3323d874f | 382 | ax12read_all_info(0,8); |
slowness | 1:fce3323d874f | 383 | ax12read_all_info(10,10); |
slowness | 1:fce3323d874f | 384 | ax12read_all_info(20,10); |
slowness | 1:fce3323d874f | 385 | ax12read_all_info(30,10); |
slowness | 1:fce3323d874f | 386 | ax12read_all_info(40,10); |
slowness | 1:fce3323d874f | 387 | |
slowness | 1:fce3323d874f | 388 | pc.attach(&rxCallback, Serial::RxIrq); |
slowness | 1:fce3323d874f | 389 | break; |
slowness | 1:fce3323d874f | 390 | |
slowness | 1:fce3323d874f | 391 | default: |
slowness | 1:fce3323d874f | 392 | break; |
slowness | 1:fce3323d874f | 393 | |
slowness | 0:c03cffe402df | 394 | } |
slowness | 0:c03cffe402df | 395 | } |
slowness | 0:c03cffe402df | 396 | } |
slowness | 1:fce3323d874f | 397 | |
slowness | 1:fce3323d874f | 398 | // Print msg to PC |
slowness | 0:c03cffe402df | 399 | /* pc.printf("\nEnter AX12+ ID to Alter... "); |
slowness | 0:c03cffe402df | 400 | while (Key != 0x0D){ |
slowness | 0:c03cffe402df | 401 | Key=pc.getc(); //Get the keypad pressed |
slowness | 0:c03cffe402df | 402 | if((Key>=0x30)&&(Key<=0x39)){ //See if it's a number (0-9) |
slowness | 0:c03cffe402df | 403 | OldID=OldID*10+(Key-48); //Strip off ASCII code, convert to int & index |
slowness | 0:c03cffe402df | 404 | pc.putc(Key); |
slowness | 0:c03cffe402df | 405 | } |
slowness | 0:c03cffe402df | 406 | else if (Key == 0x20){ // Space was pressed |
slowness | 0:c03cffe402df | 407 | pc.printf("\n\rMoving sevros..."); |
slowness | 0:c03cffe402df | 408 | while(1){ |
slowness | 0:c03cffe402df | 409 | Key=pc.getc(); //Get the keypad pressed |
slowness | 0:c03cffe402df | 410 | if((Key>=0x30)&&(Key<=0x39)){ //See if it's a number (0-9) |
slowness | 0:c03cffe402df | 411 | pc.putc(Key); |
slowness | 0:c03cffe402df | 412 | myAX12[(Key-48)]->Set_Goal(60); |
slowness | 0:c03cffe402df | 413 | wait(0.5); |
slowness | 0:c03cffe402df | 414 | myAX12[(Key-48)]->Set_Goal(240); |
slowness | 0:c03cffe402df | 415 | wait(0.5); |
slowness | 0:c03cffe402df | 416 | } |
slowness | 0:c03cffe402df | 417 | } |
slowness | 0:c03cffe402df | 418 | } |
slowness | 0:c03cffe402df | 419 | } |
slowness | 0:c03cffe402df | 420 | Key = 0; // Reset Key |
slowness | 0:c03cffe402df | 421 | pc.printf("\n\rEnter new AX12+ ID... "); |
slowness | 0:c03cffe402df | 422 | while (Key != 0x0D){ |
slowness | 0:c03cffe402df | 423 | Key=pc.getc(); //Get the keypad pressed |
slowness | 0:c03cffe402df | 424 | if((Key>=0x30)&&(Key<=0x39)){ //See if it's a number (0-9) |
slowness | 0:c03cffe402df | 425 | NewID=NewID*10+(Key-48); //Strip off ASCII code, convert to int & index |
slowness | 0:c03cffe402df | 426 | pc.putc(Key); |
slowness | 0:c03cffe402df | 427 | } |
slowness | 0:c03cffe402df | 428 | } |
slowness | 0:c03cffe402df | 429 | pc.printf("\n\rChanging AX12+ No. %d ID from %d to %d...\n\r",OldID,OldID,NewID); |
slowness | 0:c03cffe402df | 430 | myAX12[OldID]->Set_ID(OldID,NewID); |
slowness | 0:c03cffe402df | 431 | } |
slowness | 0:c03cffe402df | 432 | } |
slowness | 0:c03cffe402df | 433 | */ |