| User | Revision | Line number | New contents of line |
|---|
| hogeschool |
0:838a1c343613
|
1
|
#include "mbed.h"
|
| hogeschool |
0:838a1c343613
|
2
|
#include "Servo.h"
|
| hogeschool |
0:838a1c343613
|
3
|
|
| hogeschool |
0:838a1c343613
|
4
|
|
| hogeschool |
0:838a1c343613
|
5
|
//#include "BusIn.h"
|
| hogeschool |
0:838a1c343613
|
6
|
//#include "DigitalIn.h"
|
| hogeschool |
0:838a1c343613
|
7
|
|
| hogeschool |
0:838a1c343613
|
8
|
|
| hogeschool |
0:838a1c343613
|
9
|
//--------------------------------------------windsensor begin
|
| hogeschool |
0:838a1c343613
|
10
|
|
| hogeschool |
0:838a1c343613
|
11
|
Timer t;
|
| hogeschool |
0:838a1c343613
|
12
|
void Goedzo_staan_groen();
|
| hogeschool |
0:838a1c343613
|
13
|
void Rechtsdraaien();
|
| hogeschool |
0:838a1c343613
|
14
|
void Linksdraaien();
|
| hogeschool |
0:838a1c343613
|
15
|
void Goedzo_BO_groen();
|
| hogeschool |
0:838a1c343613
|
16
|
void Boven_draaien();
|
| hogeschool |
0:838a1c343613
|
17
|
void Onder_draaien();
|
| hogeschool |
0:838a1c343613
|
18
|
void veiligheidsmodus();
|
| hogeschool |
0:838a1c343613
|
19
|
void counter();
|
| hogeschool |
0:838a1c343613
|
20
|
|
| hogeschool |
0:838a1c343613
|
21
|
Serial pc(USBTX, USBRX);
|
| hogeschool |
0:838a1c343613
|
22
|
|
| hogeschool |
0:838a1c343613
|
23
|
volatile int IRQcount;
|
| hogeschool |
0:838a1c343613
|
24
|
// DigitalIn inputpin(p5);//was p2
|
| hogeschool |
0:838a1c343613
|
25
|
int pin_irq = 0; //IRQ that matches to pin 2
|
| hogeschool |
0:838a1c343613
|
26
|
|
| hogeschool |
0:838a1c343613
|
27
|
InterruptIn Windinterrupt(p5);
|
| hogeschool |
0:838a1c343613
|
28
|
|
| hogeschool |
0:838a1c343613
|
29
|
|
| hogeschool |
0:838a1c343613
|
30
|
DigitalIn PMS_PIN(p6);
|
| hogeschool |
0:838a1c343613
|
31
|
bool count = false;
|
| hogeschool |
0:838a1c343613
|
32
|
int t1 = 0, t2 = 0;
|
| hogeschool |
0:838a1c343613
|
33
|
int hits = 0;
|
| hogeschool |
0:838a1c343613
|
34
|
int rpp = 0;
|
| hogeschool |
0:838a1c343613
|
35
|
int rps = 0;
|
| hogeschool |
0:838a1c343613
|
36
|
int pwm = 13;
|
| hogeschool |
0:838a1c343613
|
37
|
|
| hogeschool |
0:838a1c343613
|
38
|
const int numReadings = 10;
|
| hogeschool |
0:838a1c343613
|
39
|
int readings[numReadings]; // the readings from the analog input
|
| hogeschool |
0:838a1c343613
|
40
|
int index1 = 0; // the index of the current reading
|
| hogeschool |
0:838a1c343613
|
41
|
int total = 0; // the running total
|
| hogeschool |
0:838a1c343613
|
42
|
int average = 0; // the average
|
| hogeschool |
0:838a1c343613
|
43
|
|
| hogeschool |
0:838a1c343613
|
44
|
|
| hogeschool |
0:838a1c343613
|
45
|
int gemrij1=0;
|
| hogeschool |
0:838a1c343613
|
46
|
|
| hogeschool |
0:838a1c343613
|
47
|
int Aantalmetingen1 = 20;
|
| hogeschool |
0:838a1c343613
|
48
|
//int reeks1[Aantalmetingen1]={0};
|
| hogeschool |
0:838a1c343613
|
49
|
const int maxwindpulsen = 20; //Te groot voor te testen
|
| hogeschool |
0:838a1c343613
|
50
|
const int MinimaalLicht = 20;
|
| hogeschool |
0:838a1c343613
|
51
|
//------------------------------------------- Windsensor einde
|
| hogeschool |
0:838a1c343613
|
52
|
Servo myservo(p21);
|
| hogeschool |
0:838a1c343613
|
53
|
int pos = 0;
|
| hogeschool |
0:838a1c343613
|
54
|
int wachttijd = 1000;
|
| hogeschool |
0:838a1c343613
|
55
|
int buttonState = 0; // variable for reading the pushbutton status
|
| hogeschool |
0:838a1c343613
|
56
|
|
| hogeschool |
0:838a1c343613
|
57
|
DigitalOut linksdraaien (p14);
|
| hogeschool |
0:838a1c343613
|
58
|
DigitalOut goedzo (p13);
|
| hogeschool |
0:838a1c343613
|
59
|
DigitalOut rechtsdraaien (p12);
|
| hogeschool |
0:838a1c343613
|
60
|
|
| hogeschool |
0:838a1c343613
|
61
|
DigitalOut bovendraaien (p11);
|
| hogeschool |
0:838a1c343613
|
62
|
DigitalOut goedzoBO (p10);
|
| hogeschool |
0:838a1c343613
|
63
|
DigitalOut onderdraaien (p9);
|
| hogeschool |
0:838a1c343613
|
64
|
|
| hogeschool |
0:838a1c343613
|
65
|
|
| hogeschool |
0:838a1c343613
|
66
|
AnalogIn Pin (p17); // Analog input pin that the potentiometer is attached to
|
| hogeschool |
0:838a1c343613
|
67
|
AnalogIn Pin1 (p18); // Analog input pin that the potentiometer is attached to
|
| hogeschool |
0:838a1c343613
|
68
|
AnalogIn Pin2 (p19); // Analog input pin that the potentiometer is attached to
|
| hogeschool |
0:838a1c343613
|
69
|
AnalogIn Pin3 (p20); // Analog input pin that the potentiometer is attached to
|
| hogeschool |
0:838a1c343613
|
70
|
//const int analogOutPin4 = 9; // Analog output pin that the LED is attached to
|
| hogeschool |
0:838a1c343613
|
71
|
|
| hogeschool |
0:838a1c343613
|
72
|
|
| hogeschool |
0:838a1c343613
|
73
|
|
| hogeschool |
0:838a1c343613
|
74
|
|
| hogeschool |
0:838a1c343613
|
75
|
|
| hogeschool |
0:838a1c343613
|
76
|
const int aantalmetingen = 2;
|
| hogeschool |
0:838a1c343613
|
77
|
|
| hogeschool |
0:838a1c343613
|
78
|
int rij1[aantalmetingen]= {0};
|
| hogeschool |
0:838a1c343613
|
79
|
int rij2[aantalmetingen]= {0};
|
| hogeschool |
0:838a1c343613
|
80
|
int rij3[aantalmetingen]= {0};
|
| hogeschool |
0:838a1c343613
|
81
|
int rij4[aantalmetingen]= {0};
|
| hogeschool |
0:838a1c343613
|
82
|
|
| hogeschool |
0:838a1c343613
|
83
|
|
| hogeschool |
0:838a1c343613
|
84
|
|
| hogeschool |
0:838a1c343613
|
85
|
|
| hogeschool |
0:838a1c343613
|
86
|
unsigned long PrevTime=0;
|
| hogeschool |
0:838a1c343613
|
87
|
int sensorValue = 0; // value read from the pot
|
| hogeschool |
0:838a1c343613
|
88
|
int sensorValue1 = 0; // value read from the pot
|
| hogeschool |
0:838a1c343613
|
89
|
int sensorValue2= 0; // value read from the pot
|
| hogeschool |
0:838a1c343613
|
90
|
int sensorValue3 = 0; // value read from the pot
|
| hogeschool |
0:838a1c343613
|
91
|
int outputValue = 0; // value output to the PWM (analog out)
|
| hogeschool |
0:838a1c343613
|
92
|
|
| hogeschool |
0:838a1c343613
|
93
|
|
| hogeschool |
0:838a1c343613
|
94
|
|
| hogeschool |
0:838a1c343613
|
95
|
|
| hogeschool |
0:838a1c343613
|
96
|
int ugemiddeld(int*);
|
| hogeschool |
0:838a1c343613
|
97
|
|
| hogeschool |
0:838a1c343613
|
98
|
int ugemiddeld(int *rijgem)
|
| hogeschool |
0:838a1c343613
|
99
|
{
|
| hogeschool |
0:838a1c343613
|
100
|
int som=0;
|
| hogeschool |
0:838a1c343613
|
101
|
for (int i=0; i<aantalmetingen; i++) {
|
| hogeschool |
0:838a1c343613
|
102
|
som=som+rijgem[i];
|
| hogeschool |
0:838a1c343613
|
103
|
}
|
| hogeschool |
0:838a1c343613
|
104
|
int gemiddeld=(som/aantalmetingen);
|
| hogeschool |
0:838a1c343613
|
105
|
return (int)gemiddeld;
|
| hogeschool |
0:838a1c343613
|
106
|
}
|
| hogeschool |
0:838a1c343613
|
107
|
|
| hogeschool |
0:838a1c343613
|
108
|
|
| hogeschool |
0:838a1c343613
|
109
|
|
| hogeschool |
0:838a1c343613
|
110
|
|
| hogeschool |
0:838a1c343613
|
111
|
|
| hogeschool |
0:838a1c343613
|
112
|
|
| hogeschool |
0:838a1c343613
|
113
|
//-------------------------------------------------------------------------- Windsensor begin
|
| hogeschool |
0:838a1c343613
|
114
|
|
| hogeschool |
0:838a1c343613
|
115
|
// put your setup code here, to run once:
|
| hogeschool |
0:838a1c343613
|
116
|
|
| hogeschool |
0:838a1c343613
|
117
|
// attachInterrupt(pin_irq, IRQcounter, RISING);
|
| hogeschool |
0:838a1c343613
|
118
|
|
| hogeschool |
0:838a1c343613
|
119
|
|
| hogeschool |
0:838a1c343613
|
120
|
|
| hogeschool |
0:838a1c343613
|
121
|
// put your setup code here, to run once:
|
| hogeschool |
0:838a1c343613
|
122
|
//-------------------------------------------------------------------------- Windsensor einde
|
| hogeschool |
0:838a1c343613
|
123
|
|
| hogeschool |
0:838a1c343613
|
124
|
|
| hogeschool |
0:838a1c343613
|
125
|
|
| hogeschool |
0:838a1c343613
|
126
|
// initialize the pushbutton pin as an input:
|
| hogeschool |
0:838a1c343613
|
127
|
|
| hogeschool |
0:838a1c343613
|
128
|
//-------------------------------------------------------------------------- Windsensor begin
|
| hogeschool |
0:838a1c343613
|
129
|
void IRQcounter()
|
| hogeschool |
0:838a1c343613
|
130
|
{
|
| hogeschool |
0:838a1c343613
|
131
|
IRQcount++;
|
| hogeschool |
0:838a1c343613
|
132
|
}
|
| hogeschool |
0:838a1c343613
|
133
|
long int frq;
|
| hogeschool |
0:838a1c343613
|
134
|
//-------------------------------------------------------------------------- Windsensor einde
|
| hogeschool |
0:838a1c343613
|
135
|
|
| hogeschool |
0:838a1c343613
|
136
|
int main()
|
| hogeschool |
0:838a1c343613
|
137
|
{
|
| hogeschool |
0:838a1c343613
|
138
|
t.start();
|
| hogeschool |
0:838a1c343613
|
139
|
while (1) {
|
| hogeschool |
0:838a1c343613
|
140
|
Windinterrupt.rise(&counter);
|
| hogeschool |
0:838a1c343613
|
141
|
|
| hogeschool |
0:838a1c343613
|
142
|
for (int thisReading = 0; thisReading < numReadings; thisReading++)
|
| hogeschool |
0:838a1c343613
|
143
|
readings[thisReading] = 0;
|
| hogeschool |
0:838a1c343613
|
144
|
|
| hogeschool |
0:838a1c343613
|
145
|
//-------------------------------------------------------------------------- Windsensor begin
|
| hogeschool |
0:838a1c343613
|
146
|
t2 = t.read();
|
| hogeschool |
0:838a1c343613
|
147
|
if (t2 >= (t1 + 1000)) {
|
| hogeschool |
0:838a1c343613
|
148
|
rpp = hits/1;// delen door 6 voor aantal rondjes
|
| hogeschool |
0:838a1c343613
|
149
|
rps = hits/6;// delen door 6 voor aantal rondjes
|
| hogeschool |
0:838a1c343613
|
150
|
hits = 0;
|
| hogeschool |
0:838a1c343613
|
151
|
|
| hogeschool |
0:838a1c343613
|
152
|
|
| hogeschool |
0:838a1c343613
|
153
|
}
|
| hogeschool |
0:838a1c343613
|
154
|
|
| hogeschool |
0:838a1c343613
|
155
|
// subtract the last reading:
|
| hogeschool |
0:838a1c343613
|
156
|
total= total - readings[index1];
|
| hogeschool |
0:838a1c343613
|
157
|
// read from the sensor:
|
| hogeschool |
0:838a1c343613
|
158
|
readings[index1] = rpp;
|
| hogeschool |
0:838a1c343613
|
159
|
// add the reading to the total:
|
| hogeschool |
0:838a1c343613
|
160
|
total= total + readings[index1];
|
| hogeschool |
0:838a1c343613
|
161
|
// advance to the next position in the array:
|
| hogeschool |
0:838a1c343613
|
162
|
index1 = index1 + 1;
|
| hogeschool |
0:838a1c343613
|
163
|
|
| hogeschool |
0:838a1c343613
|
164
|
// if we're at the end of the array...
|
| hogeschool |
0:838a1c343613
|
165
|
if (index1 >= numReadings)
|
| hogeschool |
0:838a1c343613
|
166
|
// ...wrap around to the beginning:
|
| hogeschool |
0:838a1c343613
|
167
|
index1 = 0;
|
| hogeschool |
0:838a1c343613
|
168
|
|
| hogeschool |
0:838a1c343613
|
169
|
// calculate the average:
|
| hogeschool |
0:838a1c343613
|
170
|
average = total / numReadings;
|
| hogeschool |
0:838a1c343613
|
171
|
// send it to the computer as ASCII digits
|
| hogeschool |
0:838a1c343613
|
172
|
|
| hogeschool |
0:838a1c343613
|
173
|
// wait_ms(1); // wait_ms in between reads for stability
|
| hogeschool |
0:838a1c343613
|
174
|
|
| hogeschool |
0:838a1c343613
|
175
|
pc.printf("pulsen per seconde: %d\t\t",rpp);
|
| hogeschool |
0:838a1c343613
|
176
|
pc.printf("RPM: %d\t\t",rps*60 );
|
| hogeschool |
0:838a1c343613
|
177
|
pc.printf("GEM: %d\n",average);
|
| hogeschool |
0:838a1c343613
|
178
|
|
| hogeschool |
0:838a1c343613
|
179
|
wait_ms(1000);
|
| hogeschool |
0:838a1c343613
|
180
|
|
| hogeschool |
0:838a1c343613
|
181
|
|
| hogeschool |
0:838a1c343613
|
182
|
// attachInterrupt(0, counter, RISING);
|
| hogeschool |
0:838a1c343613
|
183
|
//-------------------------------------------------------------------------- Windsensor einde
|
| hogeschool |
0:838a1c343613
|
184
|
|
| hogeschool |
0:838a1c343613
|
185
|
// read the state of the pushbutton value:
|
| hogeschool |
0:838a1c343613
|
186
|
|
| hogeschool |
0:838a1c343613
|
187
|
// read the analog in value:
|
| hogeschool |
0:838a1c343613
|
188
|
PrevTime = t.read();
|
| hogeschool |
0:838a1c343613
|
189
|
//while (PrevPrevPrevPrevTime==wachttijd)//variabele van maken!!!
|
| hogeschool |
0:838a1c343613
|
190
|
|
| hogeschool |
0:838a1c343613
|
191
|
for (int huidigemeting=0; huidigemeting <= aantalmetingen; huidigemeting++) {
|
| hogeschool |
0:838a1c343613
|
192
|
wait_ms(10);
|
| hogeschool |
0:838a1c343613
|
193
|
rij1[huidigemeting] = Pin.read();
|
| hogeschool |
0:838a1c343613
|
194
|
rij2[huidigemeting] = Pin1.read();
|
| hogeschool |
0:838a1c343613
|
195
|
rij3[huidigemeting] = Pin2.read();
|
| hogeschool |
0:838a1c343613
|
196
|
rij4[huidigemeting] = Pin3.read();
|
| hogeschool |
0:838a1c343613
|
197
|
|
| hogeschool |
0:838a1c343613
|
198
|
}
|
| hogeschool |
0:838a1c343613
|
199
|
|
| hogeschool |
0:838a1c343613
|
200
|
|
| hogeschool |
0:838a1c343613
|
201
|
sensorValue = Pin.read();
|
| hogeschool |
0:838a1c343613
|
202
|
sensorValue1 = Pin1.read();
|
| hogeschool |
0:838a1c343613
|
203
|
sensorValue2 = Pin2.read();
|
| hogeschool |
0:838a1c343613
|
204
|
sensorValue3 = Pin3.read();
|
| hogeschool |
0:838a1c343613
|
205
|
// map it to the range of the analog out:
|
| hogeschool |
0:838a1c343613
|
206
|
|
| hogeschool |
0:838a1c343613
|
207
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
208
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
209
|
|
| hogeschool |
0:838a1c343613
|
210
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
211
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
212
|
|
| hogeschool |
0:838a1c343613
|
213
|
int gemrij1en3=(gemrij1 + gemrij3)/2;
|
| hogeschool |
0:838a1c343613
|
214
|
int gemrij2en4=(gemrij2 + gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
215
|
int gemrij1en2=(gemrij1 + gemrij2)/2;
|
| hogeschool |
0:838a1c343613
|
216
|
int gemrij3en4=(gemrij3 + gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
217
|
|
| hogeschool |
0:838a1c343613
|
218
|
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ Te weinig Ligt
|
| hogeschool |
0:838a1c343613
|
219
|
int TeWeinigLicht = (gemrij1 + gemrij2 + gemrij3 + gemrij4)/4;
|
| hogeschool |
0:838a1c343613
|
220
|
|
| hogeschool |
0:838a1c343613
|
221
|
pc.printf("------------------------------------" );
|
| hogeschool |
0:838a1c343613
|
222
|
pc.printf("sensorGem = %d, %d, %d, %d \n " ,gemrij1, gemrij2, gemrij3, gemrij4 );
|
| hogeschool |
0:838a1c343613
|
223
|
|
| hogeschool |
0:838a1c343613
|
224
|
|
| hogeschool |
0:838a1c343613
|
225
|
// check if the pushbutton is not pressed.
|
| hogeschool |
0:838a1c343613
|
226
|
// if it is, the buttonState is LOW
|
| hogeschool |
0:838a1c343613
|
227
|
if ( average <= maxwindpulsen && TeWeinigLicht >= MinimaalLicht)
|
| hogeschool |
0:838a1c343613
|
228
|
|
| hogeschool |
0:838a1c343613
|
229
|
{
|
| hogeschool |
0:838a1c343613
|
230
|
if(gemrij2en4 > gemrij1en3-10&&gemrij2en4 < gemrij1en3+10) {
|
| hogeschool |
0:838a1c343613
|
231
|
Goedzo_staan_groen();
|
| hogeschool |
0:838a1c343613
|
232
|
wait_ms(10);
|
| hogeschool |
0:838a1c343613
|
233
|
|
| hogeschool |
0:838a1c343613
|
234
|
|
| hogeschool |
0:838a1c343613
|
235
|
}
|
| hogeschool |
0:838a1c343613
|
236
|
|
| hogeschool |
0:838a1c343613
|
237
|
if(gemrij2en4 > gemrij1en3+10) {
|
| hogeschool |
0:838a1c343613
|
238
|
Rechtsdraaien();
|
| hogeschool |
0:838a1c343613
|
239
|
|
| hogeschool |
0:838a1c343613
|
240
|
} else {
|
| hogeschool |
0:838a1c343613
|
241
|
if(gemrij1en3 > gemrij2en4+10) {
|
| hogeschool |
0:838a1c343613
|
242
|
Linksdraaien();
|
| hogeschool |
0:838a1c343613
|
243
|
|
| hogeschool |
0:838a1c343613
|
244
|
}
|
| hogeschool |
0:838a1c343613
|
245
|
}
|
| hogeschool |
0:838a1c343613
|
246
|
|
| hogeschool |
0:838a1c343613
|
247
|
if(gemrij1en2 > gemrij3en4-10&&gemrij1en2 < gemrij3en4+10) {
|
| hogeschool |
0:838a1c343613
|
248
|
Goedzo_BO_groen();
|
| hogeschool |
0:838a1c343613
|
249
|
|
| hogeschool |
0:838a1c343613
|
250
|
}
|
| hogeschool |
0:838a1c343613
|
251
|
|
| hogeschool |
0:838a1c343613
|
252
|
if(gemrij1en2 > gemrij3en4+10) {
|
| hogeschool |
0:838a1c343613
|
253
|
Boven_draaien();
|
| hogeschool |
0:838a1c343613
|
254
|
|
| hogeschool |
0:838a1c343613
|
255
|
} else {
|
| hogeschool |
0:838a1c343613
|
256
|
if(gemrij3en4 > gemrij1en2+10) {
|
| hogeschool |
0:838a1c343613
|
257
|
Onder_draaien();
|
| hogeschool |
0:838a1c343613
|
258
|
}
|
| hogeschool |
0:838a1c343613
|
259
|
}
|
| hogeschool |
0:838a1c343613
|
260
|
}
|
| hogeschool |
0:838a1c343613
|
261
|
|
| hogeschool |
0:838a1c343613
|
262
|
else {
|
| hogeschool |
0:838a1c343613
|
263
|
|
| hogeschool |
0:838a1c343613
|
264
|
veiligheidsmodus();
|
| hogeschool |
0:838a1c343613
|
265
|
}
|
| hogeschool |
0:838a1c343613
|
266
|
|
| hogeschool |
0:838a1c343613
|
267
|
}
|
| hogeschool |
0:838a1c343613
|
268
|
}
|
| hogeschool |
0:838a1c343613
|
269
|
|
| hogeschool |
0:838a1c343613
|
270
|
void Goedzo_staan_groen()
|
| hogeschool |
0:838a1c343613
|
271
|
{
|
| hogeschool |
0:838a1c343613
|
272
|
|
| hogeschool |
0:838a1c343613
|
273
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
274
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
275
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
276
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
277
|
int gemrij1en3=(gemrij1 + gemrij3)/2;
|
| hogeschool |
0:838a1c343613
|
278
|
int gemrij2en4=(gemrij2 + gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
279
|
|
| hogeschool |
0:838a1c343613
|
280
|
|
| hogeschool |
0:838a1c343613
|
281
|
pc.printf("Is goed zo! Verschil links rechts = %d , Pos = %d \n", gemrij2en4-gemrij1en3, pos);
|
| hogeschool |
0:838a1c343613
|
282
|
|
| hogeschool |
0:838a1c343613
|
283
|
// wait_ms(10)(1000);
|
| hogeschool |
0:838a1c343613
|
284
|
rechtsdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
285
|
goedzo = 1; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
286
|
linksdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
287
|
}
|
| hogeschool |
0:838a1c343613
|
288
|
|
| hogeschool |
0:838a1c343613
|
289
|
void Linksdraaien()
|
| hogeschool |
0:838a1c343613
|
290
|
{
|
| hogeschool |
0:838a1c343613
|
291
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
292
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
293
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
294
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
295
|
int gemrij1en3 = (gemrij1+gemrij3)/2;
|
| hogeschool |
0:838a1c343613
|
296
|
int gemrij2en4 = (gemrij2+gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
297
|
|
| hogeschool |
0:838a1c343613
|
298
|
if(gemrij1en3 > gemrij2en4+10) {
|
| hogeschool |
0:838a1c343613
|
299
|
pos+=2;
|
| hogeschool |
0:838a1c343613
|
300
|
myservo.write(pos);
|
| hogeschool |
0:838a1c343613
|
301
|
|
| hogeschool |
0:838a1c343613
|
302
|
pc.printf("Sensor1en3 = %d < Sensor2en4 = %d \n ",gemrij1en3, gemrij2en4);
|
| hogeschool |
0:838a1c343613
|
303
|
// wait_ms(10)(1000);
|
| hogeschool |
0:838a1c343613
|
304
|
rechtsdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
305
|
goedzo = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
306
|
linksdraaien = 1; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
307
|
}
|
| hogeschool |
0:838a1c343613
|
308
|
}
|
| hogeschool |
0:838a1c343613
|
309
|
|
| hogeschool |
0:838a1c343613
|
310
|
void Rechtsdraaien()
|
| hogeschool |
0:838a1c343613
|
311
|
{
|
| hogeschool |
0:838a1c343613
|
312
|
|
| hogeschool |
0:838a1c343613
|
313
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
314
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
315
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
316
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
317
|
int gemrij1en3 = (gemrij1+gemrij3)/2;
|
| hogeschool |
0:838a1c343613
|
318
|
int gemrij2en4 = (gemrij2+gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
319
|
pos-=2;
|
| hogeschool |
0:838a1c343613
|
320
|
myservo.write(pos);
|
| hogeschool |
0:838a1c343613
|
321
|
pc.printf("Sensor1en3 = %d > Sensor2en4 = %d \n ",gemrij1en3,gemrij2en4);
|
| hogeschool |
0:838a1c343613
|
322
|
|
| hogeschool |
0:838a1c343613
|
323
|
// wait_ms(1000);
|
| hogeschool |
0:838a1c343613
|
324
|
rechtsdraaien = 1; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
325
|
goedzo = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
326
|
linksdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
327
|
}
|
| hogeschool |
0:838a1c343613
|
328
|
void Goedzo_BO_groen()
|
| hogeschool |
0:838a1c343613
|
329
|
{
|
| hogeschool |
0:838a1c343613
|
330
|
|
| hogeschool |
0:838a1c343613
|
331
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
332
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
333
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
334
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
335
|
int gemrij1en2=(gemrij1 + gemrij2)/2;
|
| hogeschool |
0:838a1c343613
|
336
|
int gemrij3en4=(gemrij3 + gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
337
|
|
| hogeschool |
0:838a1c343613
|
338
|
|
| hogeschool |
0:838a1c343613
|
339
|
pc.printf("Is goed zo! Verschil boven onder = %d , Pos = %d",gemrij1en2-gemrij3en4,pos );
|
| hogeschool |
0:838a1c343613
|
340
|
|
| hogeschool |
0:838a1c343613
|
341
|
wait_ms(1000);
|
| hogeschool |
0:838a1c343613
|
342
|
onderdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
343
|
goedzoBO = 1; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
344
|
bovendraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
345
|
}
|
| hogeschool |
0:838a1c343613
|
346
|
|
| hogeschool |
0:838a1c343613
|
347
|
void Onder_draaien()
|
| hogeschool |
0:838a1c343613
|
348
|
{
|
| hogeschool |
0:838a1c343613
|
349
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
350
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
351
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
352
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
353
|
int gemrij1en2=(gemrij1 + gemrij2)/2;
|
| hogeschool |
0:838a1c343613
|
354
|
int gemrij3en4=(gemrij3 + gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
355
|
|
| hogeschool |
0:838a1c343613
|
356
|
if(gemrij3en4 > gemrij1en2+10) {
|
| hogeschool |
0:838a1c343613
|
357
|
//pos+=2;
|
| hogeschool |
0:838a1c343613
|
358
|
//myservo.write(pos);
|
| hogeschool |
0:838a1c343613
|
359
|
|
| hogeschool |
0:838a1c343613
|
360
|
pc.printf("Sensor3en4 = %d < Sensor1en2 = %d ", gemrij3en4, gemrij1en2);
|
| hogeschool |
0:838a1c343613
|
361
|
|
| hogeschool |
0:838a1c343613
|
362
|
// wait_ms(1000);
|
| hogeschool |
0:838a1c343613
|
363
|
onderdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
364
|
goedzoBO = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
365
|
bovendraaien = 1; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
366
|
}
|
| hogeschool |
0:838a1c343613
|
367
|
}
|
| hogeschool |
0:838a1c343613
|
368
|
|
| hogeschool |
0:838a1c343613
|
369
|
void Boven_draaien()
|
| hogeschool |
0:838a1c343613
|
370
|
{
|
| hogeschool |
0:838a1c343613
|
371
|
|
| hogeschool |
0:838a1c343613
|
372
|
int gemrij1=ugemiddeld(rij1);
|
| hogeschool |
0:838a1c343613
|
373
|
int gemrij2=ugemiddeld(rij2);
|
| hogeschool |
0:838a1c343613
|
374
|
int gemrij3=ugemiddeld(rij3);
|
| hogeschool |
0:838a1c343613
|
375
|
int gemrij4=ugemiddeld(rij4);
|
| hogeschool |
0:838a1c343613
|
376
|
int gemrij1en2=(gemrij1 + gemrij2)/2;
|
| hogeschool |
0:838a1c343613
|
377
|
int gemrij3en4=(gemrij3 + gemrij4)/2;
|
| hogeschool |
0:838a1c343613
|
378
|
//pos-=2;
|
| hogeschool |
0:838a1c343613
|
379
|
//myservo.write(pos);
|
| hogeschool |
0:838a1c343613
|
380
|
pc.printf("Sensor3en4 = %d > Sensor1en2 = %d", gemrij3en4, gemrij1en2);
|
| hogeschool |
0:838a1c343613
|
381
|
|
| hogeschool |
0:838a1c343613
|
382
|
// wait_ms(1000);
|
| hogeschool |
0:838a1c343613
|
383
|
onderdraaien = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
384
|
goedzoBO = 0; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
385
|
bovendraaien = 1; // turn the LED off by making the voltage LOW
|
| hogeschool |
0:838a1c343613
|
386
|
}
|
| hogeschool |
0:838a1c343613
|
387
|
void veiligheidsmodus()
|
| hogeschool |
0:838a1c343613
|
388
|
{
|
| hogeschool |
0:838a1c343613
|
389
|
while (pos <= 180) {
|
| hogeschool |
0:838a1c343613
|
390
|
// goes from 180 degrees to 0 degrees
|
| hogeschool |
0:838a1c343613
|
391
|
myservo.write(pos); // tell servo to go to position in variable 'pos'
|
| hogeschool |
0:838a1c343613
|
392
|
wait_ms(15);
|
| hogeschool |
0:838a1c343613
|
393
|
pos += 1;
|
| hogeschool |
0:838a1c343613
|
394
|
// GROOT wait_ms VOOR WACHTEN TOT WEER OMHOOG GAAN OFTEWEL VOLGENDE METING
|
| hogeschool |
0:838a1c343613
|
395
|
}
|
| hogeschool |
0:838a1c343613
|
396
|
}
|
| hogeschool |
0:838a1c343613
|
397
|
//-------------------------------------------------------------------------- Windsensor begin
|
| hogeschool |
0:838a1c343613
|
398
|
void counter(void)
|
| hogeschool |
0:838a1c343613
|
399
|
{
|
| hogeschool |
0:838a1c343613
|
400
|
if (PMS_PIN.read() == 1) {
|
| hogeschool |
0:838a1c343613
|
401
|
if (!count) {
|
| hogeschool |
0:838a1c343613
|
402
|
count = true;
|
| hogeschool |
0:838a1c343613
|
403
|
hits++;
|
| hogeschool |
0:838a1c343613
|
404
|
}
|
| hogeschool |
0:838a1c343613
|
405
|
count = false;
|
| hogeschool |
0:838a1c343613
|
406
|
} else {
|
| hogeschool |
0:838a1c343613
|
407
|
count = false;
|
| hogeschool |
0:838a1c343613
|
408
|
}
|
| hogeschool |
0:838a1c343613
|
409
|
}
|
| hogeschool |
0:838a1c343613
|
410
|
//-------------------------------------------------------------------------- Windsensor einde
|
| hogeschool |
0:838a1c343613
|
411
|
|