diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 223 |
1 files changed, 148 insertions, 75 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 459d4c1..691b1d0 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -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++)
{
@@ -35,6 +35,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()
{
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
@@ -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
-
- robotSpeed = sin(45) * maxMotorSpeed
- maxMotorSpeed = robotSpeed / sin(45)
+ float maxEngineSpeed = 255.0f;
+ float minEngineSpeed = -255.0f;
- if(relAngel > 45)
- {
- sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
+ float maxSingleSpeed = max(max(vLeft, vBack), vRight);
- back = speed / sin(relAngel);
- }
- else
+ float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
+ vBack * (vBack + vLeft + vRight));
+
+ if(calcSpeed != 1.0f)
{
-
+ speedCorrection = 1.0f / calcSpeed;
}
- left =
- back = sin(relAngel) * speed
-
-
- direction = 0:
- orientation = 0:
- left = speed
- right = -speed
- back = 0
+ float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
+ if(maxOverallSpeed > maxEngineSpeed)
+ {
+ robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
+ }
+
- direction = 0:
- orientation = 90:
- left = speed
- right = speed
- back = (sinVcos)(45) * speed
+ vLeft = vLeft * this->robotSpeed * speedCorrection;
+ vBack = vBack * this->robotSpeed * speedCorrection;
+ vRight = vRight * this->robotSpeed * speedCorrection;
- direction = 0:
- orientation = 45:
- left = speed
- right = 0
- back = -speed
+ maxSingleSpeed = max(max(vLeft, vBack), vRight);
+ float minSingleSpeed = min(min(vLeft, vBack), vRight);
- direction = 0:
- orientation = 180:
- left = -speed
- right = speed
- back = 0
+ 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;
+ }
+ }
+ }
+ 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)
{
- Stop();
- }
+ 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();
}
}
-
-// 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
-}*/
|