199 lines
4 KiB
C
Executable file
199 lines
4 KiB
C
Executable file
#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<Engine>(i))->SetSpeed(0);
|
||
(parent->GetModule<Engine>(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<Engine>(i))->SetSpeed(rotationSpeed);
|
||
(parent->GetModule<Engine>(i))->SetEnabled(true);
|
||
}
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
void Navigator::Update()
|
||
{
|
||
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(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<53>rke der einzelnen Motoren berechnen
|
||
double v0 = (-x+sqrt(3)*y)/2;
|
||
double v1 = x;
|
||
double v2 = (-x-sqrt(3)*y)/2;
|
||
|
||
// Ausgerechnete St<53>rke an die Motoren <20>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
|
||
}*/
|