+++ Additional Codework

source/Concept/Framework/default/Makefile

@@ -13,7 +13,7 @@ COMMON = -mmcu=$(MCU)
 
 ## Compile options common for all C compilation units.
 CFLAGS = $(COMMON)
-CFLAGS += -Wall -gdwarf-2 -O3 -fsigned-char 
+CFLAGS += -Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char 
 CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d 
 
 ## Assembly specific flags
@@ -22,7 +22,7 @@ ASMFLAGS += -x assembler-with-cpp -Wa,-gdwarf2
 
 ## Linker flags
 LDFLAGS = $(COMMON)
-LDFLAGS += 
+LDFLAGS += -lm 
 
 
 ## Intel Hex file production flags
@@ -36,6 +36,9 @@ HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0
 ## Include Directories
 INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\executor" -I"Y:\Concept\Framework\modules\input" -I"Y:\Concept\Framework\modules\interpreter" -I"Y:\Concept\Framework\modules\logic" -I"Y:\Concept\Framework\modules\output" 
 
+## Libraries
+LIBS = -lm 
+
 ## Objects that must be built in order to link
 OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o 
 

source/Concept/Framework/main.c

@@ -132,13 +132,54 @@ int main()
 	uint16 value = 0;
 	int8 value2 = 0;
 	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
+	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
 	uint32 i = 1;
-	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
+	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+	float rotation = 0;
+
+	ourDisplay->Print("Starting...", 1, 1);
 
 	//Run
 	while(true)
 	{
-		msleep(500);
+		uint8 someInput = ourKeyboard->GetInput();
+		switch(someInput)
+		{
+			case 1:
+				ourNavigator->Drive(225.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 2:
+				ourNavigator->Drive(180.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 3:
+				ourNavigator->Drive(135.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 4:
+				ourNavigator->Drive(270.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 5:
+				ourNavigator->Stop();
+				break;
+			case 6:
+				ourNavigator->Drive(90.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 7:
+				ourNavigator->Drive(315.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 8:
+				ourNavigator->Drive(0.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 9:
+				ourNavigator->Drive(45.0f * PI / 180.0f, rotation, 200, rotation);
+				break;
+			case 10:
+				rotation -= 10;
+				break;
+			case 12:
+				rotation += 10;
+				break;
+		}
+		msleep(50);/*
 		ourDisplay->Clear();
 		ourDisplay->Print(i++);
 		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
@@ -172,15 +213,11 @@ int main()
 		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
 		value = ourSensor->GetIRIntensity();
 		ourDisplay->Print(value);
-		ourDisplay->Print(";");
+		ourDisplay->Print(";");*/
-		value2 = (localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT))->GetXMovement();
-		ourDisplay->Print(value2, 10, 4);
-		value2 = (localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT))->GetYMovement();
-		ourDisplay->Print(value2, 15, 4);
 
 		localRobot->Update();
 
-		ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);
+		//ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);
 	}
 
 	//Cleanup

source/Concept/Framework/modules/executor/navigator.c

@@ -7,7 +7,7 @@ void Navigator::Stop()
 	this->targetAngle = -1.0f;
 	this->targetX = -1.0f;
 	this->targetY = -1.0f;
-	this->speed = 0;
+	this->robotSpeed = 0;
 	this->rotationSpeed = 0;
 
 	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
@@ -20,12 +20,12 @@ void Navigator::Stop()
 //-----------------------------------------------------------------------------
 void Navigator::Rotate(float rotationSpeed)
 {
-	this->rotationSpeed = rotationSpeed;
+	this->rotationSpeed = min(rotationSpeed, 255.0f);;
 	this->direction = -1.0f;
 	this->targetAngle = -1.0f;
 	this->targetX = -1.0f;
 	this->targetY = -1.0f;
-	this->speed = 0;
+	this->robotSpeed = 0;
 
 	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
 	{
@@ -34,6 +34,66 @@ void Navigator::Rotate(float rotationSpeed)
 	}
 }
 
+//-----------------------------------------------------------------------------
+void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)
+{
+	this->rotationSpeed = min(rotationSpeed, 255.0f);
+	this->direction = newDirection;
+	this->targetAngle = newAngle;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->robotSpeed = newSpeed;
+
+	if(targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI)
+	{
+		if(rotationSpeed > 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+	else
+	{
+		if(rotationSpeed < 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+
+	Update();
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
+{
+	if(newX < 0 || newY < 0) return;
+
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	this->rotationSpeed = min(rotationSpeed, 255.0f);
+	this->targetX = newX;
+	this->targetY = newY;
+	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
+	this->targetAngle = newAngle;
+	this->robotSpeed = newSpeed;
+
+	if(targetAngle - locationeer->GetOrientation() > PI)
+	{
+		if(rotationSpeed > 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+	else
+	{
+		if(rotationSpeed < 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+
+	Update();
+}
+
 //-----------------------------------------------------------------------------
 void Navigator::Update()
 {
@@ -103,97 +163,110 @@ void Navigator::Update()
 	//Calculate directional/rotational engine speed
 	if(hasDistanceToDrive)
 	{
+		float relativeDirection = this->direction - locationeer->GetOrientation();
 
-
+		if(targetX >= 0 && targetY >= 0)
-		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;
-
-		if(speed > maxRobotSpeed)
 		{
-			speed = maxRobotSpeed;
+			float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
+			relativeDirection = directionToTarget - locationeer->GetOrientation();
 		}
 
-		maxMotorSpeed = speed / (sqrt(2) / 2);
+		float xDrive = cos(relativeDirection);
+		float yDrive = sin(relativeDirection);
 
+		float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f;
+		float vBack = yDrive;
+		float vRight = (-yDrive - sqrt(3) * xDrive)/2;
 
+		float speedCorrection = 1.0f;
 
-		relAngel = direction - orientation
+		float maxEngineSpeed = 255.0f;
+		float minEngineSpeed = -255.0f;
 
-		robotSpeed = sin(45) * maxMotorSpeed
+		float maxSingleSpeed = max(max(vLeft, vBack), vRight);
-		maxMotorSpeed = robotSpeed / sin(45)
 
-		if(relAngel > 45)
+		float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 
+											vBack * (vBack + vLeft + vRight));
+
+		if(calcSpeed != 1.0f)
 		{
-			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
+			speedCorrection = 1.0f / calcSpeed;
+		}
 
-			back = speed / sin(relAngel);
+		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
+		if(maxOverallSpeed > maxEngineSpeed)
+		{
+			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
+		}
+		
+
+		vLeft = vLeft * this->robotSpeed * speedCorrection;
+		vBack = vBack * this->robotSpeed * speedCorrection;
+		vRight = vRight * this->robotSpeed * speedCorrection;
+
+		maxSingleSpeed = max(max(vLeft, vBack), vRight);
+		float minSingleSpeed = min(min(vLeft, vBack), vRight);
+
+		if(hasOrientationToAdjust)
+		{
+			if(this->rotationSpeed > 0)
+			{
+				if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)
+				{
+					vLeft += maxEngineSpeed - maxSingleSpeed;
+					vBack += maxEngineSpeed - maxSingleSpeed;
+					vRight += maxEngineSpeed - maxSingleSpeed;
 				}
 				else
 				{
-			
+					vLeft += this->rotationSpeed;
+					vBack += this->rotationSpeed;
+					vRight += this->rotationSpeed;
+				}
+			}
+			else
+			{
+				if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)
+				{
+					vLeft -= minEngineSpeed - minSingleSpeed;
+					vBack -= minEngineSpeed - minSingleSpeed;
+					vRight -= minEngineSpeed - minSingleSpeed;
+				}
+				else
+				{
+					vLeft -= this->rotationSpeed;
+					vBack -= this->rotationSpeed;
+					vRight -= this->rotationSpeed;
+				}
+			}
 		}
 
-		left = 
+		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
-		back = sin(relAngel) * speed
+		curEngine->SetSpeed(vLeft);
-
+		curEngine->SetEnabled(true);
-
+		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
-		direction = 0:
+		curEngine->SetSpeed(vBack);
-		orientation = 0:
+		curEngine->SetEnabled(true);
-		left = speed
+		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
-		right = -speed
+		curEngine->SetSpeed(vRight);
-		back = 0
+		curEngine->SetEnabled(true);
-
+		curEngine = NULL;
-		direction = 0:
-		orientation = 90:
-		left = speed
-		right = speed
-		back = (sinVcos)(45) * speed
-
-		direction = 0:
-		orientation = 45:
-		left = speed
-		right = 0
-		back = -speed
-
-		direction = 0:
-		orientation = 180:
-		left = -speed
-		right = speed
-		back = 0
-
 	}
-	else if(!hasOrientationToAdjust)
+	else if(hasOrientationToAdjust)
+	{
+		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
+		curEngine->SetSpeed(this->rotationSpeed);
+		curEngine->SetEnabled(true);
+		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
+		curEngine->SetSpeed(this->rotationSpeed);
+		curEngine->SetEnabled(true);
+		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
+		curEngine->SetSpeed(this->rotationSpeed);
+		curEngine->SetEnabled(true);
+		curEngine = NULL;
+	}	
+	else
 	{
 		Stop();
 	}
-	else
-	{
 }
-}
-
-// Aktualieren ohne Parameter
-/*void Navigator::Update() {	
-	// Richtung in x und y-Kompontente zerlegen
-	double y = cos((double)direction*0.01745); // richtung ist winkel
-	double x = sin((double)direction*0.01745);
-	
-	// Abweichung der Ausrichtung ermitteln(als winkel)
-	int w = sensor.GetAusrichtung() - ausrichtung;
-
-	// Stärke der einzelnen Motoren berechnen	
-	double v0 = (-x+sqrt(3)*y)/2;
-	double v1 = x;
-	double v2 = (-x-sqrt(3)*y)/2;
-
-	// Ausgerechnete Stärke an die Motoren übergeben
-	board.motor(0,(int)((double)v0*speed +w));
-	board.motor(1,(int)((double)v1*speed +w));
-	board.motor(2,(int)((double)v2*speed +w));
-}
-
-// Aktualieren mit allen Parametern
-void Navigator::Drive(float newDirection, float newAngle, float newSpeed) {
-	SetDirection(newDirection);
-	SetAngle(newAngle);
-	SetSpeed(newSpeed);
-	Update(); // Und anwenden
-}*/

source/Concept/Framework/modules/executor/navigator.h

@@ -1,6 +1,7 @@
 #ifndef _NAVIGATOR_H
 #define _NAVIGATOR_H
 
+//#include <math.h>
 #include "../../stdafx.h"
 
 class Navigator : public IO_Module
@@ -14,7 +15,7 @@ public:
 		this->targetAngle = -1.0f;
 		this->targetX = -1.0f;
 		this->targetY = -1.0f;
-		this->speed = 0;
+		this->robotSpeed = 0;
 		this->rotationSpeed = 0;
 	}
 
@@ -26,7 +27,7 @@ public:
 		this->targetAngle = -1.0f;
 		this->targetX = -1.0f;
 		this->targetY = -1.0f;
-		this->speed = 0;
+		this->robotSpeed = 0;
 		this->rotationSpeed = 0;
 	}
 
@@ -35,7 +36,7 @@ protected:
 	float targetAngle;
 	float targetX;
 	float targetY;
-	float speed;
+	float robotSpeed;
 	float rotationSpeed;
 
 public:
@@ -45,9 +46,14 @@ public:
 
 	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
 
-	void DriveTo(float newX, float newY, float newAngle);
+	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
 
 	void Rotate(float rotationSpeed);
+
+	void SetSpeed(float newSpeed)
+	{
+		this->robotSpeed = newSpeed;
+	}
 };
 
 #endif

source/Concept/Framework/tools.h

@@ -9,6 +9,10 @@
 	void operator delete(void* p);
 #endif
 
+#define max(a, b) (((a) > (b)) ? (a) : (b))
+#define min(a, b) (((a) < (b)) ? (a) : (b))
+#define sign(a) (((a) < 0) ? (-1) : (1))
+
 inline void sleep(int sec)
 {
   for (int s=0; s<sec; s++) {
@@ -42,4 +46,9 @@ inline float distance2d(float x1, float y1, float x2, float y2)
 	return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
 }
 
+inline float direction2d(float x1, float y1, float x2, float y2)
+{
+	return atan((y2 - y1) / (x2 - x1));
+}
+
 #endif