File content as of revision 2:1103f5d61035:

#include "NXPCam.h"
#include <iostream>
#include <vector>

using namespace std;

extern Serial bt;

int diff1, diff2; // provisoire

NXPCam::NXPCam() : pixy(p28, p27)
{
    
}

/// INIT ///
void NXPCam::init()
{
    Timer timer;
    timer.start();
    bt.printf("Init...\n\r");
    /// INIT ATTENDS 2 VECTEURS ///
    while(pixy.Pixy2_numVectors != 2 && timer.read() < 2)
        pixy.pixy2_getAllFeature(PIXY2_VECTOR);
    
    this->v1past = Vector(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
    this->v2past = Vector(pixy.Pixy2_vectors[1].pixX0, pixy.Pixy2_vectors[1].pixY0, pixy.Pixy2_vectors[1].pixX1, pixy.Pixy2_vectors[1].pixY1);
    
    if(this->v1past.estADroiteDe(this->v2past))
    {
        Vector savev1 = v1;
        v1 = v2;
        v2 = savev1;
    }
    
    bt.printf("Lancement du programme.\n\r");
}

/// DETECTION DEVIATION VIA POINT PROJETé ///
int NXPCam::getDeviationTrajectoire()
{
    vector<Vector> vectors;
    vector<Vector> vectorSupp;
    
    if ((pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR) == PIXY2_VECTOR) // Si la pixy a capté des vectors
    {
        /// CAS D'1 SEUL VECTEUR ///
        if(pixy.Pixy2_numVectors == 1)
        {
            bt.printf("1 seul vecteur\n\r");
            Vector v(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
            return -COEFF_VIRAGE/v.getCoeffDir(); // (v.getCoeffDir() < 0) ? 75 : -75; 
        }
        
        /// RECOLTE DE TOUS LES VECTEURS + CREATION DE CEUX QUI SONT JOINTS ///
        for(int i=0; i<pixy.Pixy2_numVectors; i++)
        {
            vectors.push_back(Vector(pixy.Pixy2_vectors[i].pixX0, pixy.Pixy2_vectors[i].pixY0, pixy.Pixy2_vectors[i].pixX1, pixy.Pixy2_vectors[i].pixY1));
            for(int j=0; j<i; j++)
            {
                Vector newVector = vectors.back().testJoinedTo(vectors[j]);
                
                if(newVector != Vector(0, 0, 0, 0))
                {
                    bt.printf("Vectors joined\n\r");
                    vectorSupp.push_back(newVector);
                    vectors.pop_back();
                    vectors.erase(vectors.begin() + j);
                }
            }
        }
        
        for(unsigned i=0; i<vectorSupp.size(); i++)
            vectors.push_back(vectorSupp[i]);
            
        /// FILTRE DES VECTEURS PARASITES ///
        Vector v1, v2;
        int diff, pastDiff1=1999, pastDiff2=2999; // grandes valeurs arbitraires
        
        for(unsigned i=0; i<vectors.size(); i++)
        {
            vectors[i].orienteVersLeHaut();
            diff = vectors[i].calcDifference(this->v1past, this->v2past);
            if(diff >= 2000 && diff < pastDiff2)
            {
                v2 = vectors[i];
                pastDiff2 = diff;
            }
            else if(diff >= 1000 && diff < pastDiff1)
            {
                v1 = vectors[i];
                pastDiff1 = diff;
            }
        }        
        if(v1 == Vector(0,0,0,0) || v2 == Vector(0,0,0,0)) 
        {
            bt.printf("1 seul vecteur mais 2 en vrai\n\r");
            return (v1 != Vector(0,0,0,0)) ? (v1.getCoeffDir() < 0) ? 70 : -70 : (v2.getCoeffDir() < 0) ? 70 : -70;
        }
        diff1 = pastDiff1 - 1000; // provisoire
        diff2 = pastDiff2 - 2000; // provisoire
        this->v1past = v1;
        this->v2past = v2;
        
        /// CALCUL DE LA DEVIATION ///
        int dev;
        Point dir = this->getPointDir(v1, v2);
        
        if(dir.y == 0)
            dev = (SCREEN_CENTER - dir.x) * (100.f / SCREEN_CENTER);
        else if(dir.x == 0)
            dev = 75;
        else
            dev = -75;
            
        return dev;
    }
    else
        return INFO_NON_EXPLOITABLE;
}

Point NXPCam::getPointDir(Vector& v1, Vector& v2)
{
    Vector vM = Vector::getVectorAuMilieuDe(v1, v2);
    
    return vM.projection();
}

void NXPCam::affVectors() const
{
    bt.printf("Vector 1 : ");
    this->v1past.affVector(bt);
    bt.printf("Vector 2 : ");
    this->v2past.affVector(bt);
}

void NXPCam::affDiffs() const
{
    bt.printf("diff1 : %d\n\rdiff2 : %d\n\r", diff1, diff2);
}