Diff: Classes/NXPCam.cpp

Revision:

0:8743b606abc3

Child:

1:a9af73d5abd4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Classes/NXPCam.cpp	Tue Feb 11 15:41:45 2020 +0000
@@ -0,0 +1,115 @@
+#include "NXPCam.h"
+
+extern Serial bt;
+
+NXPCam::NXPCam() : pixy(p28, p27)
+{
+    
+}
+
+/// DETECTION DEVIATION VIA CENTRE DE LA PISTE ///
+int NXPCam::getDeviationTrajectoire()
+{
+    float centrePiste;
+    static int x0, x1; // les 2 pts au bout des 2 vecteurs
+    int x; // si 1 seul vecteur détecté
+    int dev;
+    
+    if (pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR && pixy.Pixy2_numVectors > 2)
+        bt.printf("Plus de 2 vecteurs !\n\r");
+    
+    if (pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR && pixy.Pixy2_numVectors >= 2) // Si la pixy a capté des vectors
+    {
+        x0 = (pixy.Pixy2_vectors[0].pixY1 < pixy.Pixy2_vectors[0].pixY0) ? pixy.Pixy2_vectors[0].pixX1 : pixy.Pixy2_vectors[0].pixX0; // Tests nécessaires pour être sûr de prendre la partie haute
+        x1 = (pixy.Pixy2_vectors[1].pixY1 < pixy.Pixy2_vectors[1].pixY0) ? pixy.Pixy2_vectors[1].pixX1 : pixy.Pixy2_vectors[1].pixX0; // du vecteur (possibilité de reverseVector à tester plus tard)
+        if(x0 > x1)
+        {
+            int savex0 = x0;
+            x0 = x1;
+            x1 = savex0;
+        }
+            
+        centrePiste = (x0 + x1) / 2.f; // valeur entre 15 et 55
+        dev = (int)((SCREEN_CENTER - centrePiste) * (200.f / SCREEN_CENTER));
+        return dev;
+    }
+    else if (pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR && pixy.Pixy2_numVectors == 1)
+    {
+        bt.printf("1 seul vecteur !\n\r");
+        this->v1 = Vector(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
+        
+        return (this->v1.getCoeffDir() < 0) ? 100 : -100;
+        // return -1.f/this(->v1.getCoeffDir();
+        
+        /*x = (pixy.Pixy2_vectors[0].pixY1 < pixy.Pixy2_vectors[0].pixY0) ? pixy.Pixy2_vectors[0].pixX1 : pixy.Pixy2_vectors[0].pixX0;
+        
+        return (abs(x0-x) < abs(x1-x)) ? -100 : 100;*/
+    }
+    else
+        return INFO_NON_EXPLOITABLE;
+}
+
+/// DETECTION DEVIATION VIA VALEURS DE COEFFICIENTS DIRECTEURS ///
+/*int NXPCam::getDeviationTrajectoire()
+{
+    if (pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR && pixy.Pixy2_numVectors >= 2) // Si la pixy a capté des vectors
+    {
+        this->v1 = Vector(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
+        this->v2 = Vector(pixy.Pixy2_vectors[1].pixX0, pixy.Pixy2_vectors[1].pixY0, pixy.Pixy2_vectors[1].pixX1, pixy.Pixy2_vectors[1].pixY1);
+        
+        if(this->v1.estADroiteDe(v2))
+        {
+            Vector savev1 = v1;
+            v1 = v2;
+            v2 = savev1;
+        }
+        
+        float c1 = v1.getCoeffDir(), c2 = v2.getCoeffDir();
+        
+        if(c1 < 0)
+            return 100;
+        if(c2 > 0)
+            return -100;
+        if(abs(c1) > 2.5)
+            return -100;
+        if(abs(c2) > 2.5)
+            return 100;
+        else return 0;
+    }
+    else
+        return INFO_NON_EXPLOITABLE;
+}*/
+
+/// DETECTION DEVIATION VIA DIFFERENCE DE COEFFICIENTS DIRECTEURS ///
+/*int NXPCam::getDeviationTrajectoire()
+{
+    if (pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR && pixy.Pixy2_numVectors >= 2) // Si la pixy a capté des vectors
+    {
+        this->v1 = Vector(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
+        this->v2 = Vector(pixy.Pixy2_vectors[1].pixX0, pixy.Pixy2_vectors[1].pixY0, pixy.Pixy2_vectors[1].pixX1, pixy.Pixy2_vectors[1].pixY1);
+        
+        if(this->v1.estADroiteDe(v2))
+        {
+            Vector savev1 = v1;
+            v1 = v2;
+            v2 = savev1;
+        }
+        
+        return (int)((float)(abs(v2.getCoeffDir()) - abs(v1.getCoeffDir())) * 15); // entre -6 et 8 de base donc *14 pour entre -100 et 100 (à peu près)
+    }
+    else
+        return INFO_NON_EXPLOITABLE;
+}*/
+
+int NXPCam::getNormeVecteurs()
+{
+    if(pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR && pixy.Pixy2_numVectors == 2)
+    {
+        Vector v1 = Vector(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
+        Vector v2 = Vector(pixy.Pixy2_vectors[1].pixX0, pixy.Pixy2_vectors[1].pixY0, pixy.Pixy2_vectors[1].pixX1, pixy.Pixy2_vectors[1].pixY1);
+        
+        return (v1.getNorme() + v2.getNorme()) / 2;
+    }
+    
+    return -1;
+}
\ No newline at end of file