main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "Moteur.h"
00004 #include "math.h"
00005 
00006 Serial pc(SERIAL_TX,SERIAL_RX);
00007 
00008 QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING);
00009 QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING);
00010 
00011 
00012 /*pour 8v*/
00013 static float Pl = 0.01; //0.03
00014 static float Il = 0.0001; //0.001
00015 static float Dl = 0;
00016 
00017 static float Pa = 0.01; //0.01
00018 static float Ia = 0.0001; //0.0001
00019 static float Da = 0;
00020 
00021 #define LINEIQUE 0
00022 #define ANGULAIRE 1
00023 
00024 #define FREQ_CORRECTION 0.01
00025 #define ERR_STATIQUE 100
00026 #define LIMITE_TEMPS 1/FREQ_CORRECTION /*3 secondes*/
00027 
00028 #define DISTANCE_ENTRE_ROUES 270 /*en mm*/
00029 #define DIMENSION_ROUE_A 70 /*en mm*/
00030 #define DIMENSION_ROUE_B 70 /*en mm*/
00031 #define TIC_PAR_TOUR_A 1200
00032 #define TIC_PAR_TOUR_B 1200
00033 
00034 #define LIMITE_VITESSE 0.5
00035 
00036 void pos(int commande_a, int commande_b);
00037 void pos_a(int commande_a);
00038 void pos_b(int commande_b);
00039 void deplacement(int theta, int r);
00040 float PID(float erreur,float type);
00041  
00042 int main() {
00043 
00044     float distance = 0;
00045     float orientation_deg = 90;
00046     
00047     float pulse_a;
00048     float pulse_b;
00049     
00050     float pid_a;
00051     float pid_b;
00052     
00053     float pid_dist;
00054     float pid_ori;
00055     
00056     float err_dist;
00057     float err_ori;
00058     
00059     float n_commande_a;
00060     float n_commande_b;
00061     
00062     float res_a = (float)DIMENSION_ROUE_A*3.14/(float)TIC_PAR_TOUR_A;
00063     float res_b = (float)DIMENSION_ROUE_B*3.14/(float)TIC_PAR_TOUR_B;
00064     
00065     float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
00066     
00067     printf("%f\n\r",orientation_dist);
00068     
00069     moteur_init();
00070     
00071     while(1)
00072     {
00073         pulse_a = (float)encodeur_a.getPulses() * res_a;
00074         pulse_b = (float)encodeur_b.getPulses()* res_b;
00075         
00076         err_dist = distance - (pulse_a + pulse_b) /2;
00077         err_ori = orientation_dist - (pulse_a - pulse_b) /2;   
00078         
00079         printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
00080         
00081         pid_dist = PID(err_dist,LINEIQUE);
00082         pid_ori = PID(err_ori,ANGULAIRE);
00083         
00084         pid_a = - pid_dist + pid_ori;
00085         pid_b = - pid_dist - pid_ori;
00086         
00087         
00088         //printf("%f\n\r",pulse_a);
00089         
00090         n_commande_a = pid_a;
00091         n_commande_b = pid_b;
00092         
00093         
00094         if (n_commande_a>1)
00095             n_commande_a=LIMITE_VITESSE;
00096         else if (n_commande_a<-LIMITE_VITESSE)
00097             n_commande_a = -LIMITE_VITESSE;
00098         
00099         if (n_commande_b>LIMITE_VITESSE)
00100             n_commande_b=LIMITE_VITESSE;
00101         else if (n_commande_b<-LIMITE_VITESSE)
00102             n_commande_b = -LIMITE_VITESSE;
00103             
00104         
00105         moteur_a(n_commande_a);
00106         moteur_b(n_commande_b);
00107         wait(FREQ_CORRECTION);     
00108         
00109     }
00110     
00111     return 0;    
00112 }
00113 
00114 
00115 
00116 
00117 float PID(float erreur,float type)
00118 { 
00119     static float errPrev = 0;
00120     static float errSum = 0;
00121     static float errDif = 0;
00122  
00123     float P,I,D;
00124  
00125     errSum += erreur;                 
00126  
00127     errDif = erreur - errPrev;      
00128     errPrev = erreur;
00129  
00130     if (type){
00131         P = erreur * Pl;                  
00132         I = errSum * Il;                 
00133         D = errDif * Dl;                 
00134     } else {
00135         P = erreur * Pa;                  
00136         I = errSum * Ia;                 
00137         D = errDif * Da; 
00138     }
00139  
00140     return P + I + D;                //Le résultat est la somme des trois
00141                                      //composantes calculées précédemment
00142 }
00143 
00144 
00145 
00146 
00147 /*asservissement des deux roues individuellement*/
00148     /*pas très utile pour le déplacement mais intéressant*/
00149     /*de garder comme un exemple*/
00150     
00151     /*
00152 
00153 void pos(int commande_a, int commande_b)
00154 {
00155 
00156     
00157     
00158     int erreur_a = 0;
00159     int somme_erreur_a = 0;
00160     
00161     int derniere_erreur_a = 0;
00162     int derivee_a = 0;    
00163     
00164     float n_commande_a;
00165     
00166     int validation_position_a = 0;
00167     
00168     int erreur_b = 0;
00169     int somme_erreur_b = 0;
00170     
00171     int derniere_erreur_b = 0;
00172     int derivee_b = 0;    
00173     
00174     float n_commande_b;
00175     
00176     int validation_position_b = 0;
00177     
00178     do
00179     {
00180         
00181         erreur_a = encodeur_a.getPulses()-commande_a;
00182         erreur_b = encodeur_b.getPulses()-commande_b;
00183         
00184         somme_erreur_a = somme_erreur_a + erreur_a;
00185         somme_erreur_b = somme_erreur_b + erreur_b;
00186         
00187         derivee_a = erreur_a - derniere_erreur_a;
00188         derivee_b = erreur_b - derniere_erreur_b;
00189         
00190         n_commande_a = erreur_a * P + somme_erreur_a * I + derivee_a * D;
00191         n_commande_b = erreur_b * P + somme_erreur_b * I + derivee_b * D;
00192         
00193         derniere_erreur_a = erreur_a;
00194         derniere_erreur_b = erreur_b;
00195         
00196         if (n_commande_a>1)
00197             n_commande_a=1;
00198         else if (n_commande_a<-1)
00199             n_commande_a = -1;
00200             
00201         if (n_commande_b>1)
00202             n_commande_b=1;
00203         else if (n_commande_b<-1)
00204             n_commande_b = -1;
00205             
00206         moteur_a(-n_commande_a);
00207         moteur_b(-n_commande_b);
00208         
00209         if (abs(erreur_a)<ERR_STATIQUE)
00210             validation_position_a++;
00211         else
00212             validation_position_a = 0;
00213             
00214         if (abs(erreur_b)<ERR_STATIQUE)
00215             validation_position_b++;
00216         else
00217             validation_position_b = 0;
00218         
00219         wait(FREQ_CORRECTION);
00220                 
00221     } while (validation_position_a<LIMITE_TEMPS || validation_position_b<LIMITE_TEMPS);
00222     moteur_a(0);
00223     moteur_b(0);
00224     
00225     encodeur_a.reset();
00226     encodeur_b.reset();
00227 }
00228 
00229 
00230 
00231 
00232 
00233 */