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

Code-stuff

Browse source
This commit is contained in:
sicarius 2007-02-19 20:57:03 +00:00
parent b3196682b1
commit 42a38959ff
12 changed files with 474 additions and 55 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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