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
|
#include "avr.h"
#include "global.h"
#include "hardware.h"
#include "util.h"
#include "LineSensor.h"
#include "LineSensorArray.h"
#include "Motor.h"
#include "Navigation.h"
#include "Srf10.h"
static void delay() {
for(unsigned long i = 0; i < 25000; i++)
asm("nop");
}
int main() {
Status status = Ok;
initHardware();
Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
LineSensor *lineSensors[5];
for(int i = 0; i < 5; i++)
lineSensors[i] = new LineSensor(i);
LineSensorArray *lineSensorArray = new LineSensorArray(lineSensors);
lineSensorArray->setMinimumDifference(CAL_MIN_DIFF);
lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF);
//Srf10 *srf10Right = new Srf10(0xE0);
Srf10 *srf10Left = new Srf10(0xE2);
//srf10Right->setUnit(Centimeters);
srf10Left->setUnit(Centimeters);
//Srf10 *srf10Last = srf10Left;
do {
lineSensorArray->update();
} while(!lineSensorArray->calibrate());
navigation->setSpeed(DEFAULT_SPEED);
while(true) {
lineSensorArray->update();
//srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
//srf10Last->updateDistance();
switch(status) {
case Ok:
if(lineSensorArray->getLine() == 0.0 && lineSensorArray->getSharpness() == 1.0)
lineSensorArray->calibrate();
// TODO: victims!!
// TODO: debris!!
if(lineSensorArray->getSharpness() < 0.0)
break;
else if(lineSensorArray->getSharpness() == 0.0) {
// TODO: white?
}
else {
if(lineSensorArray->getLine() == 0.0) {
navigation->setSpeed(DEFAULT_SPEED);
navigation->setDirection(0.0);
navigation->setSpin(0.0);
}
else if(ABS(lineSensorArray->getLine()) < 1.0) {
navigation->setSpeed(DEFAULT_SPEED);
navigation->setDirection(0.0);
if(lineSensorArray->getLine() < 0.0)
navigation->setSpin(CURVE_SPIN);
else
navigation->setSpin(-CURVE_SPIN);
}
else if(ABS(lineSensorArray->getLine()) < 1.5) {
navigation->setSpeed(DEFAULT_SPEED);
if(lineSensorArray->getLine() < 0.0) {
navigation->setDirection(STRAFE_DIRECTION);
navigation->setSpin(CURVE_SPIN);
}
else {
navigation->setDirection(-STRAFE_DIRECTION);
navigation->setSpin(-CURVE_SPIN);
}
}
else {
navigation->setSpeed(0.0);
if(lineSensorArray->getLine() < 0.0)
navigation->setSpin(TURN_SPIN);
else
navigation->setSpin(-TURN_SPIN);
}
}
break;
case White:
break;
case Debris:
break;
}
}
return 0;
}
|
