clement lignie
config AX12
Fork of
configure_ax12_test_bras_module
by 
CRAC Team
Diff: main.cpp
- Revision:
- 1:fce3323d874f
- Parent:
- 0:c03cffe402df
- Child:
- 2:91b6646ea994
--- a/main.cpp Wed Feb 03 14:01:53 2016 +0000
+++ b/main.cpp Fri Nov 25 17:38:14 2016 +0000
@@ -7,12 +7,12 @@
int Key=0,mode=0;
// This function is called when a character goes into the RX buffer.
-void rxCallback()
+void rxCallback()
{
- Key=pc.getc();
+ Key=pc.getc();
}
-
-
+
+
void menu()
{
pc.printf("------------------------------------------\n\n\r");
@@ -41,23 +41,20 @@
pc.printf("\rVitesse actuelle de l'AX12: ");
pc.scanf("%d",&vitesse);
pc.printf("%d \n\n\r",vitesse);
- for(i=0;i<=0;i++)
- {
+ for(i=0; i<=0; i++) {
//vitesse=2000000/(i+1);
ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
status = -1; // Reset status variable
status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
- if(1)//status == numero_ID)
- {
- pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
+ if(1) { //status == numero_ID)
+ pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
pc.printf("readinfo \n\n\r");
ptr_myAX12->read_all_info(start,longueur);
wait(1.);
- }
- else
+ } else
pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
delete ptr_myAX12;
- }
+ }
}
@@ -79,25 +76,22 @@
pc.printf("\rVitesse actuelle de l'AX12: ");
pc.scanf("%d",&vitesse);
pc.printf("%d \n\n\r",vitesse);
- for(i=0;i<=0;i++)
- {
+ for(i=0; i<=0; i++) {
//vitesse=2000000/(i+1);
ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
status = -1; // Reset status variable
status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
- if(1)//status == numero_ID)
- {
- pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
+ if(1) { //status == numero_ID)
+ pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
pc.printf("reset \n\n\r");
ptr_myAX12->reset();
wait(1.);
- }
- else
+ } else
pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
delete ptr_myAX12;
- }
+ }
}
-
+
void change_ident()
{
AX12 *ptr_myAX12;
@@ -113,9 +107,8 @@
ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
status = -1; // Reset status variable
status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
- if(1)//status == numero_ID)
- {
- pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
+ if(1) { //status == numero_ID)
+ pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
pc.printf("\rNouvelle ID de l'AX12: ");
pc.scanf("%d",&ID_new);
status = -1; // Reset status variable
@@ -125,22 +118,18 @@
status = -1; // Reset status variable
status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
pc.printf("Changement de ID");
- if(status == ID_new)
- {
- pc.printf(" reussi \n\r"); // If it does print address and position to the PC
- }
- else
- pc.printf(" rate \n\r"); // If it does print address and position to the PC
- delete ptr_myAX12;
+ if(status == ID_new) {
+ pc.printf(" reussi \n\r"); // If it does print address and position to the PC
+ } else
+ pc.printf(" rate \n\r"); // If it does print address and position to the PC
+ delete ptr_myAX12;
+ } else {
+ pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
+
+
}
- else
- {
- pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
-
-
- }
-
-
+
+
}
void vitesse()
@@ -158,237 +147,255 @@
ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
status = -1; // Reset status variable
status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
- if(status == numero_ID)
- {
- pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
+ if(status == numero_ID) {
+ pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
pc.printf("Nouvelle Vitesse de l'AX12: ");
pc.scanf("%d",&vitesseNew);
baud=(2000000/vitesseNew)-1;
pc.printf("\n\r %d \n\r",baud);
status = -1; // Reset status variable
- status = ptr_myAX12->Set_Baud(baud);
-
- // See if the AX12 exists at this address
+ status = ptr_myAX12->Set_Baud(baud);
+
+ // See if the AX12 exists at this address
delete ptr_myAX12;
ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesseNew); // Create objects
status = -1; // Reset status variable
status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
pc.printf("Changement de vitesse");
- if(status == numero_ID)
- {
- pc.printf(" reussi \n\r"); // If it does print address and position to the PC
- }
- else
- pc.printf(" rate \n\r"); // If it does print address and position to the PC
- delete ptr_myAX12;
+ if(status == numero_ID) {
+ pc.printf(" reussi \n\r"); // If it does print address and position to the PC
+ } else
+ pc.printf(" rate \n\r"); // If it does print address and position to the PC
+ delete ptr_myAX12;
+ } else {
+ pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
+
+
}
- else
- {
- pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
-
-
- }
-
-
+
+
}
void test()
{
- AX12 *ptr_myAX12;
+ AX12 *bas_myAX12,*milieu_myAX12, *haut_myAX12;
unsigned char numero_ID;
char status=-1;
- unsigned long vitesse,vitesseNew;
- pc.printf("\rNumero de l'Id de l'AX12: ");
- pc.scanf("%d",&numero_ID);
- pc.printf("%d \n\n\r",numero_ID);
- pc.printf("\rVitesse actuelle de l'AX12: ");
- pc.scanf("%d",&vitesse);
- pc.printf("%d \n\n\r",vitesse);
- ptr_myAX12 = new AX12(p9, p10, numero_ID,vitesse); // Create objects
- status = -1; // Reset status variable
- status = ptr_myAX12->Get_ID(); // See if the AX12 exists at this address
- if(status == numero_ID)
+ unsigned long vitesse=1023,vitesseNew,position;
+ bas_myAX12 = new AX12(p9, p10, 2,1000000); // Create objects
+ milieu_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects
+ haut_myAX12 = new AX12(p9, p10, 3,1000000); // Create objects
+ bas_myAX12->Set_Goal_speed(1023);//wait(0.2);
+ milieu_myAX12->Set_Goal_speed(1023);//wait(0.2);
+ haut_myAX12->Set_Goal_speed(1023);//wait(0.2);
+
+ do
{
- pc.printf("Servo found at address %d and baudrate %d\n\r",numero_ID,vitesse); // If it does print address and position to the PC
- do
+ pc.printf("numero de mouvement");
+ pc.scanf("%d",&position);
+
+
+ switch(position)
{
- ptr_myAX12->Set_Secure_Goal(50);
- //wait(0.2);
- ptr_myAX12->Set_Secure_Goal(155);
- //wait(0.2);
+ case 1:
+ bas_myAX12->Set_Secure_Goal(150);
+ milieu_myAX12->Set_Secure_Goal(150-60);
+ haut_myAX12->Set_Secure_Goal(150+60);
+ break;
+
+ case 2:
+ bas_myAX12->Set_Secure_Goal(60);
+ break;
+
+ case 3:
+ milieu_myAX12->Set_Secure_Goal(150-90);
+ bas_myAX12->Set_Secure_Goal(60+45);
+ haut_myAX12->Set_Secure_Goal(150+45);
+ break;
+
+ case 4:
+ haut_myAX12->Set_Secure_Goal(150);
+ milieu_myAX12->Set_Secure_Goal(105);
+ bas_myAX12->Set_Secure_Goal(110);
+ break;
+
+ case 5:
+ bas_myAX12->Set_Secure_Goal(105);
+ haut_myAX12->Set_Secure_Goal(105);
+ milieu_myAX12->Set_Secure_Goal(150);
+ break;
+
}
- while(pc.readable()==0);
- }
- else
- pc.printf("no AX12 in baudrate: %d and in ID=%d\n\r",vitesse,numero_ID);
- delete ptr_myAX12;
-}
+ //wait(0.2);
+ }while(pc.readable()==0);
+
+ }
void scan()
{
AX12 *myAX12[100];
int status=0,cpt=0;
- long baud=0,baudrate=0,find_servo=0,tabbaud[11]={64,1,2,3,4,7,9,16,34,103,208};//{207,103,34,16,9,7,4,3,2,1,0};//
+ long baud=0,baudrate=0,find_servo=0,tabbaud[11]= {1,2,3,4,7,9,16,34,64,103,208}; //{207,103,34,16,9,7,4,3,2,1,0};//
// Print msg to PC
- pc.printf("Scanning chain for AX12+ Servos...\n\n\r");
- // See which addresses contain AX12+ servos
- for(baud=0;baud<11;baud++)
- {
- baudrate=2000000/(tabbaud[baud]+1);
- find_servo=0;
- pc.printf("\r\n");
- for (int i = 0;i < 254; i++)
- {
- myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects
- pc.printf("\r%03d",i);
- status = -1; // Reset status variable
- status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address
- if(status == i)
- {
- pc.printf("\rServo found at address %d and baudrate %d\r\n",i,baudrate); // If it does print address and position to the PC
- find_servo=find_servo++;
- cpt++;
- }
- else
- {
- delete myAX12[cpt];
- /*if(i%16==0)
- pc.printf("\r");
- */}
+ pc.printf("Scanning chain for AX12+ Servos...\n\n\r");
+ // See which addresses contain AX12+ servos
+ for(baud=7; baud<11; baud++) {
+ baudrate=2000000/(tabbaud[baud]+1);
+ find_servo=0;
+ pc.printf("\r\n");
+ pc.printf("\rcheck servo in %d baudrate\r\n",baudrate);
+ for (int i = 0; i < 254; i++) {
+ myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects
+ pc.printf("\r%03d",i);
+ status = -1; // Reset status variable
+ status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address
+ if(status == i) {
+ pc.printf("\rServo found at address %d and baudrate %d\r\n",i,baudrate); // If it does print address and position to the PC
+ find_servo=find_servo++;
+ cpt++;
+ } else {
+ delete myAX12[cpt];
+ /*if(i%16==0)
+ pc.printf("\r");
+ */
}
- if(find_servo==0)
- pc.printf("\rno AX12 in this baudrate=%d\n\r",baudrate);
- //wait(2.);
}
-
+ if(find_servo==0)
+ pc.printf("\rno AX12 in this baudrate=%d\n\r",baudrate);
+ //wait(2.);
+ }
+
}
void scan_all()
{
AX12 *myAX12[15];
int status=0,cpt=0,nombre_servo;
- long baud=0,baudrate=0,find_servo=0,tabbaud[11]={64,1,2,3,4,7,9,16,34,103,208};//{207,103,34,16,9,7,4,3,2,1,0};//
+ long baud=0,baudrate=0,find_servo=0,tabbaud[11]= {64,1,2,3,4,7,9,16,34,103,208}; //{207,103,34,16,9,7,4,3,2,1,0};//
// Print msg to PC
- pc.printf("Scanning chain for AX12+ Servos not more than 15 servos ...\n\n\r");
- pc.printf("combien de servo à chercher? ");
- pc.attach(NULL, Serial::RxIrq);
- pc.scanf("%d",&nombre_servo);
- pc.attach(&rxCallback, Serial::RxIrq);// See which addresses contain AX12+ servos
- for(baud=0;baud<=255;baud++)
- {
- if((Key=='q')||(Key=='Q'))
- {
- Key=0;
- break;
+ pc.printf("Scanning chain for AX12+ Servos not more than 15 servos ...\n\n\r");
+ pc.printf("combien de servo à chercher? ");
+ pc.attach(NULL, Serial::RxIrq);
+ pc.scanf("%d",&nombre_servo);
+ pc.attach(&rxCallback, Serial::RxIrq);// See which addresses contain AX12+ servos
+ for(baud=0; baud<=255; baud++) {
+ if((Key=='q')||(Key=='Q')) {
+ Key=0;
+ break;
+ }
+ baudrate=2000000/(baud+1);
+ find_servo=0;
+ pc.printf("\r\n");
+ for (int i = 0; i < 253; i++) {
+ myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects
+ pc.printf("\r%03d",i);
+ status = -1; // Reset status variable
+ status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address
+ if(status == i) {
+ pc.printf("\r Servo found at address %d and baudrate %d\n\r",i,baudrate); // If it does print address and position to the PC
+ find_servo=find_servo++;
+ cpt++;
+
+ } else {
+ delete myAX12[cpt];
+ /*if(i%16==0)
+ pc.printf("\r");
+ */
}
- baudrate=2000000/(baud+1);
- find_servo=0;
- pc.printf("\r\n");
- for (int i = 0;i < 253; i++)
- {
- myAX12[cpt] = new AX12(p9, p10, i,baudrate); // Create objects
- pc.printf("\r%03d",i);
- status = -1; // Reset status variable
- status = myAX12[cpt]->Get_ID(); // See if the AX12 exists at this address
- if(status == i)
- {
- pc.printf("\r Servo found at address %d and baudrate %d\n\r",i,baudrate); // If it does print address and position to the PC
- find_servo=find_servo++;
- cpt++;
-
- }
- else
- {
- delete myAX12[cpt];
- /*if(i%16==0)
- pc.printf("\r");
- */}
- wait(0.001);
- }
- if(cpt>=nombre_servo)
- break;
- if(find_servo==0)
- pc.printf("\r %03d no AX12 in this baudrate=%d\n\r",baud,baudrate);
-
+ wait(0.001);
}
-
+ if(cpt>=nombre_servo)
+ break;
+ if(find_servo==0)
+ pc.printf("\r %03d no AX12 in this baudrate=%d\n\r",baud,baudrate);
+
+ }
+
}
-
-
-int main() {
+
+
+int main()
+{
int NewID=0, OldID=0;
t.start();
menu();
pc.attach(&rxCallback, Serial::RxIrq);
- while(1)
- {
- switch(Key)
- {
+ while(1) {
+ switch(Key) {
case 's':
- case 'S': Key=0;
- scan_all();
- menu();
- break;
-
+ case 'S':
+ Key=0;
+ scan_all();
+ menu();
+ break;
+
case 'l':
- case 'L': Key=0;
- lecture();
- menu();
- break;
- case 'm':
- case 'M': Key=0;
- menu();
- break;
-
- case 'v':
- case 'V': Key=0;
- pc.attach(NULL, Serial::RxIrq);
- vitesse();
- pc.attach(&rxCallback, Serial::RxIrq);
- menu();
- break;
-
- case 'i':
- case 'I': Key=0;
- pc.attach(NULL, Serial::RxIrq);
- change_ident();
- pc.attach(&rxCallback, Serial::RxIrq);
- menu();
- break;
- case 't':
- case 'T': Key=0;
- pc.attach(NULL, Serial::RxIrq);
- test();
- pc.attach(&rxCallback, Serial::RxIrq);
- break;
-
- case 'r':
- case 'R': Key=0;
- pc.attach(NULL, Serial::RxIrq);
- ax12reset();
- pc.attach(&rxCallback, Serial::RxIrq);
- break;
-
- case 'a':
- case 'A': Key=0;
- pc.attach(NULL, Serial::RxIrq);
- ax12read_all_info(0,8);
- ax12read_all_info(10,10);
- ax12read_all_info(20,10);
- ax12read_all_info(30,10);
- ax12read_all_info(40,10);
-
- pc.attach(&rxCallback, Serial::RxIrq);
- break;
-
- default: break;
-
+ case 'L':
+ Key=0;
+ lecture();
+ menu();
+ break;
+ case 'm':
+ case 'M':
+ Key=0;
+ menu();
+ break;
+
+ case 'v':
+ case 'V':
+ Key=0;
+ pc.attach(NULL, Serial::RxIrq);
+ vitesse();
+ pc.attach(&rxCallback, Serial::RxIrq);
+ menu();
+ break;
+
+ case 'i':
+ case 'I':
+ Key=0;
+ pc.attach(NULL, Serial::RxIrq);
+ change_ident();
+ pc.attach(&rxCallback, Serial::RxIrq);
+ menu();
+ break;
+ case 't':
+ case 'T':
+ Key=0;
+ pc.attach(NULL, Serial::RxIrq);
+ test();
+ pc.attach(&rxCallback, Serial::RxIrq);
+ break;
+
+ case 'r':
+ case 'R':
+ Key=0;
+ pc.attach(NULL, Serial::RxIrq);
+ ax12reset();
+ pc.attach(&rxCallback, Serial::RxIrq);
+ break;
+
+ case 'a':
+ case 'A':
+ Key=0;
+ pc.attach(NULL, Serial::RxIrq);
+ ax12read_all_info(0,8);
+ ax12read_all_info(10,10);
+ ax12read_all_info(20,10);
+ ax12read_all_info(30,10);
+ ax12read_all_info(40,10);
+
+ pc.attach(&rxCallback, Serial::RxIrq);
+ break;
+
+ default:
+ break;
+
}
}
}
-
- // Print msg to PC
+
+// Print msg to PC
/* pc.printf("\nEnter AX12+ ID to Alter... ");
while (Key != 0x0D){
Key=pc.getc(); //Get the keypad pressed