From 3c3c628b617dc53f0b7b59285c7d67888074c33d Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Sun, 18 Feb 2007 00:14:00 +0000
Subject: +++ Additional Codework

---
 .../Framework/modules/interpreter/ball_tracker.c   | 125 +++++++++++++++++++++
 1 file changed, 125 insertions(+)
 create mode 100755 source/Concept/Framework/modules/interpreter/ball_tracker.c

(limited to 'source/Concept/Framework/modules/interpreter/ball_tracker.c')

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;
+	}
+}
-- 
cgit v1.2.3