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@2:1103f5d61035, 2020-03-11 (annotated)

Committer:
Wael_H
Date:
Wed Mar 11 14:24:48 2020 +0000
Revision:
2:1103f5d61035
Parent:
1:a9af73d5abd4
Child:
3:07109a6bb472
publish IHM_bt pour ajout de commentaires

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