+++ Additional Codework

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

@@ -0,0 +1,199 @@
+#include "navigator.h"
+
+//-----------------------------------------------------------------------------
+void Navigator::Stop()
+{	
+	this->direction = -1.0f;
+	this->targetAngle = -1.0f;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->speed = 0;
+	this->rotationSpeed = 0;
+
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		(parent->GetModule<Engine>(i))->SetSpeed(0);
+		(parent->GetModule<Engine>(i))->SetEnabled(true);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Rotate(float rotationSpeed)
+{
+	this->rotationSpeed = rotationSpeed;
+	this->direction = -1.0f;
+	this->targetAngle = -1.0f;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->speed = 0;
+
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		(parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);
+		(parent->GetModule<Engine>(i))->SetEnabled(true);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	bool hasDistanceToDrive = true;
+	bool hasOrientationToAdjust = true;
+
+	//Check for distance to drive
+	if((targetX >= 0) != (targetY >= 0))
+	{
+		targetX = -1.0f;
+		targetY = -1.0f;
+
+		hasDistanceToDrive = false;
+	}
+	else if(targetX >= 0 && targetY >= 0)
+	{
+		if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+		{
+			targetX = -1.0f;
+			targetY = -1.0f;
+
+			hasDistanceToDrive = false;
+		}
+		else
+		{
+			hasDistanceToDrive = true;
+		}
+	}
+	else
+	{
+		if(direction >= 0)
+		{
+			hasDistanceToDrive = true;
+		}
+		else
+		{
+			hasDistanceToDrive = false;
+		}
+	}
+
+	//Check for orientation to adjust
+	if(targetAngle >= 0)
+	{
+		if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
+		{
+			hasOrientationToAdjust = false;
+		}
+		else
+		{
+			hasOrientationToAdjust = true;
+		}
+	}
+	else
+	{
+		if(rotationSpeed != 0)
+		{
+			hasOrientationToAdjust = true;
+		}
+		else
+		{
+			hasOrientationToAdjust = false;
+		}
+	}
+
+	//Calculate directional/rotational engine speed
+	if(hasDistanceToDrive)
+	{
+
+
+		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;
+
+		if(speed > maxRobotSpeed)
+		{
+			speed = maxRobotSpeed;
+		}
+
+		maxMotorSpeed = speed / (sqrt(2) / 2);
+
+
+
+		relAngel = direction - orientation
+		
+		robotSpeed = sin(45) * maxMotorSpeed
+		maxMotorSpeed = robotSpeed / sin(45)
+
+		if(relAngel > 45)
+		{
+			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
+
+			back = speed / sin(relAngel);
+		}
+		else
+		{
+			
+		}
+
+		left = 
+		back = sin(relAngel) * speed
+
+
+		direction = 0:
+		orientation = 0:
+		left = speed
+		right = -speed
+		back = 0
+
+		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)
+	{
+		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

@@ -0,0 +1,53 @@
+#ifndef _NAVIGATOR_H
+#define _NAVIGATOR_H
+
+#include "../../stdafx.h"
+
+class Navigator : public IO_Module
+{
+public:
+	Navigator()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->direction = -1.0f;
+		this->targetAngle = -1.0f;
+		this->targetX = -1.0f;
+		this->targetY = -1.0f;
+		this->speed = 0;
+		this->rotationSpeed = 0;
+	}
+
+	Navigator(uint32 navigatorId)
+	{
+		this->parent = NULL;
+		this->moduleId = navigatorId;
+		this->direction = -1.0f;
+		this->targetAngle = -1.0f;
+		this->targetX = -1.0f;
+		this->targetY = -1.0f;
+		this->speed = 0;
+		this->rotationSpeed = 0;
+	}
+
+protected:
+	float direction; 
+	float targetAngle;
+	float targetX;
+	float targetY;
+	float speed;
+	float rotationSpeed;
+
+public:
+	void Update();	
+
+	void Stop();
+
+	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
+
+	void DriveTo(float newX, float newY, float newAngle);
+
+	void Rotate(float rotationSpeed);
+};
+
+#endif