SmartVehicle-Basis
SmartFactory
Vision.cpp
Go to the documentation of this file.
1
23
// #include "Vision.h"
24
// #include "Arduino.h"
25
26
// Vision::Vision(int startAngle, int visionPin, int dddelay, int toleranceL, int toleranceR) {
27
// DBFUNCCALLln("Vision::Vision(int startAngle, int visionPin, int dddelay, int toleranceL, int toleranceR)");
28
// DBINFO1("Initializing vision...");
29
// pixy.init();
30
// _servoPin = visionPin;
31
// ddelay = dddelay;
32
33
// toleranceLeft = toleranceL;
34
// toleranceRight = toleranceR;
35
36
// visionState.servoAngle = startAngle;
37
// visionState.target = 0;
38
// target1 = 0;
39
// target2 = 0;
40
// target3 = 0;
41
// target4 = 0;
42
// target5 = 0;
43
// target6 = 0;
44
// target7 = 0;
45
// target8 = 0;
46
// targetFound = 0;
47
// targetFound1 = 0;
48
// targetFound2 = 0;
49
// targetFound3 = 0;
50
// targetFound4 = 0;
51
// failCounter = 0;
52
53
// DBINFO1ln("compelete!");
54
// }
55
56
// void Vision::loop(VisionState *state) {
57
// if (state->reset == true) {
58
// reset(state);
59
// }
60
// visionServo.attach(_servoPin);
61
// DBINFO1ln("Beginn visionloop");
62
// static int ii = 0; //Cycle variable
63
// DBINFO1("I: ");
64
// DBINFO1ln(ii);
65
// int j;
66
// uint16_t blocks;
67
// char buf[32];
68
// DBINFO1("Target: ");
69
// visionState.target = state->target;
70
// DBINFO1ln(visionState.target);
71
72
// blocks = pixy.getBlocks();
73
// DBINFO1("ServoAngle: ");
74
// DBINFO1(visionState.servoAngle);
75
// visionServo.write(visionState.servoAngle);
76
// state->servoAngle = visionState.servoAngle;
77
// state->targetDetected = visionState.targetDetected;
78
// DBINFO1("targetdetected");
79
// DBINFO1ln(state->targetDetected);
80
// delay(13); //Empirically testet value
81
// DBINFO1ln("before first if-case");
82
// if (blocks) {
83
// DBINFO1ln("inside first if-case");
84
// ii++;
85
// if ((ii % ddelay) == 0) {
86
// failCounter = 0;
87
// DBINFO1("Current angle: ");
88
// DBINFO1ln(visionState.servoAngle);
89
90
// sprintf(buf, "Detected %d:\n", blocks);
91
// DBINFO1(buf);
92
// for (j = 0; j < blocks; j++) {
93
// sprintf(buf, " block %d: ", j);
94
// DBINFO1(buf);
95
// pixy.blocks[j].print();
96
// }
97
// DBINFO1("The Target is = ");
98
// DBINFO1ln(visionState.target);
99
// DBINFO1("signature= ");
100
// DBINFO1(pixy.blocks[0].signature);
101
// DBINFO1(" | ");
102
// DBINFO1(pixy.blocks[1].signature);
103
// DBINFO1(" | ");
104
// DBINFO1(pixy.blocks[2].signature);
105
// DBINFO1(" | ");
106
// DBINFO1(pixy.blocks[3].signature);
107
// DBINFO1(" | ");
108
// DBINFO1(pixy.blocks[4].signature);
109
// DBINFO1(" | ");
110
// DBINFO1(pixy.blocks[5].signature);
111
// DBINFO1(" | ");
112
// DBINFO1(pixy.blocks[6].signature);
113
// DBINFO1(" | ");
114
// DBINFO1ln(pixy.blocks[7].signature);
115
// target1 = 0;
116
// target2 = 0;
117
// target3 = 0;
118
// target4 = 0;
119
// target5 = 0;
120
// target6 = 0;
121
// target7 = 0;
122
// target8 = 0;
123
// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124
// if ((pixy.blocks[0].signature == visionState.target) && (pixy.blocks[0].x > toleranceRight) && (targetFound1 == 0)) {
125
// target1 = 1;
126
// visionState.servoAngle--;
127
// DBINFO1ln("Target1 true");
128
// } else {
129
// target1 = 0;
130
// }
131
132
// if ((pixy.blocks[0].signature == visionState.target) && (pixy.blocks[0].x < toleranceLeft) && (targetFound1 == 0) && (target1 == 0)) {
133
// target2 = 1;
134
// visionState.servoAngle++;
135
// DBINFO1ln("Target2 true");
136
// } else {
137
// target2 = 0;
138
// }
139
140
// if ((pixy.blocks[1].signature == visionState.target) && (pixy.blocks[1].x > toleranceRight) && (targetFound2 == 0) && (target1 == 0) && (target2 == 0)) {
141
// target3 = 1;
142
// visionState.servoAngle--;
143
// DBINFO1ln("Target3 true");
144
// } else {
145
// target3 = 0;
146
// }
147
148
// if ((pixy.blocks[1].signature == visionState.target) && (pixy.blocks[1].x < toleranceLeft) && (targetFound2 == 0) && (target1 == 0) && (target2 == 0) && (target3 == 0)) {
149
// target4 = 1;
150
// visionState.servoAngle++;
151
// DBINFO1ln("Target4 true");
152
// } else {
153
// target4 = 0;
154
// }
155
156
// if ((pixy.blocks[2].signature == visionState.target) && (pixy.blocks[2].x > toleranceRight) && (targetFound3 == 0) && (target1 == 0) && (target2 == 0) && (target3 == 0) && (target4 == 0)) {
157
// target5 = 1;
158
// visionState.servoAngle--;
159
// DBINFO1ln("Target5 true");
160
// } else {
161
// target5 = 0;
162
// }
163
164
// if ((pixy.blocks[2].signature == visionState.target) && (pixy.blocks[2].x < toleranceLeft) && (targetFound3 == 0) && (target1 == 0) && (target2 == 0) && (target3 == 0) && (target4 == 0) && (target5 == 0)) {
165
// target6 = 1;
166
// visionState.servoAngle++;
167
// DBINFO1ln("Target6 true");
168
// } else {
169
// target6 = 0; ///<@todo Fehler im sourcecode!?
170
// }
171
172
// if ((pixy.blocks[3].signature == visionState.target) && (pixy.blocks[3].x > toleranceRight) && (targetFound4 == 0) && (target1 == 0) && (target2 == 0) && (target3 == 0) && (target4 == 0) && (target5 == 0) && (target6 == 0)) {
173
// target7 = 1;
174
// visionState.servoAngle--;
175
// DBINFO1ln("Target7 true");
176
// } else {
177
// target7 = 0;
178
// }
179
180
// if ((pixy.blocks[3].signature == visionState.target) && (pixy.blocks[3].x < toleranceLeft) && (targetFound4 == 0) && (target1 == 0) && (target2 == 0) && (target3 == 0) && (target4 == 0) && (target5 == 0) && (target6 == 0) && (target7 == 0)) {
181
// target8 = 1;
182
// visionState.servoAngle++;
183
// DBINFO1ln("Target8 true");
184
// } else {
185
// target8 = 0;
186
// }
187
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
188
// if ((pixy.blocks[0].signature == visionState.target) && (pixy.blocks[0].x >= toleranceRight) && (pixy.blocks[0].x <= toleranceLeft)) {
189
// targetFound1 = 1;
190
// } else {
191
// targetFound1 = 0;
192
// }
193
194
// if ((pixy.blocks[1].signature == visionState.target) && (pixy.blocks[1].x >= toleranceRight) && (pixy.blocks[1].x <= toleranceLeft)) {
195
// targetFound2 = 1;
196
// } else {
197
// targetFound2 = 0;
198
// }
199
200
// if ((pixy.blocks[2].signature == visionState.target) && (pixy.blocks[2].x >= toleranceRight) && (pixy.blocks[2].x <= toleranceLeft)) {
201
// targetFound3 = 1;
202
// } else {
203
// targetFound3 = 0;
204
// }
205
206
// if ((pixy.blocks[3].signature == visionState.target) && (pixy.blocks[3].x >= toleranceRight) && (pixy.blocks[3].x <= toleranceLeft)) {
207
// targetFound4 = 1;
208
// } else {
209
// targetFound4 = 0;
210
// }
211
212
// targetFound = target1 + target2 + target3 + target4 + target5 + target6 + target7 + target8 + targetFound1 + targetFound2 + targetFound3 + targetFound4;
213
214
// DBINFO1ln();
215
// DBINFO1ln("All targets: ");
216
// DBINFO1(target1);
217
// DBINFO1(target2);
218
// DBINFO1(target2);
219
// DBINFO1(target3);
220
// DBINFO1(target4);
221
// DBINFO1(target5);
222
// DBINFO1(target6);
223
// DBINFO1(target7);
224
// DBINFO1ln(target8);
225
// DBINFO1ln("Found targets: ");
226
// DBINFO1(targetFound1);
227
// DBINFO1(targetFound2);
228
// DBINFO1(targetFound3);
229
// DBINFO1ln(targetFound4);
230
231
// if ((visionState.servoAngle > 179) && (targetFound == 0)) {
232
// visionState.servoAngle = 0;
233
// DBWARNINGln("Wrong object detected!!");
234
// }
235
// if ((visionState.servoAngle <= 179) && (targetFound == 0)) {
236
// visionState.servoAngle++;
237
// DBWARNINGln("Wrong object detected!!");
238
// }
239
// DBINFO1ln("------------------------END---------------------------");
240
// }
241
// } else {
242
// ii++;
243
// DBINFO1("I:");
244
// DBINFO1ln(ii);
245
// if ((ii % ddelay) == 0) {
246
// failCounter++;
247
// if (failCounter > 8) {
248
// targetFound = 0;
249
// targetFound1 = 0;
250
// targetFound2 = 0;
251
// targetFound3 = 0;
252
// targetFound4 = 0;
253
// DBINFO1ln("Nothing found");
254
255
// if ((visionState.servoAngle > 179) && (targetFound == 0)) {
256
// visionState.servoAngle = 0;
257
// DBINFO1ln("Adjust camera");
258
// }
259
// if ((visionState.servoAngle <= 179) && (targetFound == 0)) {
260
// visionState.servoAngle = visionState.servoAngle + 1;
261
// DBINFO1ln("Adjust camera");
262
// }
263
264
// DBINFO1ln("----------------------END-------------------------------");
265
// }
266
// }
267
// }
268
// // DBINFO1ln("before signaturechange");
269
// pixy.blocks[0].signature = 10;
270
// pixy.blocks[1].signature = 10;
271
// pixy.blocks[2].signature = 10;
272
// pixy.blocks[3].signature = 10;
273
// pixy.blocks[4].signature = 10;
274
// pixy.blocks[5].signature = 10;
275
// pixy.blocks[6].signature = 10;
276
// pixy.blocks[7].signature = 10;
277
278
// if (ii == 200) {
279
// ii = 0;
280
// }
281
282
// if ((targetFound1 == 1) || (targetFound2 == 1) || (targetFound3 == 1) || (targetFound4 == 1)) {
283
// DBINFO1ln("targetdetected inside loop");
284
// state->targetDetected = true;
285
// } else {
286
// state->targetDetected = false;
287
// }
288
// visionServo.detach();
289
// }
290
291
// void Vision::turnVision(int angle) {
292
// DBFUNCCALLln("Vision::turnVision(int angle)");
293
// DBINFO1("Angle: ");
294
// DBINFO1ln(angle);
295
// visionServo.attach(_servoPin);
296
// visionServo.write(angle);
297
// visionServo.detach();
298
// }
299
300
// void Vision::reset(VisionState *state) {
301
// DBFUNCCALLln("Vision::reset()");
302
// failCounter = 0;
303
// targetFound4 = 0;
304
// targetFound3 = 0;
305
// targetFound2 = 0;
306
// targetFound1 = 0;
307
// targetFound = 0;
308
// target8 = 0;
309
// target7 = 0;
310
// target6 = 0;
311
// target5 = 0;
312
// target4 = 0;
313
// target3 = 0;
314
// target2 = 0;
315
// target1 = 0;
316
// visionState.target = 0;
317
// visionState.servoAngle = 90;
318
// state->reset = false;
319
// }
320
// void Vision::Test(const int test) {
321
// DBFUNCCALLln("Vision::Test(const int test)");
322
// int testdelay = 10;
323
// if (test == 0 || test == 1) { //Test Servo turnVision
324
// visionServo.attach(_servoPin);
325
// int maxinc = 90; //Chose max increment range for left/ritgh-turn
326
// for (size_t i = 0; i < maxinc; i++) {
327
// turnVision(i + 90);
328
// delay(testdelay);
329
// }
330
// for (int i = -maxinc; i < maxinc; i++) {
331
// turnVision(-i + 90);
332
// delay(testdelay);
333
// }
334
// for (size_t i = 0; i < maxinc; i++) {
335
// turnVision(i);
336
// delay(testdelay);
337
// }
338
// turnVision(90);
339
// delay(testdelay);
340
// visionServo.detach();
341
// delay(testdelay);
342
// } else if (test == 0 || test == 2) {
343
// /*func*/
344
// } else {
345
// DBERROR("No vailid Test selected.");
346
// DBINFO1("Your Input: ");
347
// DBINFO1ln(test);
348
// }
349
// }
src
EnvironmentDetection
Vision
Vision.cpp
Generated by
1.8.15
1.8.15 1.8.15