CRAC Team
AX12 gros robot
Fork of
command_AX12_gros_robot
by 
CRAC Team
Diff: main.cpp
- Revision:
- 1:b3ff77670606
- Parent:
- 0:a2a44c043932
- Child:
- 2:99b1cb0d9f5e
--- a/main.cpp Wed Mar 15 16:36:50 2017 +0000
+++ b/main.cpp Tue Mar 21 14:46:54 2017 +0000
@@ -5,23 +5,37 @@
/* DECLARATION VARIABLES */
Timer t;
Ticker flipper;
-static char TAB[25]= {0x01,0x9A, 0x02, 0xFF, 0x00, // socle
- 0x02,0x0A, 0x01, 0xFF, 0x00, // bas
- 0x05,0xC8, 0x00, 0xFF, 0x00, // milieu
- 0x0D,0x90, 0x01, 0xFF, 0x00, // haut
- 0x09,0xD2, 0x01, 0xFF, 0x00}; //ventouse
-
-static char TAB2[25]= {0x01,0xE9, 0x00,0xFF, 0x00, // socle
- 0x02,0x2C, 0x01, 0xFF, 0x00, // bas
- 0x05,0xA6, 0x00, 0xFF, 0x00, // milieu
- 0x0D,0xD2, 0x01, 0xFF, 0x00, // haut
- 0x09,0xFF, 0x03, 0xFF, 0x00}; //ventouse
-
-static char TAB3[25]= {0x01,0xA6, 0x00,0xFF, 0x00, // socle
- 0x02,0x6E, 0x01, 0xFF, 0x00, // bas
- 0x05,0xE9, 0x00, 0xFF, 0x00, // milieu
- 0x0D,0x90, 0x01, 0xFF, 0x00, // haut
- 0x09,0xDD, 0x02, 0xFF, 0x00}; //ventouse
+static char TAB1[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
+ 0x12,0xBC, 0x02, 0xFF, 0x00,
+ 0x04,0xE9, 0x00, 0xFF, 0x00,
+ 0x07,0x4D, 0x01, 0xFF, 0x00,
+ 0x0F,0x90, 0x01, 0xFF, 0x00};
+
+static char TAB2[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
+ 0x12,0x25, 0x02, 0xFF, 0x00,
+ 0x04,0x15, 0x02, 0xFF, 0x00,
+ 0x07,0x4D, 0x01, 0xFF, 0x00,
+ 0x0F,0x90, 0x01, 0xFF, 0x00};
+
+static char TAB3[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
+ 0x12,0x2C, 0x01, 0xFF, 0x00,
+ 0x04,0x15, 0x02, 0xFF, 0x00,
+ 0x07,0x4D, 0x01, 0xFF, 0x00,
+ 0x0F,0x90, 0x01, 0xFF, 0x00};
+
+static char TAB4[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
+ 0x12,0x2C, 0x01, 0xFF, 0x00,
+ 0x04,0x4D, 0x01, 0xFF, 0x00,
+ 0x07,0x4D, 0x01, 0xFF, 0x00,
+ 0x0F,0x90, 0x01, 0xFF, 0x00};
+
+static char TAB5[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,
+ 0x12,0x2C, 0x01, 0xFF, 0x00,
+ 0x04,0x4D, 0x01, 0xFF, 0x00,
+ 0x07,0x6E, 0x01, 0xFF, 0x00,
+ 0x0F,0x90, 0x01, 0xFF, 0x00};
+
+
/* ANGLE */
@@ -36,7 +50,18 @@
90° = 0x2C, 0x01 | 190°= 0x79, 0x02
100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
-
+ /* NUMERO AX12 */
+
+/* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
+ 1 = 0x01 | 10 = 0x0A
+ 2 = 0x02 | 11 = 0x0B
+ 3 = 0x03 | 12 = 0x0C
+ 4 = 0x04 | 13 = 0x0D
+ 5 = 0x05 | 14 = 0x0E
+ 6 = 0x06 | 15 = 0x0F
+ 7 = 0x07 | 16 = 0x10
+ 8 = 0x08 | 17 = 0x11 */
+
/*static char Tab1[5]={ 0x09,
0x5E,
0x01,
@@ -50,13 +75,12 @@
0x05};*/
float angle=0.0;
float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
-int torque_socle=0, torque_bas=0, torque_milieu=0, torque_haut=0, torque_ventouse=0;
-int error_torque_socle=0, error_torque_bas=0, error_torque_milieu=0, error_torque_haut=0, error_torque_ventouse=0;
+
/* DECLARATION PROTOTYPES & POINTEURS */
short vitesse=700; // Vitesse angulaire (0-1023)
-AX12 *ventouse_myAX12, *haut_myAX12, *milieu_myAX12, *bas_myAX12, *socle_myAX12, *multiple_AX12 ;
+AX12 *dixhuit_myAX12, *quatre_myAX12,*six_myAX12, *sept_myAX12, *quinze_myAX12, *multiple_AX12;
void Init_AX12(void);
void Lecture(void);
void Flip(void);
@@ -69,20 +93,20 @@
void Init_AX12() // Initialisation des différents paramétres
{
- ventouse_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- haut_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- milieu_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- bas_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- socle_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
+ dixhuit_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
+ quatre_myAX12-> Set_Goal_speed(vitesse);
+ six_myAX12-> Set_Goal_speed(vitesse);
+ sept_myAX12-> Set_Goal_speed(vitesse);
+ quinze_myAX12-> Set_Goal_speed(vitesse);
- ventouse_myAX12->Set_Mode(0); // Mode Position(0) Mode Continu(1)
- haut_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
- milieu_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
- bas_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
- socle_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
+ dixhuit_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1)
+ quatre_myAX12-> Set_Mode(0);
+ six_myAX12-> Set_Mode(0);
+ sept_myAX12-> Set_Mode(0);
+ quinze_myAX12-> Set_Mode(0);
}
-void Lecture()
+/*void Lecture()
{
angle= socle_myAX12-> Get_Position();
test_socle = socle_myAX12 -> read_and_test(angle,TAB);
@@ -119,76 +143,44 @@
#ifdef AX12_DEBUG
printf("\nvaleur_test = %f",valeur_test);
#endif
-}
+}*/
/* PROGRAMME PRINCIPAL*/
int main()
{
- flipper.attach(&Flip, 2.0);
+ // flipper.attach(&Flip, 2.0);
+ dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000);
+ quatre_myAX12 = new AX12(p13, p14, 4, 1000000);
+ six_myAX12 = new AX12(p13, p14, 6, 1000000);
+ sept_myAX12 = new AX12(p13, p14, 7, 1000000);
+ quinze_myAX12 = new AX12(p13, p14, 15, 1000000);
+ multiple_AX12 = new AX12(p13,p14,0xFE,1000000); // Création objet : data pin 9&10, ID Broadcast, baud
- ventouse_myAX12 = new AX12(p9, p10, 9, 1000000); // Création objet : data pin 9&10, ID moteur 9, baud
- haut_myAX12 = new AX12(p9, p10,13, 1000000); // Création objet : data pin 9&10, ID moteur 13, baud
- milieu_myAX12 = new AX12(p9, p10, 5, 1000000); // Création objet : data pin 9&10, ID moteur 5, baud
- bas_myAX12 = new AX12(p9, p10, 2, 1000000); // Création objet : data pin 9&10, ID moteur 2, baud
- socle_myAX12 = new AX12(p9, p10, 1, 1000000); // Création objet : data pin 9&10, ID moteur 1, baud
- multiple_AX12 = new AX12(p9,p10,0xFE,1000000); // Création objet : data pin 9&10, ID Broadcast, baud
-
- multiple_AX12->multiple_goal_and_speed(5,TAB);
+ multiple_AX12->multiple_goal_and_speed(5,TAB1);
+ wait(2);
+ multiple_AX12->multiple_goal_and_speed(5,TAB2);
+ wait(2);
+ multiple_AX12->multiple_goal_and_speed(5,TAB3);
+ wait(2);
+ multiple_AX12->multiple_goal_and_speed(5,TAB4);
+ wait(2);
+ multiple_AX12->multiple_goal_and_speed(5,TAB5);
- while(valeur_test<4)
+ //six_myAX12 -> Set_Secure_Goal(200);
+ /*quatorze_myAX12 -> Set_Secure_Goal(260);
+ gauche_myAX12 -> Set_Secure_Goal(280);
+ quatorze_myAX12 -> Set_Secure_Goal(300); //avoir ax12 14 a 90°*/
+
+ /* while(valeur_test<4)
{
Lecture();
- }
+ }*/
- multiple_AX12->multiple_goal_and_speed(5,TAB2);
+
}
-
-void Flip ()
-{
-
- #ifdef AX12_DEBUG
- printf("\ntorque_ventouse avant = %d",torque_ventouse);
- #endif
-
- torque_socle= socle_myAX12 -> Get_Torque_Enable();
- torque_bas= bas_myAX12 -> Get_Torque_Enable();
- torque_milieu= milieu_myAX12 -> Get_Torque_Enable();
- torque_haut= haut_myAX12 -> Get_Torque_Enable();
- torque_ventouse=ventouse_myAX12 -> Get_Torque_Enable();
-
-
-
- /*if(torque_socle==1)
- {
- error_torque_socle=socle_myAX12 -> Set_Torque_Enable(0);
-
- }
- if(torque_bas==1)
- {
- error_torque_bas=bas_myAX12 -> Set_Torque_Enable(0);
-
- }
- if(torque_milieu==1)
- {
- error_torque_milieu=milieu_myAX12-> Set_Torque_Enable(0);
-
- }
- if(torque_haut==1)
- {
- error_torque_haut=haut_myAX12 -> Set_Torque_Enable(0);
-
- } */
-
- error_torque_ventouse=ventouse_myAX12 -> Set_Torque_Enable(0);
- //error_torque_ventouse=ventouse_myAX12 -> Set_Torque_Enable(1);
- #ifdef AX12_DEBUG
- printf("\ntorque_ventouse apres = %d",torque_ventouse);
- #endif
-
-}