File content as of revision 0:a2a44c043932:

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