HomologationCachan

Dependencies:   Moteur CACHAN_HOMOG mbed

Fork of Cachan_Homolog by Luis Enriquez

Files at this revision

API Documentation at this revision

Comitter:
lenriquez389
Date:
Sat Jun 09 03:59:07 2018 +0000
Commit message:
Homologation Cachan

Changed in this revision

Control.h Show annotated file Show diff for this revision Revisions of this file
Globales.cpp Show annotated file Show diff for this revision Revisions of this file
Moteur.lib Show annotated file Show diff for this revision Revisions of this file
TPixy-Interface.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 105ca010735c Control.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control.h	Sat Jun 09 03:59:07 2018 +0000
@@ -0,0 +1,15 @@
+#ifndef CONTROL_H
+#define CONTROL_H
+#include "mbed.h"
+#include "Moteur.h"
+#include "Pixy.h"
+#include "TPixyInterface.h"
+#include "TPixy.h"
+extern Moteur MotD,MotG;
+extern Serial serial;
+extern SPI spi;
+extern PixySPI pixy;
+extern ServoLoop panLoop;
+extern ServoLoop tiltLoop;
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 105ca010735c Globales.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Globales.cpp	Sat Jun 09 03:59:07 2018 +0000
@@ -0,0 +1,12 @@
+#include "mbed.h"
+#include "Moteur.h"
+#include "Pixy.h"
+#include "TPixyInterface.h"
+#include "TPixy.h"
+Moteur MotD(p25);
+Moteur MotG(p23);
+Serial serial(USBTX, USBRX);
+SPI spi(p5, p6, p7);
+PixySPI pixy(&spi, &serial);
+ServoLoop panLoop(450,250);
+ServoLoop tiltLoop(300,150);
\ No newline at end of file
diff -r 000000000000 -r 105ca010735c Moteur.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Moteur.lib	Sat Jun 09 03:59:07 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/lenriquez389/code/Moteur/#b1590d097032
diff -r 000000000000 -r 105ca010735c TPixy-Interface.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TPixy-Interface.lib	Sat Jun 09 03:59:07 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/lenriquez389/code/CACHAN_HOMOG/#ee635fc62b6e
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
diff -r 000000000000 -r 105ca010735c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Jun 09 03:59:07 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file