From 3c3c628b617dc53f0b7b59285c7d67888074c33d Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 18 Feb 2007 00:14:00 +0000 Subject: +++ Additional Codework --- .../Concept/Framework/modules/executor/navigator.c | 199 +++++++++++++++++++++ .../Concept/Framework/modules/executor/navigator.h | 53 ++++++ 2 files changed, 252 insertions(+) create mode 100755 source/Concept/Framework/modules/executor/navigator.c create mode 100755 source/Concept/Framework/modules/executor/navigator.h (limited to 'source/Concept/Framework/modules/executor') diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c new file mode 100755 index 0000000..459d4c1 --- /dev/null +++ b/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(i))->SetSpeed(0); + (parent->GetModule(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(i))->SetSpeed(rotationSpeed); + (parent->GetModule(i))->SetEnabled(true); + } +} + +//----------------------------------------------------------------------------- +void Navigator::Update() +{ + Position_Tracker* locationeer = parent->GetModule(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 +}*/ diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h new file mode 100755 index 0000000..ed8b3c8 --- /dev/null +++ b/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 -- cgit v1.2.3