File content as of revision 0:8743b606abc3:

#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;
}