AX12 gros robot

Fork of command_AX12_gros_robot by CRAC Team

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
-   
-}