523 lines
16 KiB
C
Executable file
523 lines
16 KiB
C
Executable file
<<<<<<< .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"
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Navigator::Stop()
|
|
{
|
|
this->direction = EMPTY_FLOAT;
|
|
this->targetAngle = EMPTY_FLOAT;
|
|
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 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;
|
|
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;
|
|
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()
|
|
{
|
|
correctionCountdown = CYCLES_PER_CORRECTION;
|
|
|
|
if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;
|
|
|
|
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
|
|
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()
|
|
{
|
|
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();
|
|
}
|
|
}
|
|
>>>>>>> .r199
|