Diff: main.cpp

Revision:

0:105ca010735c

diff -r 000000000000 -r 105ca010735c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 09 03:59:07 2018 +0000
@@ -0,0 +1,65 @@
+#include "mbed.h"
+#include "Control.h"
+#define Vm 15
+int32_t panError,tiltError;
+uint16_t blocks;
+void Suiveur_Pixy(void);
+void Suiveur_Moteurs(void);
+Timer temps_moteur,fin_jeux;
+int main()
+{
+    temps_moteur.start();
+    temps_moteur.reset();
+    fin_jeux.start();
+    fin_jeux.reset();
+    while(1) {
+        blocks=pixy.getBlocks();
+        if(blocks) {
+            Suiveur_Pixy();
+            Suiveur_Moteurs();
+        }
+        if(temps_moteur.read()>20) {
+        MotD.setVit(0);
+        MotD.setVit(0);
+        break;
+        }
+    }
+}
+
+void Suiveur_Pixy(void)
+{
+
+    //panError = X_CENTER - pixy.blocks[0].x;
+    tiltError = pixy.blocks[0].y - Y_CENTER;
+    //panLoop.update(panError);
+    tiltLoop.update(tiltError);
+    pixy.setServos(500L,tiltLoop.m_pos);
+}
+
+int16_t eant=0,e;
+float kp_=0.0007,kd_=0;
+//float kp_=2,kd_=0;
+void Suiveur_Moteurs(void)
+{
+    float dt=temps_moteur.read();
+    temps_moteur.reset();
+
+    uint16_t posy=tiltLoop.m_pos;
+    int vit=((Vm-4)*(PIXY_RCS_MAX_POS-posy))/(350L)+4;
+
+    uint16_t posx=pixy.blocks[0].x;
+    e=posx-PIXY_CENTER_X;
+
+    float PID=kp_*(e)+kd_*(e-eant)/dt;
+    float Vd=vit-PID;
+    if(Vd<0)Vd=0;
+    if(Vd>50)Vd=50;
+    float Vi=Vm+PID;
+    if(Vi<0)Vi=0;
+    if(Vi>50)Vi=50;
+    // printf("VitD: %d\t VitG: %d\n\r",(int)Vd,(int)Vi);
+    MotD.setVit((int)(Vd));
+    MotG.setVit((int)(Vi));
+
+    eant=e;
+}
\ No newline at end of file