NXPCup_Cachan / Mbed 2 deprecated NXPCup

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Classes/NXPCam.cpp@3:07109a6bb472, 2020-03-12 (annotated)

Committer:
EISR
Date:
Thu Mar 12 15:22:02 2020 +0000
Revision:
3:07109a6bb472
Parent:
2:1103f5d61035 
commentaires de la partie bluetooth;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wael_H 0:8743b606abc3 1 #include "NXPCam.h"
Wael_H 2:1103f5d61035 2 #include <iostream>
Wael_H 2:1103f5d61035 3 #include <vector>
Wael_H 2:1103f5d61035 4
Wael_H 2:1103f5d61035 5 using namespace std;
Wael_H 0:8743b606abc3 6
Wael_H 0:8743b606abc3 7 extern Serial bt;
Wael_H 0:8743b606abc3 8
Wael_H 2:1103f5d61035 9 int diff1, diff2; // provisoire
Wael_H 2:1103f5d61035 10
EISR 3:07109a6bb472 11 NXPCam::NXPCam() : pixy(p28, p27),xG(0),xD(0)
Wael_H 0:8743b606abc3 12 {
Wael_H 0:8743b606abc3 13
Wael_H 0:8743b606abc3 14 }
Wael_H 0:8743b606abc3 15
Wael_H 2:1103f5d61035 16 /// INIT ///
Wael_H 2:1103f5d61035 17 void NXPCam::init()
Wael_H 2:1103f5d61035 18 {
Wael_H 2:1103f5d61035 19 Timer timer;
Wael_H 2:1103f5d61035 20 timer.start();
Wael_H 2:1103f5d61035 21 bt.printf("Init...\n\r");
Wael_H 2:1103f5d61035 22 /// INIT ATTENDS 2 VECTEURS ///
Wael_H 2:1103f5d61035 23 while(pixy.Pixy2_numVectors != 2 && timer.read() < 2)
Wael_H 2:1103f5d61035 24 pixy.pixy2_getAllFeature(PIXY2_VECTOR);
Wael_H 2:1103f5d61035 25
Wael_H 2:1103f5d61035 26 this->v1past = Vector(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
Wael_H 2:1103f5d61035 27 this->v2past = Vector(pixy.Pixy2_vectors[1].pixX0, pixy.Pixy2_vectors[1].pixY0, pixy.Pixy2_vectors[1].pixX1, pixy.Pixy2_vectors[1].pixY1);
Wael_H 2:1103f5d61035 28
EISR 3:07109a6bb472 29 xG=(pixy.Pixy2_vectors[0].pixX0 + pixy.Pixy2_vectors[0].pixX1)/2;
EISR 3:07109a6bb472 30 xD=(pixy.Pixy2_vectors[1].pixX0 + pixy.Pixy2_vectors[1].pixX1)/2;
EISR 3:07109a6bb472 31
Wael_H 2:1103f5d61035 32 if(this->v1past.estADroiteDe(this->v2past))
Wael_H 2:1103f5d61035 33 {
Wael_H 2:1103f5d61035 34 Vector savev1 = v1;
Wael_H 2:1103f5d61035 35 v1 = v2;
Wael_H 2:1103f5d61035 36 v2 = savev1;
Wael_H 2:1103f5d61035 37 }
Wael_H 2:1103f5d61035 38
Wael_H 2:1103f5d61035 39 bt.printf("Lancement du programme.\n\r");
Wael_H 2:1103f5d61035 40 }
Wael_H 2:1103f5d61035 41
Wael_H 2:1103f5d61035 42 /// DETECTION DEVIATION VIA POINT PROJETé ///
Wael_H 0:8743b606abc3 43 int NXPCam::getDeviationTrajectoire()
Wael_H 0:8743b606abc3 44 {
Wael_H 2:1103f5d61035 45 vector<Vector> vectors;
Wael_H 2:1103f5d61035 46 vector<Vector> vectorSupp;
Wael_H 0:8743b606abc3 47
Wael_H 2:1103f5d61035 48 if ((pixy.pixy2_getAllFeature(PIXY2_VECTOR) & PIXY2_VECTOR) == PIXY2_VECTOR) // Si la pixy a capté des vectors
Wael_H 0:8743b606abc3 49 {
Wael_H 2:1103f5d61035 50 /// CAS D'1 SEUL VECTEUR ///
Wael_H 2:1103f5d61035 51 if(pixy.Pixy2_numVectors == 1)
Wael_H 2:1103f5d61035 52 {
Wael_H 2:1103f5d61035 53 bt.printf("1 seul vecteur\n\r");
Wael_H 2:1103f5d61035 54 Vector v(pixy.Pixy2_vectors[0].pixX0, pixy.Pixy2_vectors[0].pixY0, pixy.Pixy2_vectors[0].pixX1, pixy.Pixy2_vectors[0].pixY1);
Wael_H 2:1103f5d61035 55 return -COEFF_VIRAGE/v.getCoeffDir(); // (v.getCoeffDir() < 0) ? 75 : -75;
Wael_H 2:1103f5d61035 56 }
Wael_H 2:1103f5d61035 57
Wael_H 2:1103f5d61035 58 /// RECOLTE DE TOUS LES VECTEURS + CREATION DE CEUX QUI SONT JOINTS ///
Wael_H 2:1103f5d61035 59 for(int i=0; i<pixy.Pixy2_numVectors; i++)
Wael_H 0:8743b606abc3 60 {
Wael_H 2:1103f5d61035 61 vectors.push_back(Vector(pixy.Pixy2_vectors[i].pixX0, pixy.Pixy2_vectors[i].pixY0, pixy.Pixy2_vectors[i].pixX1, pixy.Pixy2_vectors[i].pixY1));
Wael_H 2:1103f5d61035 62 for(int j=0; j<i; j++)
Wael_H 2:1103f5d61035 63 {
Wael_H 2:1103f5d61035 64 Vector newVector = vectors.back().testJoinedTo(vectors[j]);
Wael_H 1:a9af73d5abd4 65
Wael_H 2:1103f5d61035 66 if(newVector != Vector(0, 0, 0, 0))
Wael_H 2:1103f5d61035 67 {
Wael_H 2:1103f5d61035 68 bt.printf("Vectors joined\n\r");
Wael_H 2:1103f5d61035 69 vectorSupp.push_back(newVector);
Wael_H 2:1103f5d61035 70 vectors.pop_back();
Wael_H 2:1103f5d61035 71 vectors.erase(vectors.begin() + j);
Wael_H 2:1103f5d61035 72 }
Wael_H 2:1103f5d61035 73 }
Wael_H 2:1103f5d61035 74 }
Wael_H 2:1103f5d61035 75
Wael_H 2:1103f5d61035 76 for(unsigned i=0; i<vectorSupp.size(); i++)
Wael_H 2:1103f5d61035 77 vectors.push_back(vectorSupp[i]);
Wael_H 0:8743b606abc3 78
Wael_H 2:1103f5d61035 79 /// FILTRE DES VECTEURS PARASITES ///
Wael_H 2:1103f5d61035 80 Vector v1, v2;
Wael_H 2:1103f5d61035 81 int diff, pastDiff1=1999, pastDiff2=2999; // grandes valeurs arbitraires
Wael_H 2:1103f5d61035 82
Wael_H 2:1103f5d61035 83 for(unsigned i=0; i<vectors.size(); i++)
Wael_H 1:a9af73d5abd4 84 {
Wael_H 2:1103f5d61035 85 vectors[i].orienteVersLeHaut();
Wael_H 2:1103f5d61035 86 diff = vectors[i].calcDifference(this->v1past, this->v2past);
Wael_H 2:1103f5d61035 87 if(diff >= 2000 && diff < pastDiff2)
Wael_H 2:1103f5d61035 88 {
Wael_H 2:1103f5d61035 89 v2 = vectors[i];
Wael_H 2:1103f5d61035 90 pastDiff2 = diff;
Wael_H 2:1103f5d61035 91 }
Wael_H 2:1103f5d61035 92 else if(diff >= 1000 && diff < pastDiff1)
Wael_H 2:1103f5d61035 93 {
Wael_H 2:1103f5d61035 94 v1 = vectors[i];
Wael_H 2:1103f5d61035 95 pastDiff1 = diff;
Wael_H 2:1103f5d61035 96 }
Wael_H 2:1103f5d61035 97 }
Wael_H 2:1103f5d61035 98 if(v1 == Vector(0,0,0,0) || v2 == Vector(0,0,0,0))
Wael_H 2:1103f5d61035 99 {
Wael_H 2:1103f5d61035 100 bt.printf("1 seul vecteur mais 2 en vrai\n\r");
Wael_H 2:1103f5d61035 101 return (v1 != Vector(0,0,0,0)) ? (v1.getCoeffDir() < 0) ? 70 : -70 : (v2.getCoeffDir() < 0) ? 70 : -70;
Wael_H 2:1103f5d61035 102 }
Wael_H 2:1103f5d61035 103 diff1 = pastDiff1 - 1000; // provisoire
Wael_H 2:1103f5d61035 104 diff2 = pastDiff2 - 2000; // provisoire
Wael_H 2:1103f5d61035 105 this->v1past = v1;
Wael_H 2:1103f5d61035 106 this->v2past = v2;
Wael_H 2:1103f5d61035 107
Wael_H 2:1103f5d61035 108 /// CALCUL DE LA DEVIATION ///
Wael_H 2:1103f5d61035 109 int dev;
Wael_H 2:1103f5d61035 110 Point dir = this->getPointDir(v1, v2);
Wael_H 2:1103f5d61035 111
Wael_H 2:1103f5d61035 112 if(dir.y == 0)
Wael_H 2:1103f5d61035 113 dev = (SCREEN_CENTER - dir.x) * (100.f / SCREEN_CENTER);
Wael_H 2:1103f5d61035 114 else if(dir.x == 0)
Wael_H 2:1103f5d61035 115 dev = 75;
Wael_H 2:1103f5d61035 116 else
Wael_H 2:1103f5d61035 117 dev = -75;
Wael_H 1:a9af73d5abd4 118
Wael_H 2:1103f5d61035 119 return dev;
Wael_H 0:8743b606abc3 120 }
Wael_H 0:8743b606abc3 121 else
Wael_H 0:8743b606abc3 122 return INFO_NON_EXPLOITABLE;
Wael_H 0:8743b606abc3 123 }
Wael_H 0:8743b606abc3 124
Wael_H 2:1103f5d61035 125 Point NXPCam::getPointDir(Vector& v1, Vector& v2)
Wael_H 0:8743b606abc3 126 {
Wael_H 2:1103f5d61035 127 Vector vM = Vector::getVectorAuMilieuDe(v1, v2);
Wael_H 0:8743b606abc3 128
Wael_H 2:1103f5d61035 129 return vM.projection();
Wael_H 1:a9af73d5abd4 130 }
Wael_H 1:a9af73d5abd4 131
Wael_H 2:1103f5d61035 132 void NXPCam::affVectors() const
Wael_H 1:a9af73d5abd4 133 {
Wael_H 2:1103f5d61035 134 bt.printf("Vector 1 : ");
Wael_H 2:1103f5d61035 135 this->v1past.affVector(bt);
Wael_H 2:1103f5d61035 136 bt.printf("Vector 2 : ");
Wael_H 2:1103f5d61035 137 this->v2past.affVector(bt);
Wael_H 1:a9af73d5abd4 138 }
Wael_H 1:a9af73d5abd4 139
Wael_H 2:1103f5d61035 140 void NXPCam::affDiffs() const
Wael_H 1:a9af73d5abd4 141 {
Wael_H 2:1103f5d61035 142 bt.printf("diff1 : %d\n\rdiff2 : %d\n\r", diff1, diff2);
Wael_H 0:8743b606abc3 143 }