SoccerTeam, Stuff from Magdeburg

6 files changed, 218 insertions, 21 deletions

diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c
index 16fdfb4..a882abb 100755
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c
@@ -3,6 +3,8 @@
 //-----------------------------------------------------------------------------

 void Ball_Tracker::Update()

 {

+	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

+

 	uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;

 	uint16 intensity[sensorCount];

 	uint8 greatestIntensity = 0;

@@ -26,15 +28,12 @@ void Ball_Tracker::Update()
 		{

 			if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran

 			{

-				ballHeld = true;

-				ballHeldCounter = 100;

+				if(ballHeldCounter < 10) ballHeldCounter++;

 			}

-			else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran

+			else if(ballHeldCounter > 0)
 			{

-				ballHeld = true;

 				ballHeldCounter--;		

 			}

-			else ballHeld = false; // ansonsten hat er den Ball nicht

 		}

 	}

 

@@ -132,13 +131,15 @@ void Ball_Tracker::Update()
 			direction = (intensity[greatestIntensity] * mainDirection + 

 									intensity[secondIntensity] * secondDirection) / 

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

-

-			direction = easyAngle(direction);

 		}

 		else

 		{

 			direction = mainDirection;

 		}

+

+		direction += ourPositionTracker->GetOrientation();

+

+		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 cb90ff2..756a09f 100755
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h
@@ -12,7 +12,6 @@ public:
 		this->moduleId = 0;

 		this->direction = EMPTY_FLOAT;

 		this->ballHeldCounter = 0;

-		this->ballHeld = false;

 	}

 

 	Ball_Tracker(uint32 trackerId)

@@ -21,13 +20,11 @@ public:
 		this->moduleId = trackerId;

 		this->direction = EMPTY_FLOAT;

 		this->ballHeldCounter = 0;

-		this->ballHeld = false;

 	}

 

 protected:

 	float direction;

 	uint8 ballHeldCounter;

-	bool ballHeld;

 

 public:

 	void Update();

@@ -42,9 +39,9 @@ public:
 		return (direction != EMPTY_FLOAT);

 	}

 

-	bool RobotHasBall()

+	bool HasBall()

 	{

-		return ballHeld;

+		return (ballHeldCounter >= 3);

 	}

 };

 

diff --git a/source/Concept/Framework/modules/interpreter/command_handler.c b/source/Concept/Framework/modules/interpreter/command_handler.c
index 7d8937b..e2a9ba2 100644
--- a/source/Concept/Framework/modules/interpreter/command_handler.c
+++ b/source/Concept/Framework/modules/interpreter/command_handler.c
@@ -3,6 +3,7 @@
 //-----------------------------------------------------------------------------

 void Command_Handler::Update()

 {

+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);

 	Keyboard* ourKeyboard = parent->GetModule<Keyboard>(IO_KEYBOARD_MAIN);

 

 	uint8 curInput = ourKeyboard->GetInput();

@@ -13,11 +14,20 @@ void Command_Handler::Update()
 		if(curInput == 10)

 		{

 			ExecuteCommand();

+

+			for(uint8 i = 0; i < this->currentCommandLength; i++)

+			{

+				this->buffer[i] = NULL;

+				ourDisplay->Print(" ", i + 1, 1);

+			}

+

+			this->currentCommandLength = 0;

 		}

 		else if(curInput == 11)

 		{

 			if(this->currentCommandLength > 0)

 			{

+				ourDisplay->Print(" ", this->currentCommandLength, 1);

 				this->currentCommandLength--;

 				this->buffer[currentCommandLength] = NULL;

 			}

@@ -28,6 +38,7 @@ void Command_Handler::Update()
 			{

 				this->buffer[this->currentCommandLength] = curInput;

 				this->currentCommandLength++;

+				ourDisplay->Print(curInput, this->currentCommandLength, 1);

 			}

 		}

 

@@ -38,8 +49,163 @@ void Command_Handler::Update()
 //-----------------------------------------------------------------------------

 void Command_Handler::ExecuteCommand()

 {

-	if(this->buffer[0] == 5)

+	if(this->buffer[0] == 1)

+	{

+		if(this->buffer[1] == 1)

+		{

+			this->displayDistanceSensors = true;

+			this->displayPositionTracker = false;

+			this->displayBallTracker = false;

+			this->displayMouseSensors = false;

+		}

+		else if(this->buffer[1] == 2)

+		{

+			this->displayDistanceSensors = false;

+			this->displayPositionTracker = true;

+			this->displayBallTracker = false;

+			this->displayMouseSensors = false;

+		}

+		else if(this->buffer[1] == 3)

+		{

+			this->displayDistanceSensors = false;

+			this->displayPositionTracker = false;

+			this->displayBallTracker = true;

+			this->displayMouseSensors = false;

+		}

+		else if(this->buffer[1] == 4)

+		{

+			this->displayDistanceSensors = false;

+			this->displayPositionTracker = false;

+			this->displayBallTracker = false;

+			this->displayMouseSensors = true;

+		}

+		else if(this->buffer[1] == 0)

+		{

+			this->displayDistanceSensors = false;

+			this->displayPositionTracker = false;

+			this->displayBallTracker = false;

+			this->displayMouseSensors = false;

+		}

+	}

+	else if(this->buffer[0] == 2)

+	{

+		Logic* ourLogic = parent->GetModule<Logic>(IO_LOGIC_MAIN);	

+		if(this->buffer[1] == 1)

+		{

+			ourLogic->SetKeeper(true);

+		}

+		else if(this->buffer[1] == 2)

+		{

+			ourLogic->SetKeeper(false);

+		}

+	}

+	else if(this->buffer[0] == 3)

+	{

+		if(this->buffer[1] == 1)

+		{

+			Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

+			ourNavigator->RotateTo(180, 200);

+		}

+		else if(this->buffer[1] == 2)

+		{

+			Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);

+			ourAktuator->Kick();

+		}

+		else if(this->buffer[1] == 3)

+		{

+			Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

+			ourNavigator->RotateTo(float(buffer[2]) * 100.0f + float(buffer[3]) * 10.0f + float(buffer[4]), 200);

+		}

+		else if(this->buffer[1] == 4)

+		{

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

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

+

+			this->ticksPerCmOffset += 2.5f;

+			ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+			ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+			ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+			ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+		}

+		else if(this->buffer[1] == 5)

+		{

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

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

+

+			this->ticksPerCmOffset -= 2.5f;

+			ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+			ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+			ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+			ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));

+		}

+	}

+	else if(this->buffer[0] == 4)

+	{

+		Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

+		int16 speed = 200;

+		switch(this->buffer[1])

+		{

+			case 1:

+				ourNavigator->Drive(225, 0, speed, 0);

+				break;

+			case 2:

+				ourNavigator->Drive(180, 0, speed, 0);

+				break;

+			case 3:

+				ourNavigator->Drive(135, 0, speed, 0);

+				break;

+			case 4:

+				ourNavigator->Drive(270, 0, speed, 0);

+				break;

+			case 5:

+				ourNavigator->Rotate(DEFAULT_ROTATION_SPEED);

+				break;

+			case 6:

+				ourNavigator->Drive(90, 0, speed, 0);

+				break;

+			case 7:

+				ourNavigator->Drive(315, 0, speed, 0);

+				break;

+			case 8:

+				ourNavigator->Drive(0, 0, speed, 0);

+				break;

+			case 9:

+				ourNavigator->Drive(45, 0, speed, 0);

+				break;

+			case 0:

+				ourNavigator->Stop();

+				break;

+		}

+	}

+	else if(this->buffer[0] == 5)

+	{

+		Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

+		if(this->buffer[1] == 1)

+		{

+			ourPositionTracker->SetPosition(float(), ourPositionTracker->GetPositionY(), ourPositionTracker->GetOrientation());

+		}

+		else if(this->buffer[1] == 0)

+		{

+			ourPositionTracker->SetPosition(0, 0, 0);

+		}

+	}

+	else if(this->buffer[0] == 6)

+	{

+		if(this->buffer[1] == 1)

+		{

+			Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);

+			ourWireless->Send("Heyho!");

+		}

+	}

+}

+

+//-----------------------------------------------------------------------------

+void Command_Handler::PrintCommand()

+{

+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);

+	

+	for(uint8 i = 0; i < currentCommandLength; i++)

 	{

-		this->displayDistanceSensors = true;

+		ourDisplay->Print(this->buffer[i], i + 1, 1);

 	}

 }

diff --git a/source/Concept/Framework/modules/interpreter/command_handler.h b/source/Concept/Framework/modules/interpreter/command_handler.h
index c3bc5bb..f376e96 100644
--- a/source/Concept/Framework/modules/interpreter/command_handler.h
+++ b/source/Concept/Framework/modules/interpreter/command_handler.h
@@ -12,6 +12,10 @@ public:
 		this->moduleId = 0;

 		this->currentCommandLength = 0;

 		this->displayDistanceSensors = false;

+		this->displayPositionTracker = false;

+		this->displayBallTracker = false;

+		this->displayMouseSensors = false;

+		this->ticksPerCmOffset = 0;

 

 		for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)

 		{

@@ -25,6 +29,10 @@ public:
 		this->moduleId = commandHandlerId;

 		this->currentCommandLength = 0;

 		this->displayDistanceSensors = false;

+		this->displayPositionTracker = false;

+		this->displayBallTracker = false;

+		this->displayMouseSensors = false;

+		this->ticksPerCmOffset = 0;

 

 		for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)

 		{

@@ -39,10 +47,17 @@ protected:
 	void ExecuteCommand();

 

 public:

-	void Update();	

+	void Update();

+

+	void PrintCommand();

 

 	//Command variables

 	bool displayDistanceSensors;

+	bool displayPositionTracker;

+	bool displayBallTracker;

+	bool displayMouseSensors;

+

+	float ticksPerCmOffset;

 };

 
 #endif
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index f6d67ac..fab8085 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -3,6 +3,9 @@
 //-----------------------------------------------------------------------------

 void Position_Tracker::Update()

 {

+	Command_Handler* ourCommandHandler = parent->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);

+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);

+

 	// 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);

@@ -40,6 +43,14 @@ void Position_Tracker::Update()
 		movementRightY = 0;

 	}

 

+	if(ourCommandHandler->displayMouseSensors)

+	{

+		ourDisplay->Print(movementLeftX, 1, 2);

+		ourDisplay->Print(movementLeftY, 10, 2);

+		ourDisplay->Print(movementRightX, 1, 3);

+		ourDisplay->Print(movementRightY, 10, 3);

+	}

+

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

 	float movementDifferenceX = movementRightX - movementLeftX;

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

@@ -47,6 +58,11 @@ void Position_Tracker::Update()
 	// Calculate the difference of orientation

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

 

+	if(ourCommandHandler->displayMouseSensors)

+	{

+		ourDisplay->Print(orientationDiff * 180.0f / PI, 1, 4);

+	}

+

 	float robotMovementX = movementDifferenceX / 2.0f;

 	float robotMovementY = movementDifferenceY / 2.0f;

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

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index 56b16c4..7a5781f 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -40,18 +40,20 @@ public:
 	}

 

 	// returns x-koordinate in mm

-	int GetPositionX() 	{

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

+	int16 GetPositionX() 

+	{

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

 	}

 

 	// returns y-koordinate in mm

-	int GetPositionY() 	{

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

+	int16 GetPositionY() 	

+	{

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

 	}

 

 	// returns orientation

-	float GetOrientation()	{

-		//return 0.0f; //tmp!!!!!!!!!!

+	float GetOrientation()

+	{

 		return orientation;

 	}

 };