blob: 170112125bd943a489c245adf182cc291a5d5e45 (
plain)
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
|
#include "ball_tracker.h"
//-----------------------------------------------------------------------------
void Ball_Tracker::Update()
{
uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
uint16 intensity[sensorCount];
uint8 greatestIntensity = 0;
for(uint8 i = 0; i < sensorCount; i++)
{
IR_Sensor* currentSensor = parent->GetModule<IR_Sensor>(i + IO_SENSOR_IR_0_DEG);
intensity[i] = 1023 - currentSensor->GetIRIntensity();
if(intensity[i] < 24)
{
intensity[i] = 0;
}
if(intensity[i] > intensity[greatestIntensity])
{
greatestIntensity = i;
}
}
if(intensity[greatestIntensity])
{
uint8 secondIntensity = 0xFF;
uint8 leftSensor = (greatestIntensity + 1) % sensorCount;
uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount;
if(intensity[leftSensor])
{
secondIntensity = leftSensor;
}
if(intensity[rightSensor] > intensity[leftSensor])
{
secondIntensity = rightSensor;
}
float mainDirection;
switch(greatestIntensity + IO_SENSOR_IR_0_DEG)
{
case IO_SENSOR_IR_0_DEG:
mainDirection = 0;
break;
case IO_SENSOR_IR_30_DEG:
mainDirection = 1.0f * PI / 6.0f;
break;
case IO_SENSOR_IR_60_DEG:
mainDirection = 1.0f * PI / 3.0f;
break;
case IO_SENSOR_IR_100_DEG:
mainDirection = 5.0f * PI / 9.0f;
break;
case IO_SENSOR_IR_180_DEG:
mainDirection = PI;
break;
case IO_SENSOR_IR_260_DEG:
mainDirection = 13.0f * PI / 9.0f;
break;
case IO_SENSOR_IR_300_DEG:
mainDirection = 15.0f * PI / 9.0f;
break;
case IO_SENSOR_IR_330_DEG:
mainDirection = 33.0f * PI / 18.0f;
break;
default:
mainDirection = EMPTY_FLOAT;
return;
break;
}
if(secondIntensity != 0xFF)
{
float secondDirection;
switch(secondIntensity + IO_SENSOR_IR_0_DEG)
{
case IO_SENSOR_IR_0_DEG:
secondDirection = 0;
break;
case IO_SENSOR_IR_30_DEG:
secondDirection = 1.0f * PI / 6.0f;
break;
case IO_SENSOR_IR_60_DEG:
secondDirection = 1.0f * PI / 3.0f;
break;
case IO_SENSOR_IR_100_DEG:
secondDirection = 5.0f * PI / 9.0f;
break;
case IO_SENSOR_IR_180_DEG:
secondDirection = PI;
break;
case IO_SENSOR_IR_260_DEG:
secondDirection = 13.0f * PI / 9.0f;
break;
case IO_SENSOR_IR_300_DEG:
secondDirection = 15.0f * PI / 9.0f;
break;
case IO_SENSOR_IR_330_DEG:
secondDirection = 33.0f * PI / 18.0f;
break;
default:
secondDirection = EMPTY_FLOAT;
return;
break;
}
direction = (intensity[greatestIntensity] * mainDirection +
intensity[secondIntensity] * secondDirection) /
(intensity[greatestIntensity] + intensity[secondIntensity]);
}
else
{
direction = mainDirection;
}
}
else
{
direction = EMPTY_FLOAT;
}
}
|
