File content as of revision 1:a9af73d5abd4:

#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 x0past, x1past; // les 2 pts au bout des 2 vecteurs de la frame derniere
    int x, xInutile; // si 1 seul vecteur détecté
    int dev;
    
    if (pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR == PIXY2_VECTOR) // Si la pixy a capté des vectors
    {
        if (pixy.Pixy2_numVectors == 3) // Si la pixy a capté des vectors
        {
            /// CAS OU ON VOIT 1 VECTEUR DE NOTRE PISTE + LES 2 DE L'AUTRE PISTE ///
            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);
            this->v3 = Vector(pixy.Pixy2_vectors[2].pixX0, pixy.Pixy2_vectors[2].pixY0, pixy.Pixy2_vectors[2].pixX1, pixy.Pixy2_vectors[2].pixY1);
            
            if(this->v1.getCoeffDir() / abs(this->v1.getCoeffDir()) == this->v2.getCoeffDir() / abs(this->v2.getCoeffDir()) && this->v2.getCoeffDir() / abs(this->v2.getCoeffDir()) == this->v3.getCoeffDir() / abs(this->v3.getCoeffDir())) // s'ils ont tous les 3 le même signe (= 2 des trois sont les vecteurs de l'autre côté de la piste)
                return (this->v1.getCoeffDir() < 0) ? 100 : -100;  
            
            /// CAS OU ON VOIT LES 2 VECTEURS DE NOTRE PISTE + UN DE L'AUTRE PISTE ///
            bt.printf("3 vecteurs !\n\r");
            int 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
            int 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)
            int x2 = (pixy.Pixy2_vectors[2].pixY1 < pixy.Pixy2_vectors[2].pixY0) ? pixy.Pixy2_vectors[2].pixX1 : pixy.Pixy2_vectors[2].pixX0;
            
            xInutile = (x0past == 0 && x1past == 0) ? this->getXinutile(x0, x1, x2) : this->getXinutile(x0, x1, x2, x0past, x1past);
                
            if(x0 == xInutile)
                x0 = x2;
            else if(x1 == xInutile)
                x1 = x2;
                
            centrePiste = (x0 + x1) / 2.f; // valeur entre 15 et 55
            dev = (int)((SCREEN_CENTER - centrePiste) * (200.f / SCREEN_CENTER));
            
            x0past = x0;
            x1past = x1;
            
            return dev;
        }
        else if (pixy.Pixy2_numVectors == 2)
        {
            /// CAS OU ON VOIT UN VECTEUR DE NOTRE PISTE + UN DE L'AUTRE PISTE ///
            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.getCoeffDir() / abs(this->v1.getCoeffDir()) == this->v2.getCoeffDir() / abs(this->v2.getCoeffDir())) // s'ils ont le même signe (= un des deux est le vecteur de l'autre côté de la piste)
                return (this->v1.getCoeffDir() < 0) ? 100 : -100;
            
            /// CAS NORMAL ///
            x0past = (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
            x1past = (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)
                
            centrePiste = (x0past + x1past) / 2.f; // valeur entre 15 et 55
            dev = (int)((SCREEN_CENTER - centrePiste) * (200.f / SCREEN_CENTER));
            return dev;
        }
        else if (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
        {
            bt.printf("Plus de 3 vecteurs !\n\r");
            return INFO_NON_EXPLOITABLE;
        }
    }
    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;
}

int NXPCam::getXinutile(int x0, int x1, int x2)
{
    if(abs(x0 - SCREEN_CENTER) >= abs(x1 - SCREEN_CENTER) && abs(x0 - SCREEN_CENTER) >= abs(x2 - SCREEN_CENTER))
        return x0;
    if(abs(x1 - SCREEN_CENTER) >= abs(x0 - SCREEN_CENTER) && abs(x1 - SCREEN_CENTER) >= abs(x2 - SCREEN_CENTER))
        return x1;
    return x2;
}

int NXPCam::getXinutile(int x0, int x1, int x2, int x0past, int x1past)
{
    int tabX[3] = {x0, x1, x2};
    
    for(int i=0; i<3; i++)
        for(int j=0; j<2-i; j++)
            if(tabX[j] > tabX[j+1])
            {
                int save = tabX[j];
                tabX[j] = tabX[j+1];
                tabX[j+1] = save;
            }
            
    x0 = tabX[0];
    x1 = tabX[1];
    x2 = tabX[2];
    
    if(x0past > x1past)
    {
        int savex0p = x0past;
        x0past = x1past;
        x1past = savex0p;
    }
    
    if(abs(x0 - x0past) > abs(x1 - x0past))
        return x0;
    return x2;
}