neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

+++ Additional Codework

Browse source
This commit is contained in:
sicarius 2007-02-18 17:57:03 +00:00
parent 3c3c628b61
commit da9860f9a4
7 changed files with 222 additions and 94 deletions
Download patch file Download diff file Expand all files Collapse all files

231
source/Concept/Framework/modules/executor/navigator.c
View file

@ -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();
float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;
if(speed > maxRobotSpeed)
if(targetX >= 0 && targetY >= 0)
{
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;
float maxSingleSpeed = max(max(vLeft, vBack), 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);
}
robotSpeed = sin(45) * maxMotorSpeed
maxMotorSpeed = robotSpeed / sin(45)
if(relAngel > 45)
{
sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
vLeft = vLeft * this->robotSpeed * speedCorrection;
vBack = vBack * this->robotSpeed * speedCorrection;
vRight = vRight * this->robotSpeed * speedCorrection;
back = speed / sin(relAngel);
}
else
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 =
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
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(!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
}*/

14
source/Concept/Framework/modules/executor/navigator.h
View file

@ -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