+++ corrected svn cripplement

2 files changed, 18 insertions, 356 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 47b1c69..a69c533 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -1,138 +1,3 @@
-<<<<<<< .mine
-#include "navigator.h"
-
-//-----------------------------------------------------------------------------
-void Navigator::Stop()
-{	
-        this->direction = 0;
-        this->targetAngle = 0;
-        //this->targetX = EMPTY_FLOAT;
-        //this->targetY = EMPTY_FLOAT;
-        this->robotSpeed = 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 newTargetAngle,float newRotationSpeed)
-{
-        this->rotationSpeed = min(newRotationSpeed, 255.0f);;
-        this->direction = 0;
-        this->targetAngle = newTargetAngle;
-        //this->targetX = EMPTY_FLOAT;
-        //this->targetY = EMPTY_FLOAT;
-        this->robotSpeed = 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::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)
-{
-        this->rotationSpeed = min(rotationSpeed, 255.0f);
-        this->direction = newDirection*PI/180;
-        this->targetAngle = newAngle;
-        this->targetX = EMPTY_FLOAT;
-        this->targetY = EMPTY_FLOAT;
-        this->robotSpeed = newSpeed;
-
-        if(targetAngle == EMPTY_FLOAT)
-        {
-                rotationSpeed = 0;
-        }
-        else
-        {
-                rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
-        }
-
-        CalculateEngines();
-}
-
-//-----------------------------------------------------------------------------
-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;
-                }
-        }
-
-        CalculateDirection();
-}
-
-//-----------------------------------------------------------------------------
-void Navigator::Update()
-{
-        Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
-
-        bool targetReached = false;
-        bool targetAngleReached = false;
-
-        if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
-        {
-                targetX = EMPTY_FLOAT;
-                targetY = EMPTY_FLOAT;
-                direction = EMPTY_FLOAT;
-                robotSpeed = 0;
-
-                targetReached = true;
-        }
-
-        if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
-        {
-                targetAngle = EMPTY_FLOAT;
-                rotationSpeed = 0;
-
-                targetAngleReached = true;
-        }
-
-        if(targetReached && targetAngleReached)
-        {
-                Stop();
-        }
-        else if(targetReached || targetAngleReached)
-        {
-                CalculateDirection();
-        }
-
-        if(!(correctionCountdown--))
-        {
-                CalculateDirection();
-        }
-}
-
-//-----------------------------------------------------------------------------
-void Navigator::CalculateDirection()
-=======
 #include "navigator.h"

 

 //-----------------------------------------------------------------------------

@@ -150,140 +15,12 @@ void Navigator::Stop()
 		(parent->GetModule<Engine>(i))->SetSpeed(0);

 		(parent->GetModule<Engine>(i))->SetEnabled(true);

 	}

-}

-

-//-----------------------------------------------------------------------------

-void Navigator::Rotate(float rotationSpeed)

->>>>>>> .r199
-{

-<<<<<<< .mine
-				Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
-        correctionCountdown = CYCLES_PER_CORRECTION;
-
-        if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;
-
-        if(HasTarget())
-        {
-                direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
-        }
-
-        if(targetAngle != EMPTY_FLOAT)
-        {
-                rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
-        }
-
-        CalculateEngines();
 }
 
-//-----------------------------------------------------------------------------
-void Navigator::CalculateEngines()
+//-----------------------------------------------------------------------------

+void Navigator::Rotate(float rotationSpeed)
 {
-        Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
-        if(direction != EMPTY_FLOAT)
-        {
-                float relativeDirection = this->direction - locationeer->GetOrientation();
-
-                float xDrive = cos(relativeDirection + PI / 6.0f);
-                float yDrive = sin(relativeDirection + PI / 6.0f);
-
-                float vLeft = xDrive;
-                float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
-                float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
-
-                float speedCorrection = 1.0f;
-
-                float maxEngineSpeed = 255.0f;
-                float minEngineSpeed = -255.0f;
-
-                float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
-
-                float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 
-                                                                                        vBack * (vBack + vLeft + vRight));
-
-                if(calcSpeed != 1.0f)
-                {
-                        speedCorrection = 1.0f / calcSpeed;
-                }
-
-                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(fabs(vLeft), fabs(vBack)), fabs(vRight));
-                float minSingleSpeed = min(min(vLeft, vBack), vRight);
-
-                if(rotationSpeed)
-                {
-                        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;
-                                }
-                        }
-                }
-
-                Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
-                curEngine->SetSpeed(vLeft);
-                curEngine->SetEnabled(true);
-                curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
-                curEngine->SetSpeed(vBack);
-                curEngine->SetEnabled(true);
-                curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
-                curEngine->SetSpeed(vRight);
-                curEngine->SetEnabled(true);
-                curEngine = NULL;
-        }
-        else if(rotationSpeed)
-        {
-                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();
-        }
-}
-=======
-	this->rotationSpeed = min(rotationSpeed, 255.0f);;

+	this->rotationSpeed = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed);

 	this->direction = EMPTY_FLOAT;

 	this->targetAngle = EMPTY_FLOAT;

 	this->targetX = EMPTY_FLOAT;

@@ -295,14 +32,14 @@ void Navigator::CalculateEngines()
 		(parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);

 		(parent->GetModule<Engine>(i))->SetEnabled(true);

 	}

-}

-

+}
+
 //-----------------------------------------------------------------------------

 void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)

 {

 	this->rotationSpeed = min(rotationSpeed, 255.0f);

-	this->direction = newDirection;

-	this->targetAngle = newAngle;

+	this->direction = newDirection * PI / 180.0f;

+	this->targetAngle = newAngle * PI / 180.0f;

 	this->targetX = EMPTY_FLOAT;

 	this->targetY = EMPTY_FLOAT;

 	this->robotSpeed = newSpeed;

@@ -317,8 +54,8 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
 	}

 

 	CalculateEngines();

-}

-

+}
+
 //-----------------------------------------------------------------------------

 void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)

 {

@@ -330,7 +67,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	this->targetX = newX;

 	this->targetY = newY;

 	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;

-	this->targetAngle = newAngle;

+	this->targetAngle = newAngle * PI / 180.0f;

 	this->robotSpeed = newSpeed;

 

 	if(targetAngle - locationeer->GetOrientation() > PI)

@@ -349,8 +86,8 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	}

 

 	CalculateDirection();

-}

-

+}
+
 //-----------------------------------------------------------------------------

 void Navigator::Update()

 {

@@ -390,8 +127,8 @@ void Navigator::Update()
 	{

 		CalculateDirection();

 	}

-}

-

+}
+
 //-----------------------------------------------------------------------------

 void Navigator::CalculateDirection()

 {

@@ -412,8 +149,8 @@ void Navigator::CalculateDirection()
 	}

 

 	CalculateEngines();

-}

-

+}
+
 //-----------------------------------------------------------------------------

 void Navigator::CalculateEngines()

 {

@@ -519,5 +256,4 @@ void Navigator::CalculateEngines()
 	{

 		Stop();

 	}

-}

->>>>>>> .r199
+}
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 848c9fa..81919f9 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -1,79 +1,6 @@
-<<<<<<< .mine
-#ifndef _NAVIGATOR_H
-#define _NAVIGATOR_H
-
-//#include <math.h>
-#include "../../stdafx.h"
-
-class Navigator : public IO_Module
-{
-public:
-        Navigator()
-        {
-                this->parent = NULL;
-                this->moduleId = 0;
-                this->correctionCountdown = CYCLES_PER_CORRECTION;
-                this->direction = EMPTY_FLOAT;
-                this->targetAngle = EMPTY_FLOAT;
-                this->targetX = EMPTY_FLOAT;
-                this->targetY = EMPTY_FLOAT;
-                this->robotSpeed = 0;
-                this->rotationSpeed = 0;
-        }
-
-        Navigator(uint32 navigatorId)
-        {
-                this->parent = NULL;
-                this->moduleId = navigatorId;
-                this->correctionCountdown = CYCLES_PER_CORRECTION;
-                this->direction = EMPTY_FLOAT;
-                this->targetAngle = EMPTY_FLOAT;
-                this->targetX = EMPTY_FLOAT;
-                this->targetY = EMPTY_FLOAT;
-                this->robotSpeed = 0;
-                this->rotationSpeed = 0;
-        }
-
-protected:
-        uint16 correctionCountdown;
-        float direction; 
-        float targetAngle;
-        float targetX;
-        float targetY;
-        float robotSpeed;
-        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, float newSpeed, float rotationSpeed);
-
-        void Rotate(float rotationSpeed);
-
-        void SetSpeed(float newSpeed)
-        {
-                this->robotSpeed = newSpeed;
-        }
-
-        void CalculateDirection();
-
-        void CalculateEngines();
-
-        bool HasTarget()
-        {
-                return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
-        }
-};
-
-=======
 #ifndef _NAVIGATOR_H

 #define _NAVIGATOR_H

 

-//#include <math.h>

 #include "../../stdafx.h"

 

 class Navigator : public IO_Module

@@ -139,6 +66,5 @@ public:
 		return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);

 	}

 };

-

->>>>>>> .r199
+
 #endif