mouvement AX12 petit robot
Fork of command_AX12 by
Diff: main.cpp
- Revision:
- 0:a2a44c043932
- Child:
- 1:b3ff77670606
diff -r 000000000000 -r a2a44c043932 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 15 16:36:50 2017 +0000 @@ -0,0 +1,194 @@ +#include "mbed.h" +#include "AX12.h" +#include "cmsis.h" + + /* 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 + + /* ANGLE */ + +/* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 + 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 + 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 + 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 + 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 + 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 + 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 + 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 + 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 + 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ + + + /*static char Tab1[5]={ 0x09, + 0x5E, + 0x01, + 0x70, + 0x01};*/ + + /*static char Tab2[5]={ 0x05, + 0x96, + 0x01, + 0x85, + 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 ; +void Init_AX12(void); +void Lecture(void); +void Flip(void); + + + + + /* FONCTIONS */ + +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) + + 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) +} + +void Lecture() +{ + angle= socle_myAX12-> Get_Position(); + test_socle = socle_myAX12 -> read_and_test(angle,TAB); + + angle= bas_myAX12-> Get_Position(); + test_bas = bas_myAX12-> read_and_test(angle,TAB); + + angle= milieu_myAX12-> Get_Position(); + test_milieu = milieu_myAX12-> read_and_test(angle,TAB); + + angle= haut_myAX12-> Get_Position(); + test_haut = haut_myAX12-> read_and_test(angle,TAB); + + angle= ventouse_myAX12-> Get_Position(); + test_ventouse = ventouse_myAX12-> read_and_test(angle,TAB); + + valeur_test=test_bas+test_milieu+test_haut+test_socle+test_ventouse; + + #ifdef AX12_DEBUG + printf("\ntest_socle = %f",test_socle); + #endif + #ifdef AX12_DEBUG + printf("\ntest_bas = %f",test_bas); + #endif + #ifdef AX12_DEBUG + printf("\ntest_milieu = %f",test_milieu); + #endif + #ifdef AX12_DEBUG + printf("\ntest_haut = %f",test_haut); + #endif + #ifdef AX12_DEBUG + printf("\ntest_ventouse = %f",test_ventouse); + #endif + #ifdef AX12_DEBUG + printf("\nvaleur_test = %f",valeur_test); + #endif +} + + /* PROGRAMME PRINCIPAL*/ + +int main() +{ + flipper.attach(&Flip, 2.0); + + 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); + + + 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 + +}