Diff: main.cpp

Revision:

1:11cbd2bf65d7

Parent:

0:a6cbfa280472

Child:

2:b94303d66b9d

--- a/main.cpp	Thu Feb 07 17:38:48 2019 +0000
+++ b/main.cpp	Tue Mar 19 19:24:04 2019 +0000
@@ -1,156 +1,233 @@
 #include "mbed.h"
-#define PERIODE 0.001
-#define PRECISION 0.05
-#define KP 1
-#define KI 1
-#define KD 1
-
-#define MOYENNAGE 256
-#define DIAMETRE 0.07
-
-void moteur_init();
-void moteur_a(int dir, float percent);
-void moteur_b(int dir, float percent);
-void trigger_b ();
-void trigger_a ();
-float moteur_pos_a();
-float moteur_pos_b();
-void encodeur_init();
-
-
-
-PwmOut mypwm_a(D6);
-PwmOut mypwm_b(D11);
-
-DigitalOut in1(D7);
-DigitalOut in2(D8);
-DigitalOut in3(D9);
-DigitalOut in4(D10);
-
-DigitalIn A(D2);
-DigitalIn B(D3);
-
-float dist_b, dist_a;
-int cpt_b, cpt_a ;
-
-InterruptIn event_b(D3);
-InterruptIn event_a(D4);
+#include "QEI.h"
+#include "Moteur.h"
+#include "math.h"
 
 Serial pc(SERIAL_TX,SERIAL_RX);
 
+QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING);
+QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING);
+
+
+/*pour 8v*/
+static float Pl = 0.01; //0.03
+static float Il = 0.0001; //0.001
+static float Dl = 0;
+
+static float Pa = 0.01; //0.01
+static float Ia = 0.0001; //0.0001
+static float Da = 0;
+
+#define LINEIQUE 0
+#define ANGULAIRE 1
+
+#define FREQ_CORRECTION 0.01
+#define ERR_STATIQUE 100
+#define LIMITE_TEMPS 1/FREQ_CORRECTION /*3 secondes*/
+
+#define DISTANCE_ENTRE_ROUES 270 /*en mm*/
+#define DIMENSION_ROUE_A 70 /*en mm*/
+#define DIMENSION_ROUE_B 70 /*en mm*/
+#define TIC_PAR_TOUR_A 1200
+#define TIC_PAR_TOUR_B 1200
+
+#define LIMITE_VITESSE 0.5
+
+void pos(int commande_a, int commande_b);
+void pos_a(int commande_a);
+void pos_b(int commande_b);
+void deplacement(int theta, int r);
+float PID(float erreur,float type);
+ 
 int main() {
-    bool dir= 1;
-    float err_a, sumerr_a, errprec_a, cmd_a, fin_a, ret_a;
-    float err_b, sumerr_b, errprec_b, cmd_b, fin_b, ret_b;
-    moteur_init();
-    encodeur_init();
-    
-    wait(1);
-    
+
+    float distance = 0;
+    float orientation_deg = 90;
     
-    cmd_a = 0;
-    cmd_b = 0;
-    fin_a = 1;
-    fin_b = 1;
+    float pulse_a;
+    float pulse_b;
+    
+    float pid_a;
+    float pid_b;
+    
+    float pid_dist;
+    float pid_ori;
+    
+    float err_dist;
+    float err_ori;
+    
+    float n_commande_a;
+    float n_commande_b;
+    
+    float res_a = (float)DIMENSION_ROUE_A*3.14/(float)TIC_PAR_TOUR_A;
+    float res_b = (float)DIMENSION_ROUE_B*3.14/(float)TIC_PAR_TOUR_B;
+    
+    float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
     
-    err_a = 1;
-    sumerr_a = 0;
-    errprec_a = 0;
+    printf("%f\n\r",orientation_dist);
+    
+    moteur_init();
     
-    while((float)err_a>0.01){
-            ret_a = moteur_pos_a();
+    while(1)
+    {
+        pulse_a = (float)encodeur_a.getPulses() * res_a;
+        pulse_b = (float)encodeur_b.getPulses()* res_b;
+        
+        err_dist = distance - (pulse_a + pulse_b) /2;
+        err_ori = orientation_dist - (pulse_a - pulse_b) /2;   
+        
+        printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
+        
+        pid_dist = PID(err_dist,LINEIQUE);
+        pid_ori = PID(err_ori,ANGULAIRE);
+        
+        pid_a = - pid_dist + pid_ori;
+        pid_b = - pid_dist - pid_ori;
         
         
-            errprec_a = err_a;
-            err_a = fin_a - ret_a; 
-            sumerr_a = err_a + sumerr_a;
-            
-            /*P*/
-            //cmd_a = KP * err_a;
-            
-            /*PI*/
-            //cmd_a = KP * err_a + KI * summerr_a;
-            
-            /*PD*/
-            //cmd_a = KP * err_a + KD * (err_a - err_prec_a);
+        //printf("%f\n\r",pulse_a);
+        
+        n_commande_a = pid_a;
+        n_commande_b = pid_b;
+        
+        
+        if (n_commande_a>1)
+            n_commande_a=LIMITE_VITESSE;
+        else if (n_commande_a<-LIMITE_VITESSE)
+            n_commande_a = -LIMITE_VITESSE;
+        
+        if (n_commande_b>LIMITE_VITESSE)
+            n_commande_b=LIMITE_VITESSE;
+        else if (n_commande_b<-LIMITE_VITESSE)
+            n_commande_b = -LIMITE_VITESSE;
             
-            /*PID*/
-            //cmd_a = KP * err_a + KI * summerr_a + KD * (err_a - err_prec_a);
-        }
-    
-    
+        
+        moteur_a(n_commande_a);
+        moteur_b(n_commande_b);
+        wait(FREQ_CORRECTION);     
+        
+    }
     
-}
-
-void encodeur_init(){
-    
-    event_b.rise(&trigger_b);
-    event_a.rise(&trigger_a);
-    
+    return 0;    
 }
 
 
-void moteur_init(){
-        
-        mypwm_a.period(PERIODE);
-        mypwm_b.period(PERIODE);
-        mypwm_a.pulsewidth(0);
-        mypwm_b.pulsewidth(0);
-        
+
+
+float PID(float erreur,float type)
+{ 
+    static float errPrev = 0;
+    static float errSum = 0;
+    static float errDif = 0;
+ 
+    float P,I,D;
+ 
+    errSum += erreur;                 
+ 
+    errDif = erreur - errPrev;      
+    errPrev = erreur;
+ 
+    if (type){
+        P = erreur * Pl;                  
+        I = errSum * Il;                 
+        D = errDif * Dl;                 
+    } else {
+        P = erreur * Pa;                  
+        I = errSum * Ia;                 
+        D = errDif * Da; 
     }
+ 
+    return P + I + D;                //Le résultat est la somme des trois
+                                     //composantes calculées précédemment
+}
 
-/*dir 0 for reverse, 1 for forward*/
-/*percent = pwm width proportion regarding the period*/
-void moteur_a(int dir, float percent){
-        bool err=0;
-        
-        if (dir==1){
-            in1 = 1;
-            in2 = 0;
-        } else if (dir==0) {
-            in1 = 0;
-            in2 = 1;
-        } else {
-            pc.printf("ERROR DIRECTION MOTOR A\n\r");
-            
-        }
-        if(err)
-            return;
-            
-        mypwm_a.pulsewidth((float)percent*(float)PERIODE);
-    }
+
+
+
+/*asservissement des deux roues individuellement*/
+    /*pas très utile pour le déplacement mais intéressant*/
+    /*de garder comme un exemple*/
+    
+    /*
+
+void pos(int commande_a, int commande_b)
+{
+
+    
+    
+    int erreur_a = 0;
+    int somme_erreur_a = 0;
+    
+    int derniere_erreur_a = 0;
+    int derivee_a = 0;    
+    
+    float n_commande_a;
+    
+    int validation_position_a = 0;
+    
+    int erreur_b = 0;
+    int somme_erreur_b = 0;
+    
+    int derniere_erreur_b = 0;
+    int derivee_b = 0;    
     
-void moteur_b(int dir, float percent){
-        bool err=0;
+    float n_commande_b;
+    
+    int validation_position_b = 0;
+    
+    do
+    {
+        
+        erreur_a = encodeur_a.getPulses()-commande_a;
+        erreur_b = encodeur_b.getPulses()-commande_b;
+        
+        somme_erreur_a = somme_erreur_a + erreur_a;
+        somme_erreur_b = somme_erreur_b + erreur_b;
         
-        if (dir==1){
-            in4 = 1;
-            in3 = 0;
-        } else if (dir==0) {
-            in4 = 0;
-            in3 = 1;
-        } else {
-            pc.printf("ERROR DIRECTION MOTOR B\n\r");
-            
-        }
-        if(err)
-            return;
+        derivee_a = erreur_a - derniere_erreur_a;
+        derivee_b = erreur_b - derniere_erreur_b;
+        
+        n_commande_a = erreur_a * P + somme_erreur_a * I + derivee_a * D;
+        n_commande_b = erreur_b * P + somme_erreur_b * I + derivee_b * D;
+        
+        derniere_erreur_a = erreur_a;
+        derniere_erreur_b = erreur_b;
+        
+        if (n_commande_a>1)
+            n_commande_a=1;
+        else if (n_commande_a<-1)
+            n_commande_a = -1;
             
-        mypwm_b.pulsewidth((float)percent*(float)PERIODE);
-    }
+        if (n_commande_b>1)
+            n_commande_b=1;
+        else if (n_commande_b<-1)
+            n_commande_b = -1;
+            
+        moteur_a(-n_commande_a);
+        moteur_b(-n_commande_b);
+        
+        if (abs(erreur_a)<ERR_STATIQUE)
+            validation_position_a++;
+        else
+            validation_position_a = 0;
+            
+        if (abs(erreur_b)<ERR_STATIQUE)
+            validation_position_b++;
+        else
+            validation_position_b = 0;
+        
+        wait(FREQ_CORRECTION);
+                
+    } while (validation_position_a<LIMITE_TEMPS || validation_position_b<LIMITE_TEMPS);
+    moteur_a(0);
+    moteur_b(0);
     
-float moteur_pos_b(){
-        return dist_b = 3.14*DIAMETRE *cpt_b/64;
-    }
+    encodeur_a.reset();
+    encodeur_b.reset();
+}
 
-float moteur_pos_a(){
-        return dist_a = 3.14*DIAMETRE *cpt_a/64;
-    }
-    
-void trigger_b () {
-    cpt_b++;
- }
+
+
 
-void trigger_a () {
-    cpt_a++;
- }
+
+*/
\ No newline at end of file