Code-stuff

2 files changed, 337 insertions, 0 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index ceeda60..47b1c69 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -1,3 +1,138 @@
+<<<<<<< .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"

 

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

@@ -19,7 +154,135 @@ void Navigator::Stop()
 

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

 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()
+{
+        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->direction = EMPTY_FLOAT;

 	this->targetAngle = EMPTY_FLOAT;

@@ -257,3 +520,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 56e7b8f..848c9fa 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -1,3 +1,75 @@
+<<<<<<< .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

 

@@ -68,4 +140,5 @@ public:
 	}

 };

 

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