diff options
Diffstat (limited to 'source/Concept/Framework/modules/interpreter/ball_tracker.c')
| -rwxr-xr-x | source/Concept/Framework/modules/interpreter/ball_tracker.c | 125 |
1 files changed, 125 insertions, 0 deletions
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c new file mode 100755 index 0000000..6679d3e --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -0,0 +1,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 = -1.0f;
+ 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 = -1.0f;
+ return;
+ break;
+ }
+
+ direction = (intensity[greatestIntensity] * mainDirection +
+ intensity[secondIntensity] * secondDirection) /
+ (intensity[greatestIntensity] + intensity[secondIntensity]);
+ }
+ else
+ {
+ direction = mainDirection;
+ }
+ }
+ else
+ {
+ direction = -1.0f;
+ }
+}
|