diff options
Diffstat (limited to 'source/Concept/Framework/modules/interpreter')
4 files changed, 212 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;
+ }
+}
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h new file mode 100755 index 0000000..ed8801f --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -0,0 +1,35 @@ +#ifndef _BALL_TRACKER_H
+#define _BALL_TRACKER_H
+
+#include "../../stdafx.h"
+
+class Ball_Tracker : public IO_Module
+{
+public:
+ Ball_Tracker()
+ {
+ this->parent = NULL;
+ this->moduleId = 0;
+ this->direction = -1.0f;
+ }
+
+ Ball_Tracker(uint32 trackerId)
+ {
+ this->parent = NULL;
+ this->moduleId = trackerId;
+ this->direction = -1.0f;
+ }
+
+protected:
+ float direction;
+
+public:
+ void Update();
+
+ float GetBallDirection()
+ {
+ return direction;
+ }
+};
+
+#endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c new file mode 100755 index 0000000..262069a --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -0,0 +1,7 @@ +#include "position_tracker.h"
+
+//-----------------------------------------------------------------------------
+void Position_Tracker::Update()
+{
+ //insert code here
+}
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h new file mode 100755 index 0000000..b31dd8f --- /dev/null +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -0,0 +1,45 @@ +#ifndef _POSITION_TRACKER_H
+#define _POSITION_TRACKER_H
+
+#include "../../stdafx.h"
+
+class Position_Tracker : public IO_Module
+{
+public:
+ Position_Tracker()
+ {
+ this->parent = NULL;
+ this->moduleId = 0;
+ }
+
+ Position_Tracker(uint32 trackerId)
+ {
+ this->parent = NULL;
+ this->moduleId = trackerId;
+ }
+
+protected:
+ float positionX;
+ float positionY;
+ float orientation;
+
+public:
+ void Update();
+
+ float GetPositionX()
+ {
+ return positionX;
+ }
+
+ float GetPositionY()
+ {
+ return positionY;
+ }
+
+ float GetOrientation()
+ {
+ return orientation;
+ }
+};
+
+#endif |