Diff: PIDboussole.cpp

Revision:

0:4e5a5b017550

Child:

1:1d8f9c73cdc4

diff -r 000000000000 -r 4e5a5b017550 PIDboussole.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDboussole.cpp	Mon Apr 01 16:16:32 2019 +0000
@@ -0,0 +1,68 @@
+#include "PIDboussole.h"
+
+#define T 0.03//0.05
+#define Bkp 2
+#define Bki 0
+#define Bkd 0
+DigitalOut LED(PA_5);
+
+PID_boussole::PID_boussole(Motor* _moteur_d, Motor* _moteur_g, CMPS03* _boussole)
+{ 
+    moteur_g = _moteur_g;
+    moteur_d = _moteur_d;
+    boussole = _boussole;
+    angle = 0;
+    V_tout_droit = 1;
+    tickPID_boussole.attach(callback(this,&PID_boussole::fPID_boussole), T);
+}
+
+void PID_boussole::fPID_boussole()
+{
+    angle = boussole->getBearing();
+    Cpre = Cact;
+    Cact = angle;
+    Csomme_erreur += Cconsigne - Cact;
+    Cnouvelle_consigne = Bkp*(Cconsigne - Cact) + Bkd*(Cpre - Cact) + Bki*Csomme_erreur;
+    //V_tourne = (0.4-(abs(Cnouvelle_consigne)/(360*4)));
+    V_tourne = abs(angle-Cconsigne)/(360)+0.4;
+    if(V_tourne>1) V_tourne=1;
+
+    if(angle<(Cconsigne+10) && angle>(Cconsigne-10))
+    {
+        moteur_g->stop(0.0);
+        moteur_d->stop(0.0);
+    }
+    else
+    {
+        if((Cconsigne - Cact) <= 0)
+        {
+            //moteur_g->speed(V_tourne-0.1);
+            moteur_g->speed(V_tourne);
+            //moteur_d->speed(-V_tout_droit);
+            moteur_d->speed(0);
+            LED = 0;
+        }
+        else
+        {
+            //moteur_g->speed(-V_tout_droit);
+            moteur_g->speed(0);
+            moteur_d->speed(V_tourne+0.1);
+            LED = 1;
+        }
+    }   
+}
+
+void PID_boussole::STOP_PID_boussole()
+{
+    tickPID_boussole.detach();
+}
+
+void PID_boussole::START_PID_boussole()
+{
+    tickPID_boussole.attach(callback(this,&PID_boussole::fPID_boussole), T);
+}
+
+void PID_boussole::PID_boussole_consigne(int cons)
+{
+    Cconsigne = cons;
+}
\ No newline at end of file