main.cpp

00001           #include "mbed.h"
00002           
00003 CAN can(PB_8,PB_9,1000000);
00004 Serial pc(USBTX, USBRX,9600);
00005 DigitalIn NPN_SENSOR_0(PB_0) ;
00006 DigitalIn PNP_SENSOR(PC_5) ;
00007 DigitalIn NPN_SENSOR_1(PC_3) ;
00008 DigitalIn NPN_SENSOR_2(PB_10) ; 
00009 
00010 char get_color_sensor_data(void) ;  //retourne la valeur des capteurs sur un octet  color_sensor_data
00011 int transfer_color_sensor_data(char sensor_data) ; //envoie l'octet de donnée sur le bus can et sur la liaison série
00012 
00013 char get_color_sensor_data(void) {
00014     char color_sensor_data = 0 ;
00015     
00016     color_sensor_data = (PNP_SENSOR<<3) | ((!NPN_SENSOR_2)<<2) | ((!NPN_SENSOR_1)<<1) | (!NPN_SENSOR_0) ;    
00017     
00018     return color_sensor_data ;
00019     
00020     }
00021 int transfer_color_sensor_data(char color_sensor_data) {
00022     int color_sensor_id = 0x1 ;
00023     char data[8] ;
00024     data[0] = color_sensor_data ;
00025     CANMessage color_sensor_msg=CANMessage(color_sensor_id,data, 1,CANData,CANStandard);
00026     pc.printf("\n%d",color_sensor_data) ;
00027     can.write(color_sensor_msg) ;
00028     return 0 ;
00029 }
00030 
00031 //PARTIE SERVO
00032 
00033 #include "fonctions_herkulex.h"
00034 #define TORQUE_ON                            0x60
00035 #define POS_Gauche                           256
00036 #define POS_Droit                            768
00037 #define POS_Haut                             512
00038 #define Playtime                             90
00039 #define GLED_ON                              0x04
00040 #define BLED_ON                              0x08
00041 #define RLED_ON                              0x10
00042 #define Pince_Ouvert                         200
00043 #define Pince_Ferme                          75
00044 #define base_initial                         750
00045 #define base_prendre                         225
00046 #define base_lever                           400
00047 #define base_sortir                          400
00048 #define base_stocker                         490
00049 #define bras_initial                         200
00050 #define bras_prendre                         500
00051 #define BRAS                                 2
00052 #define BASE                                 1
00053 #define PINCE                                3
00054 #define JP1                                  1
00055 #define JP2                                  2
00056 #define JP3                                  3
00057 #define JP4                                  4
00058 #define JP5                                  5
00059 #define Vert                                 2
00060 int couleur_gob = Vert;
00061 uint16_t pos_initiale[3] = {base_initial,bras_initial,Pince_Ouvert};
00062 //uint16_t pos_[3] = {base_,bras_,Pince_};
00063 void Position_initiale(int sortie);
00064 void prendre_gobelet(int sortie);
00065 void poser_gobelet(int sortie);
00066 void deverrouiller_couple(int sortie);
00067 void ranger_gobelet(int sortie);
00068 void sortir_gobelet(int sortie);
00069 int automate_bras(int sortie);
00070 void test_bras(int sortie);
00071 
00072 
00073 
00074 int main(void)
00075 {   
00076     deverrouiller_couple(JP1);
00077     deverrouiller_couple(JP3);
00078     deverrouiller_couple(JP2);
00079     while(true)
00080     {
00081         automate_bras(JP1);
00082         automate_bras(JP2);
00083         automate_bras(JP3);
00084         
00085     }
00086 }
00087 void Position_initiale(int sortie)
00088 {
00089     positionControl(PINCE,Pince_Ouvert,Playtime,GLED_ON,sortie);
00090     positionControl(BASE,base_sortir,Playtime,GLED_ON,sortie);
00091     positionControl(BRAS,bras_initial,Playtime,GLED_ON,sortie);
00092     positionControl(BASE,base_initial,Playtime,GLED_ON,sortie);
00093 }
00094 void prendre_gobelet(int sortie)
00095 {
00096     positionControl(BASE,base_sortir,Playtime,GLED_ON,sortie);
00097     positionControl(BRAS,bras_prendre,Playtime,GLED_ON,sortie);
00098     positionControl(BASE,base_prendre,Playtime,GLED_ON,sortie);
00099     //wait(1);
00100     positionControl(PINCE,Pince_Ferme,Playtime,GLED_ON,sortie);
00101     //wait(1);
00102 }
00103 void poser_gobelet(int sortie)
00104 {
00105     positionControl(BASE,base_prendre,Playtime,GLED_ON,sortie);
00106     positionControl(BRAS,bras_prendre,Playtime,GLED_ON,sortie);
00107     positionControl(PINCE,Pince_Ouvert,Playtime,GLED_ON,sortie);
00108 }
00109 void deverrouiller_couple(int sortie)
00110 {
00111     setTorque(BASE ,TORQUE_ON ,sortie);
00112     setTorque(BRAS ,TORQUE_ON ,sortie);
00113     setTorque(PINCE ,TORQUE_ON ,sortie);
00114 }
00115 void ranger_gobelet(int sortie)
00116 {
00117     positionControl(PINCE,Pince_Ferme,Playtime,GLED_ON,sortie);
00118     positionControl(BASE,base_sortir,Playtime,GLED_ON,sortie);
00119     //positionControl_Mul_playtime_different(BASE, base_stocker,Playtime,GLED_ON,BRAS,bras_initial,Playtime,GLED_ON,sortie);
00120     positionControl(BRAS,320,Playtime,GLED_ON,sortie);
00121     positionControl(BASE, base_stocker,Playtime,GLED_ON,sortie);
00122     positionControl(BRAS,bras_initial,Playtime,GLED_ON,sortie);
00123     positionControl(BASE,base_initial,Playtime,GLED_ON,sortie);
00124 }
00125 void sortir_gobelet(int sortie)
00126 {
00127     positionControl(BASE, base_stocker,Playtime,GLED_ON,sortie);
00128     positionControl(BRAS,320,Playtime,GLED_ON,sortie);
00129     positionControl(BASE,base_sortir,Playtime,GLED_ON,sortie);
00130     //positionControl_Mul_playtime_different(BASE, base_stocker,Playtime,GLED_ON,BRAS,bras_initial,Playtime,GLED_ON,sortie);
00131     //positionControl(BRAS,bras_initial,Playtime,GLED_ON,sortie);
00132     //positionControl(BASE, base_stocker,Playtime,GLED_ON,sortie);
00133     //positionControl(BASE,base_sortir,Playtime,GLED_ON,sortie);
00134     //wait(1);
00135     positionControl(BRAS,bras_prendre,Playtime,GLED_ON,sortie);
00136     positionControl(BASE,base_prendre,Playtime,GLED_ON,sortie);
00137     //wait(1);
00138     positionControl(PINCE,Pince_Ouvert,Playtime,GLED_ON,sortie);
00139 }
00140 void test_bras(int sortie)
00141 {
00142     Position_initiale(sortie);
00143     prendre_gobelet(sortie);
00144     ranger_gobelet(sortie);
00145     wait(0.5);
00146     sortir_gobelet(sortie);
00147     Position_initiale(sortie);
00148 }
00149 int automate_bras(int sortie)
00150 {   char couleur ;
00151     int masque[3] = {0x01, 0x02,0x04} ;
00152     Position_initiale(sortie);
00153     prendre_gobelet(sortie);
00154     ranger_gobelet(sortie);
00155     wait(0.5);
00156     couleur = get_color_sensor_data() ;
00157     transfer_color_sensor_data(couleur) ;
00158     if(couleur &masque[sortie-1])
00159     { 
00160         sortir_gobelet(sortie);
00161         Position_initiale(sortie);
00162     }
00163 }
00164     
00165 
00166 
00167 
00168