Codework

4 files changed, 44 insertions, 12 deletions

diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c
index 1701121..2d85b96 100755
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c
@@ -109,9 +109,16 @@ void Ball_Tracker::Update()
 					break;

 			}

 

+			if(fabs(mainDirection - secondDirection) > PI)

+			{

+				min(mainDirection, secondDirection) += 2.0f * PI;

+			}

+

 			direction = (intensity[greatestIntensity] * mainDirection + 

 									intensity[secondIntensity] * secondDirection) / 

 									(intensity[greatestIntensity] + intensity[secondIntensity]);

+

+			direction = easyAngle(direction);

 		}

 		else

 		{

diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h
index bea4a19..c62f05e 100755
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h
@@ -30,6 +30,16 @@ public:
 	{

 		return direction;

 	}

+

+	bool KnowsBallDirection()

+	{

+		return direction != EMPTY_FLOAT;

+	}

+

+	bool RobotHasBall()

+	{

+		//fill me!

+	}

 };

 

 #endif
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index 63a7f5f..a64ab60 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -3,9 +3,11 @@
 //-----------------------------------------------------------------------------

 void Position_Tracker::Update()

 {

+	// We want to use the mouse-sensors

 	Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);

 	Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

 

+	// Generate a vector for the left mouse

 	int8 leftX = mouseLeft->GetXMovement();

 	int8 leftY = mouseLeft->GetYMovement();

 	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);

@@ -14,12 +16,14 @@ void Position_Tracker::Update()
 	float movementLeftX = cos(angleLeft) * distanceLeft;

 	float movementLeftY = sin(angleLeft) * distanceLeft;

 

+	// AVR calculates a little bit strange ;)

 	if(!leftX && !leftY)

 	{

 		movementLeftX = 0;

 		movementLeftY = 0;

 	}

 

+	// Generate a vector for the right mouse

 	int8 rightX = mouseRight->GetXMovement();

 	int8 rightY = mouseRight->GetYMovement();

 	float distanceRight = sqrt(rightX * rightX + rightY * rightY);

@@ -28,21 +32,24 @@ void Position_Tracker::Update()
 	float movementRightX = cos(angleRight) * distanceRight;

 	float movementRightY = sin(angleRight) * distanceRight;

 

+	// AVR calculates a little bit strange ;)

 	if(!rightX && !rightY)

 	{

 		movementRightX = 0;

 		movementRightY = 0;

 	}

 

-	float movementDifferenceX = (movementRightX + mouseRight->GetPositionX()) - (movementLeftX + mouseLeft->GetPositionX());

+	// Generate vector from P:left to P:right

+	float movementDifferenceX = movementRightX - movementLeftX;

 	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());

 

+	// Calculate the difference of orientation

 	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);

 

 	float robotMovementX = movementDifferenceX / 2.0f;

 	float robotMovementY = movementDifferenceY / 2.0f;

 	robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));

-	robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (-mouseLeft->GetPositionX() * sin(orientationDiff));

+	robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));

 	//float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);

 

 	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;

@@ -58,7 +65,7 @@ void Position_Tracker::Update()
 	}

 

 	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);

-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1);

 	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);

 

 	this->positionX += absoluteDiffX;

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index e93ef0c..47d0740 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -32,19 +32,27 @@ protected:
 public:

 	void Update();

 

-	float GetPositionX()

-	{

-		return positionX;

+	// Sets the current position; x and y in mm

+	void SetPosition(int newPositionX, int newPositionY, float newOrientation) {

+		positionX = newPositionX*(TICKS_PER_CM/10.0f);

+		positionY = newPositionY*(TICKS_PER_CM/10.0f);

+		orientation = easyAngle(newOrientation);

 	}

 

-	float GetPositionY()

-	{

-		return positionY;

+	// returns x-koordinate in mm

+	int GetPositionX() 	{

+		return (int)((positionX*10.0f)/TICKS_PER_CM);

 	}

 

-	float GetOrientation()

-	{

-		return orientation;

+	// returns y-koordinate in mm

+	int GetPositionY() 	{

+		return (int)((positionY*10.0f)/TICKS_PER_CM);

+	}

+

+	// returns orientation

+	float GetOrientation()	{

+		return 0.0f; //tmp!!!!!!!!!!

+		//return orientation;

 	}

 };