Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#include "Detection.h"
#include "Arduino.h"
#include "Capteur.h"
#include "Ultrason.h"
#include "Sharp.h"
//enum sharpDetectant { AUCUN, GAUCHE, DROITE, TTAVANT, ARRIERE, TTGAUCHE, TTDROITE, TT, AUCUNETHAUT, GAUCHEETHAUT, DROITEETHAUT, TTAVANTETHAUT, ARRIEREETHAUT, TTGAUCHEETHAUT, TTDROITEETHAUT, TTETHAUT, AUCUNETMILIEU, GAUCHEETMILIEU, DROITEETMILIEU, TTAVANTETMILIEU, ARRIEREETMILIEU, TTGAUCHEETMILIEU, TTDROITEETMILIEU, TTETMILIEU, AUCUNETHAUTETMILIEU, GAUCHEETHAUTETMILIEU, DROITEETHAUTETMILIEU, TTAVANTETHAUTETMILIEU, ARRIEREETHAUTETMILIEU, TTGAUCHEETHAUTETMILIEU, TTDROITEETHAUTETMILIEU, TTETHAUTETMILIEU};
//enum sharpDetectant { AUCUN, GAUCHE, DROITE, TTAVANT, ARRIERE, TTGAUCHE, TTDROITE, TT };
enum numeroSharp { GAUCHE, DROITE, MILIEU, HAUT, ARRIERE };
enum sens { AVANCER, RECULER, TOURNEGAUCHE, TOURNEDROITE, AVANCERBOUTEILLE };
/*#define pinSharpAvG A6
#define pinSharpAvD A4
#define pinSharpArr A7
#define pinUltrason 10*/
#define seuilSharpAvG 350
#define seuilSharpAvD 350
#define seuilSharpMilieu 350
#define seuilSharpHaut 350
#define seuilSharpArr 350
Detection::Detection()
{
for (int initcvqqch = 0 ; initcvqqch<NOMBRECAPTEUR ; initcvqqch++)
{
capteurVoitQQch[initcvqqch]=0;
capteurVoitObstacle[initcvqqch]=0;
}
//int t=0;
//Ultrason ping(10);
capteurSharp = (Sharp**) malloc(sizeof( Sharp*)*NOMBRECAPTEUR);
capteurSharp[GAUCHE] = new Sharp(A1, seuilSharpAvG);
capteurSharp[DROITE] = new Sharp(A3, seuilSharpAvD);
capteurSharp[MILIEU] = new Sharp(A5, seuilSharpMilieu);
capteurSharp[HAUT] = new Sharp(A4, seuilSharpHaut);
capteurSharp[ARRIERE]= new Sharp(A6, seuilSharpArr);
//Sharp capteurAvantG(A6, seuilSharpAvG), capteurAvantD(A4, seuilSharpAvD), capteurArriere(A7, seuilSharpArr);//Initialisation des 3 capteurs sharp.
/*capteurAvantG = new Sharp(A1, seuilSharpAvG);
capteurAvantD = new Sharp(A3, seuilSharpAvD);
capteurMilieu = new Sharp(A5, seuilSharpMilieu);//peut-etre l'inverse avec haut
capteurHaut = new Sharp(A4, seuilSharpHaut);//peut-etre l'inverse avec milieu
capteurArriere = new Sharp(A6, seuilSharpArr);//Initialisation des 5 capteurs sharp.*/
//ping = new Ultrason(t);
}
Detection::~Detection()
{
}
int Detection::JePeuxAvancer(int sensVoulu)//Utilise LireCapteursSharp pour déterminer si le robot peux avancer ou non.
{
LireCapteursDistance();//On met à jour
if(sensVoulu==AVANCER)
{
return !(capteurVoitObstacle[GAUCHE] || capteurVoitObstacle[DROITE] || capteurVoitObstacle[MILIEU]|| capteurVoitQQch[HAUT]);// || capteurVoitQQch[HAUT]);
}
if(sensVoulu==RECULER)
{
return !(capteurVoitObstacle[ARRIERE]);
}
if(sensVoulu==TOURNEGAUCHE)
{
return !(capteurVoitObstacle[GAUCHE] || capteurVoitObstacle[MILIEU]);
}
if(sensVoulu==TOURNEDROITE)
{
return !(capteurVoitObstacle[DROITE] || capteurVoitObstacle[MILIEU]);
}
if(sensVoulu==AVANCERBOUTEILLE)
{
if((capteurVoitObstacle[GAUCHE] || capteurVoitObstacle[DROITE] || capteurVoitObstacle[MILIEU]) && !capteurVoitObstacle[HAUT])
{
return 2;//On est à deux doigts de taper la bouteille
}
else
{
return !(capteurVoitObstacle[HAUT]);
}
}
}
void Detection::LireCapteursDistance()//Systeme de mémoire hystérisis : si on a vu qqch, il faut rien voir pendant 8 tours pour décider qu'il n'y a plus d'obstacle, et inversement pour voir si on n'a rien vu.
{
for(int iLireCapteurs = GAUCHE ; iLireCapteurs<= ARRIERE ; iLireCapteurs++)
{
if(capteurVoitObstacle[iLireCapteurs])//Si on sait etre devant un obstacle
{
if(!capteurSharp[iLireCapteurs]->VoitQQch())
{
capteurVoitQQch[iLireCapteurs]--;//On décrémente si on ne voit rien.
}
else
{
capteurVoitQQch[iLireCapteurs]=8;//Sinon on remet le compteur à 8.
}
if(capteurVoitQQch[iLireCapteurs]==0)
{
capteurVoitObstacle[iLireCapteurs]=0;
}
}
else//Si on n'est pas devant un obstacle
{
if(capteurSharp[iLireCapteurs]->VoitQQch())
{
capteurVoitQQch[iLireCapteurs]++;//On décrémente si on ne voit rien.
}
else
{
capteurVoitQQch[iLireCapteurs]=0;//Sinon on remet le compteur à 8.
}
if(capteurVoitQQch[iLireCapteurs]==8)
{
capteurVoitObstacle[iLireCapteurs]=1;
}
}
}
/*
capteurVoitQQch[GAUCHE]=min(8,(capteurVoitQQch[GAUCHE]+1)*capteurAvantG->VoitQQch());
capteurVoitQQch[DROITE]=min(8,(capteurVoitQQch[DROITE]+1)*capteurAvantD->VoitQQch());
capteurVoitQQch[MILIEU]=min(8,(capteurVoitQQch[MILIEU]+1)*capteurMilieu->VoitQQch());
capteurVoitQQch[HAUT]=min(8,(capteurVoitQQch[MILIEU]+1)*capteurHaut->VoitQQch());
capteurVoitQQch[ARRIERE]=min(8,(capteurVoitQQch[ARRIERE]+1)*capteurArriere->VoitQQch());*/
/*capteurVoitQQch[GAUCHE]=capteurAvantG->VoitQQch();
capteurVoitQQch[DROITE]=capteurAvantD->VoitQQch();
capteurVoitQQch[MILIEU]=capteurMilieu->VoitQQch();
capteurVoitQQch[HAUT]=capteurHaut->VoitQQch();
capteurVoitQQch[ARRIERE]=capteurArriere->VoitQQch();*/
}