Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005SimpleBasicBehaviors.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005SimpleBasicBehaviors.cpp
    00003 *
    00004 * Implementation of basic behaviors defined in simple-basic-behaviors.xml.
    00005 *
    00006 * @author Uwe Düffert
    00007 * @author Jan Hoffmann
    00008 * @author Matthias Jüngel
    00009 * @author Martin Lötzsch
    00010 * @author Max Risler
    00011 * @author Tim Laue
    00012 */
    00013
    00014 #include "GT2005SimpleBasicBehaviors.h"
    00015 #include "Tools/Math/PIDsmoothedValue.h"
    00016 #include "Tools/Math/Geometry.h"
    00017 #include "Tools/Math/Matrix2x2.h"
    00018 #include "Tools/Math/Common.h"
    00019 #include "Representations/Perception/SlamPercept.h"
    00020
    00021 #include "Platform/GTAssert.h"
    00022
    00023 #define slamPercept (*(SlamPercept *)specialPercept.slamData)
    00024
    00025 #ifdef APERIOS1_3_2
    00026   #include <ERA201D1.h>
    00027 #endif
    00028
    00029 void GT2005BasicBehaviorNewGoToBall::execute()
    00030 {
    00031   if (maxSpeed == 0) maxSpeed = 600;
    00032   if (maxSpeedY == 0) maxSpeedY = 600;
    00033
    00034   double maxTurnSpeed;
    00035   if (this->maxTurnSpeed == 0)
    00036     maxTurnSpeed = fromDegrees(180);
    00037   else
    00038     maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
    00039
    00040   Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    00041     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
    00042   double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
    00043   double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
    00044
    00045   double factor;
    00046   // if distance is between 400 and 600,
    00047   // perform linear interpolation
    00048   // factor = 1 - (distanceToBall - 400) / 200;
    00049   // if (factor > 1) factor = 1;
    00050   // if (factor < 0) factor = 0;
    00051
    00052   //targetAngle *= factor;
    00053   //angleToBall += targetAngle;
    00054
    00055   // destination = ball position in robot
    00056   // coordintes plus 200mm in x direction
    00057   Vector2<double> destination(
    00058     distanceToBall * cos(angleToBall) + 400, // ?? shouldnt be 200?
    00059     distanceToBall * sin(angleToBall) + yOffset);
    00060
    00061   double factorClipping = 1.0; // full speed
    00062
    00063   if (distanceToBall < slowDownDistance + 50)
    00064   {
    00065   factorClipping = 0.9; // smoother approach
    00066   }
    00067   // adjust forward speed:
    00068   // if a lot of turning is necessary, don't walk so fast!
    00069   factor = (pi-fabs(angleToBall))/pi;
    00070   if (factor > factorClipping) factor = factorClipping;
    00071   if (factor < 0) factor = 0;
    00072
    00073   destination.normalize();
    00074
    00075   if (distanceToBall < slowDownDistance)
    00076   {
    00077     destination *= maxSpeed*((distanceToBall/2)/slowDownDistance);
    00078   }
    00079   else
    00080     destination *= (maxSpeed*factor);
    00081
    00082   /*
    00083   factor = .2 + distanceToBall / 500;
    00084   if (factor > 1) factor = 1;
    00085   if (factor < 0) factor = 0;
    00086
    00087     destination *= factor;
    00088   */
    00089
    00090   motionRequest.motionType = MotionRequest::walk;
    00091   motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    00092   motionRequest.walkRequest.walkParams.translation.x = destination.x;
    00093   motionRequest.walkRequest.walkParams.translation.y = destination.y;
    00094
    00095   if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    00096     motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
    00097
    00098   if(angleFactor == 0)
    00099     angleFactor = 1.0;
    00100
    00101   if(distanceToBall > 300)
    00102     factor = angleFactor;
    00103   else
    00104     factor = angleFactor + 0.5;
    00105   /*
    00106   factor = .2 + 1.5 * distanceToBall / 800;
    00107   if (factor > 1.5) factor = 1.5;
    00108   if (factor < 0) factor = 0;
    00109   */
    00110   motionRequest.walkRequest.walkParams.rotation = angleToBall * factor; /* this should also be factored!!!! */
    00111
    00112   // clip rotation speed
    00113   motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
    00114   motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);
    00115 }
    00116
    00117 void GT2005BasicBehaviorGoToBall::execute()
    00118 {
    00119   //accelerationRestrictor.saveLastWalkParameters();
    00120   double targetAngle;
    00121
    00122   if (maxSpeed == 0) maxSpeed = 350;
    00123   if (maxSpeedY == 0) maxSpeed = 350;
    00124   if (slowDownDistance == 0) slowDownDistance = 400;
    00125   if (targetAngleToBall == 0)
    00126     targetAngle = 0;
    00127   else
    00128     targetAngle = targetAngleToBall;
    00129
    00130
    00131   double maxTurnSpeed;
    00132   if (this->maxTurnSpeed == 0)
    00133     maxTurnSpeed = fromDegrees(180);
    00134   else
    00135     maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
    00136
    00137   Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    00138     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
    00139   double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
    00140   double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
    00141
    00142   // if distance is between 400 and 600,
    00143   // perform linear interpolation
    00144
    00145
    00146   //double factor = 1 - (distanceToBall - 400) / 200; // unused
    00147   //if (factor > 1) factor = 1; // unused
    00148   //if (factor < 0) factor = 0; // unused
    00149
    00150   //targetAngle *= factor;
    00151   //angleToBall += targetAngle;
    00152
    00153   // destination = ball position in robot
    00154   // coordintes plus 200mm in x direction
    00155   Vector2<double> destination(
    00156     distanceToBall * cos(angleToBall) + 400,
    00157     distanceToBall * sin(angleToBall) + targetAngleToBall + yOffset);
    00158
    00159   double factorClipping = 1.0; // full speed
    00160
    00161   if (distanceToBall < slowDownDistance + 50)
    00162   {
    00163   factorClipping = 0.9; // smoother approach
    00164   }
    00165
    00166   // adjust forward speed:
    00167   // if a lot of turning is necessary, don't walk so fast!
    00168   double factor = (pi-fabs(angleToBall))/pi;
    00169   if (factor > factorClipping) factor = factorClipping;
    00170   if (factor < 0) factor = 0;
    00171
    00172   destination.normalize();
    00173   destination *= (maxSpeed*factor);
    00174
    00175   /*
    00176   factor = .2 + distanceToBall / 500;
    00177   if (factor > 1) factor = 1;
    00178   if (factor < 0) factor = 0;
    00179
    00180     destination *= factor;
    00181   */
    00182
    00183   motionRequest.motionType = MotionRequest::walk;
    00184   motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    00185   motionRequest.walkRequest.walkParams.translation.x = destination.x;
    00186   motionRequest.walkRequest.walkParams.translation.y = destination.y;
    00187
    00188   if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    00189     motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
    00190
    00191   factor = 1.5;
    00192   /*
    00193   factor = .2 + 1.5 * distanceToBall / 800;
    00194   if (factor > 1.5) factor = 1.5;
    00195   if (factor < 0) factor = 0;
    00196   */
    00197   motionRequest.walkRequest.walkParams.rotation = angleToBall * factor; /* this should also be factored!!!! */
    00198
    00199   // clip rotation speed
    00200   motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
    00201   motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);
    00202 }
    00203
    00204 void GT2005BasicBehaviorGoToBallPropagated::execute()
    00205 {
    00206   //accelerationRestrictor.saveLastWalkParameters();
    00207
    00208   double targetAngle;
    00209
    00210   if (maxSpeed == 0) maxSpeed = 450;
    00211   if (maxSpeedY == 0) maxSpeed = 450;
    00212   if (targetAngleToBall == 0)
    00213     targetAngle = 0;
    00214   else
    00215     targetAngle = targetAngleToBall;
    00216
    00217
    00218   double maxTurnSpeed;
    00219   if (this->maxTurnSpeed == 0)
    00220     maxTurnSpeed = fromDegrees(180);
    00221   else
    00222     maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
    00223
    00224   Vector2<double> ballInWorldCoord = ballModel.propagated.positionField;
    00225
    00226   double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
    00227   double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
    00228
    00229
    00230   // if distance is between 400 and 600,
    00231   // perform linear interpolation
    00232   double factor = 1 - (distanceToBall - 400) / 200;
    00233   if (factor > 1) factor = 1;
    00234   if (factor < 0) factor = 0;
    00235
    00236   //targetAngle *= factor;
    00237   //angleToBall += targetAngle;
    00238
    00239   // destination = ball position in robot
    00240   // coordintes plus 200mm in x direction
    00241   Vector2<double> destination(
    00242     distanceToBall * cos(angleToBall) + 400,
    00243     distanceToBall * sin(angleToBall) + targetAngleToBall + yOffset);
    00244
    00245   // adjust forward speed:
    00246   // if a lot of turning is necessary, don't walk so fast!
    00247   factor = (pi-fabs(angleToBall))/pi;
    00248   if (factor > 1) factor = 1;
    00249   if (factor < 0) factor = 0;
    00250   factor = 1;
    00251
    00252   destination.normalize();
    00253   destination *= (maxSpeed*factor);
    00254
    00255   /*
    00256   factor = .2 + distanceToBall / 500;
    00257   if (factor > 1) factor = 1;
    00258   if (factor < 0) factor = 0;
    00259
    00260     destination *= factor;
    00261   */
    00262
    00263   motionRequest.motionType = MotionRequest::walk;
    00264   motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    00265   motionRequest.walkRequest.walkParams.translation.x = destination.x;
    00266   motionRequest.walkRequest.walkParams.translation.y = destination.y;
    00267
    00268   if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    00269     motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
    00270   if (motionRequest.walkRequest.walkParams.translation.y < -maxSpeedY)
    00271     motionRequest.walkRequest.walkParams.translation.y = -maxSpeedY;
    00272
    00273   factor = 1.5;
    00274   /*
    00275   factor = .2 + 1.5 * distanceToBall / 800;
    00276   if (factor > 1.5) factor = 1.5;
    00277   if (factor < 0) factor = 0;
    00278   */
    00279   motionRequest.walkRequest.walkParams.rotation = angleToBall * factor; /* this should also be factored!!!! */
    00280
    00281   // clip rotation speed
    00282   motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
    00283   motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);
    00284 }
    00285
    00286
    00287 void GT2005BasicBehaviorGoToBallWithoutTurning::execute()
    00288 {
    00289   if (maxSpeed == 0) maxSpeed = 250;
    00290
    00291   Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    00292     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
    00293   double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
    00294   double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
    00295
    00296   Vector2<double> destination(
    00297     distanceToBall * cos(angleToBall),
    00298     distanceToBall * sin(angleToBall) );
    00299
    00300   destination.normalize();
    00301   destination *= maxSpeed;
    00302
    00303   motionRequest.motionType = MotionRequest::walk;
    00304   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00305   motionRequest.walkRequest.walkParams.translation.x = destination.x;
    00306   motionRequest.walkRequest.walkParams.translation.y = destination.y;
    00307
    00308   motionRequest.walkRequest.walkParams.rotation = 0;
    00309 }
    00310
    00311
    00312 void GT2005BasicBehaviorGoaliePosition::execute()
    00313 {
    00314
    00315   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine) {
    00316     goaliePose = lastRobotPose = robotPose;
    00317     lastOdometry = odometryData;
    00318   }
    00319
    00320   // optional parameter initialising
    00321   if (maxSpeed == 0 )
    00322     maxSpeed = 150;
    00323
    00324   //Aldi
    00325   if (cutY == 0)
    00326     cutY = 300; // max distance from middle of goal
    00327   if (guardDirectToGoal == 0)
    00328     guardDirectToGoal = 200; // if Ball.x distance to goal < this, goalie goes back to groundline
    00329
    00330   if (guardLine == 0)
    00331     guardLine = -150;
    00332
    00333   // begin improved localization
    00334   // use *goaliePose* instead of robotPose
    00335
    00336   Pose2D diffPose;
    00337   diffPose.translation = robotPose.translation - lastGoaliePose.translation;
    00338   diffPose.rotation = (robotPose.rotation - lastGoaliePose.rotation);
    00339
    00340   Pose2D diffOdo = odometryData - lastOdometry;
    00341   Vector2<double> diffTranslation = (diffPose.translation*weightPose + diffOdo.translation*weightOdo);
    00342
    00343   goaliePose.translation += diffTranslation;
    00344   goaliePose.rotation += (diffPose.rotation*weightPose) + (diffOdo.rotation*weightOdo);
    00345
    00346   CIRCLE(goaliePositionField, goaliePose.translation.x, goaliePose.translation.y, 200, 6, 0, Drawings::blue);
    00347   CIRCLE(goaliePositionField, robotPose.translation.x, robotPose.translation.y, 180, 6, 0, Drawings::yellow);
    00348   //CIRCLE(goaliePositionField, diffPose.translation.x, diffPose.translation.y, 140, 6, 0, Drawings::blue);
    00349   //CIRCLE(goaliePositionField, diffOdo.translation.x, diffOdo.translation.y, 140, 6, 0, Drawings::red);
    00350   //CIRCLE(goaliePositionField, diffTranslation.x, diffTranslation.y, 140, 6, 0, Drawings::yellow);
    00351
    00352   // end improved localization
    00353
    00354   // begin calculating guardPosition
    00355   Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    00356     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
    00357   double angleToBall = Geometry::angleTo(goaliePose,ballInWorldCoord);
    00358
    00359   Vector2<double> guardPosition; //(Range<double>(-3000,3000),Range<double>(-200,200));
    00360
    00361   // x-pos: Goalie stays *guardDirectToGoal* mm before goalline, except the half balldistance.x is shorter
    00362   guardPosition.x = min(xPosOwnGroundline + guardDirectToGoal, xPosOwnGroundline - (xPosOwnGroundline-ballInWorldCoord.x)/2);
    00363
    00364
    00365   //guardPosition.y = ballInWorldCoord.y / 2;
    00366
    00367     //line from xPosOwnGroundline+guardLine
    00368   double deltaX = ballInWorldCoord.x - (xPosOwnGroundline+guardLine);
    00369   double m = 0;
    00370   if (deltaX > 0)
    00371     m = ballInWorldCoord.y / deltaX;
    00372   double n = ballInWorldCoord.y - m*ballInWorldCoord.x;
    00373
    00374   guardPosition.y = m*goaliePose.translation.x + n;
    00375   ARROW(goaliePositionField, xPosOwnGroundline+guardLine,0,ballInWorldCoord.x,ballInWorldCoord.y,6,0,Drawings::gray);
    00376   Vector2<double> pointToGuard = ballInWorldCoord - Vector2<double>(xPosOwnGroundline,0);
    00377
    00378   if (fabs(guardPosition.y) > cutY){
    00379     guardPosition.y = min(cutY, guardPosition.y);
    00380     guardPosition.y = max(-cutY, guardPosition.y);
    00381     LINE(goaliePositionField,xPosOwnGroundline, guardPosition.y,-xPosOwnGroundline,guardPosition.y,6,0,Drawings::red);
    00382   }
    00383
    00384   if (xPosOwnGroundline-ballInWorldCoord.x > -guardDirectToGoal){
    00385     guardPosition.x = xPosOwnGroundline+67;
    00386     LINE (goaliePositionField,xPosOwnGroundline, 1000,xPosOwnGroundline+67, -1000,6,0,Drawings::red);
    00387   }
    00388
    00389   //guardPosition.normalize();
    00390
    00391   CIRCLE(goaliePositionField, guardPosition.x, guardPosition.y, 150, 3, 0, Drawings::red);
    00392   // end calculation guardPosition
    00393
    00394   // begin calculation direction
    00395   Vector2<double> direction = guardPosition - goaliePose.translation;
    00396   Vector2<double> c1(cos(goaliePose.rotation), -sin(goaliePose.rotation)),
    00397     c2(sin(goaliePose.rotation), cos(goaliePose.rotation));
    00398   Matrix2x2<double> rotate(c1, c2);
    00399
    00400   direction = rotate*direction;
    00401   if (direction.abs() > maxSpeed)
    00402   {
    00403     direction.normalize();
    00404     direction *= maxSpeed;
    00405   }
    00406
    00407   double rotation = angleToBall;
    00408   //end calculation direction
    00409
    00410   // begin generation motionRequest with clipping
    00411   // anti-zucking
    00412   if( (fabs(rotation) < 0.2) && (abs(int(direction.x)) < minXTrans) && (abs(int(direction.y)) < minYTrans)){
    00413     motionRequest.motionType = MotionRequest::stand;
    00414   //  goaliePose = lastGoaliePose;
    00415   }
    00416   else
    00417   {
    00418     motionRequest.motionType = MotionRequest::walk;
    00419     motionRequest.walkRequest.walkType = WalkRequest::normal;
    00420     if (!((abs(int(direction.x)) < minXTrans) && (abs(int(direction.y)) < minYTrans))){
    00421       motionRequest.walkRequest.walkParams.translation.x = direction.x;
    00422       motionRequest.walkRequest.walkParams.translation.y = direction.y;
    00423     }
    00424     if(!(fabs(rotation) < 0.2))
    00425       motionRequest.walkRequest.walkParams.rotation = rotation;
    00426   }
    00427   lastGoaliePose = goaliePose;
    00428   lastOdometry = odometryData;
    00429   lastRobotPose = robotPose;
    00430   //end generation motion request
    00431   DEBUG_DRAWING_FINISHED(goaliePositionField);
    00432
    00433 }
    00434
    00435
    00436 void GT2005BasicBehaviorGoaliePositionReturn::execute()
    00437 {
    00438   Vector2<double> destination(x,y);
    00439   const Vector2<double>& self = robotPose.translation;
    00440
    00441   double distanceToDestination = Geometry::distanceTo(self,destination);
    00442
    00443   double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
    00444
    00445   if ((fabs(toDegrees(angleDifference))<25)&&(distanceToDestination<90))
    00446   {
    00447     motionRequest.motionType = MotionRequest::stand;
    00448   }
    00449   else
    00450   {
    00451     accelerationRestrictor.saveLastWalkParameters();
    00452
    00453     if (maxSpeed==0) maxSpeed = 200;
    00454
    00455     double maxTurnSpeed = fromDegrees(90);
    00456
    00457     double angleToDestination = Geometry::angleTo(robotPose,destination);
    00458
    00459     motionRequest.motionType = MotionRequest::walk;
    00460     motionRequest.walkRequest.walkType = WalkRequest::normal;
    00461
    00462     //this time does not necessarily include time for turning!:
    00463
    00464     if (distanceToDestination > 200)
    00465     {
    00466       motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    00467       motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
    00468     }
    00469     else
    00470     {
    00471       motionRequest.walkRequest.walkParams.translation.x =
    00472         cos(angleToDestination) * maxSpeed*distanceToDestination/200;
    00473       motionRequest.walkRequest.walkParams.translation.y =
    00474         sin(angleToDestination) * maxSpeed*distanceToDestination/200;
    00475     }
    00476
    00477
    00478     motionRequest.walkRequest.walkParams.rotation = angleDifference * 2;
    00479
    00480     if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed)
    00481       motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
    00482
    00483     if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
    00484       motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;
    00485
    00486
    00487
    00488     accelerationRestrictor.restrictAccelerations(150,150,100);
    00489   }
    00490 }
    00491
    00492
    00493 void GT2005BasicBehaviorGoalieReturnToGoal::execute()
    00494 {
    00495   double angleToTurn;
    00496   if(SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 3000)
    00497     //If the ball has been seen: Face the ball.
    00498     angleToTurn = Geometry::angleTo(robotPose, ballModel.getKnownPosition(5000));
    00499   else
    00500     //Otherwise turn forward to the field:
    00501     angleToTurn = -robotPose.rotation;
    00502
    00503   // The destination to walk to:
    00504   Vector2<double> destination(x,y);
    00505   double angleToDestination(Geometry::angleTo(robotPose,destination));
    00506   double distanceToDestination(Geometry::distanceTo(robotPose.translation,destination));
    00507
    00508   // If this Basic Behavior is used in conjunction with GoaliePosition, this
    00509   // condition will never be true. Anyway:
    00510   if ((fabs(toDegrees(angleToTurn))<25)&&(distanceToDestination<90))
    00511   {
    00512     motionRequest.motionType = MotionRequest::stand;
    00513   }
    00514   else
    00515   {
    00516     accelerationRestrictor.saveLastWalkParameters();
    00517     if (maxSpeed==0) maxSpeed = 250;
    00518
    00519     // The robot has two goals: The destination and a certain distance to the groundline
    00520     // (to avoid collisions with the goal)
    00521     Vector2<double> vecToDestination(cos(angleToDestination), sin(angleToDestination));
    00522     Vector2<double> vecFromGroundLine(cos(-robotPose.rotation), sin(-robotPose.rotation));
    00523     double importanceToAvoidGroundLine(0.0);
    00524     if(robotPose.translation.x <= xPosOwnGroundline + 50)
    00525       importanceToAvoidGroundLine = 1.0;
    00526     else if(robotPose.translation.x <= xPosOwnGroundline+300)
    00527       importanceToAvoidGroundLine = 1.0 - (fabs(double(xPosOwnGroundline - robotPose.translation.x + 50)) / 250.0);
    00528     motionRequest.walkRequest.walkParams.translation = ((vecToDestination * (1 - importanceToAvoidGroundLine)) +
    00529                                                         (vecFromGroundLine * importanceToAvoidGroundLine)) * maxSpeed;
    00530
    00531     //Slow down near destination (should not be true, if used in conjunction with GoaliePosition):
    00532     if(distanceToDestination <= 200)
    00533       motionRequest.walkRequest.walkParams.translation /= (distanceToDestination / 200.0);
    00534
    00535     //Compute rotation:
    00536     double maxTurnSpeed(fromDegrees(120));
    00537     motionRequest.walkRequest.walkParams.rotation = angleToTurn * 2.0;
    00538     if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed)
    00539       motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
    00540     else if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
    00541       motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;
    00542
    00543     motionRequest.motionType = MotionRequest::walk;
    00544     motionRequest.walkRequest.walkType = WalkRequest::normal;
    00545     accelerationRestrictor.restrictAccelerations(250,250,120);
    00546   }
    00547 }
    00548
    00549
    00550 void GT2005BasicBehaviorTurnAroundPoint::execute()
    00551 {
    00552   double maxTurnSpeed = fromDegrees(240);
    00553   if (forwardComponent == 0) forwardComponent = 200.0;
    00554
    00555   Vector2<double> destinationInWorldCoord(x,y);
    00556   double angleToDestination = Geometry::angleTo(robotPose.getPose(), destinationInWorldCoord);
    00557   double distance = Geometry::distanceTo(robotPose.getPose(), destinationInWorldCoord);
    00558
    00559   // destination = ball position in robot
    00560   // coordintes plus 100mm in x direction
    00561   Vector2<double> destination(
    00562     distance * cos(angleToDestination),
    00563     distance * sin(angleToDestination));
    00564
    00565   double factor =
    00566     Range<double>::Range(0, 1).
    00567       limit((distance - radius) / (3*radius));
    00568
    00569   destination *= factor;
    00570   destination.y += (leftRight > 0 ? 1 : -1)*(200)*(1-factor);
    00571
    00572   if (destination.x < forwardComponent)
    00573     destination.x = forwardComponent;
    00574
    00575   double slowDownForMuchTurning =
    00576     Range<double>::Range(0.1, 1).limit
    00577     ((pi-fabs(angleToDestination))/pi);
    00578
    00579   motionRequest.motionType = MotionRequest::walk;
    00580   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00581   motionRequest.walkRequest.walkParams.translation.x = destination.x * slowDownForMuchTurning;
    00582   motionRequest.walkRequest.walkParams.translation.y = destination.y * slowDownForMuchTurning;
    00583
    00584   // set rotation speed in range
    00585   motionRequest.walkRequest.walkParams.rotation =
    00586     Range<double>::Range(-maxTurnSpeed, maxTurnSpeed).
    00587     limit(angleToDestination + .6*(leftRight > 0 ? -1 : 1)*(1-factor));
    00588 }
    00589
    00590 void GT2005BasicBehaviorTurnAroundPointFast::execute()
    00591 {
    00592   double maxTurnSpeed = fromDegrees(200);
    00593   if (forwardComponent == 0) forwardComponent = 200.0;
    00594
    00595   Vector2<double> destinationInWorldCoord(x,y);
    00596   double angleToDestination = Geometry::angleTo(robotPose.getPose(), destinationInWorldCoord);
    00597   double distance = Geometry::distanceTo(robotPose.getPose(), destinationInWorldCoord);
    00598
    00599   // destination = ball position in robot
    00600   // coordintes plus 100mm in x direction
    00601   Vector2<double> destination(
    00602     distance * cos(angleToDestination),
    00603     distance * sin(angleToDestination));
    00604
    00605   double factor =
    00606     Range<double>::Range(0, 1).
    00607       limit((distance - radius) / (3*radius));
    00608
    00609   destination *= factor * 2;
    00610   destination.y += (leftRight > 0 ? 1 : -1)*(450)*(1-factor);
    00611
    00612   if (destination.x < forwardComponent)
    00613     destination.x = forwardComponent;
    00614
    00615   double slowDownForMuchTurning =
    00616     Range<double>::Range(0.3, 1).limit
    00617     ((pi-fabs(angleToDestination))/pi);
    00618
    00619   motionRequest.motionType = MotionRequest::walk;
    00620   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00621   motionRequest.walkRequest.walkParams.translation.x = destination.x * slowDownForMuchTurning;
    00622   motionRequest.walkRequest.walkParams.translation.y = destination.y * slowDownForMuchTurning;
    00623
    00624   // set rotation speed in range
    00625   motionRequest.walkRequest.walkParams.rotation =
    00626     Range<double>::Range(-maxTurnSpeed, maxTurnSpeed).
    00627     limit(angleToDestination + .6*(leftRight > 0 ? -1 : 1)*(1-factor));
    00628 }
    00629
    00630 void GT2005BasicBehaviorTurnAroundPointWithRadius::execute()
    00631 {
    00632   double maxTurnSpeed = fromDegrees(240);
    00633   if (forwardComponent == 0) forwardComponent = 200.0;
    00634
    00635   Vector2<double> destinationInWorldCoord(x,y);
    00636   double angleToDestination = Geometry::angleTo(robotPose.getPose(), destinationInWorldCoord);
    00637   double distance = Geometry::distanceTo(robotPose.getPose(), destinationInWorldCoord);
    00638
    00639   // destination = ball position in robot
    00640   // coordintes plus 100mm in x direction
    00641   Vector2<double> destination(
    00642     distance * cos(angleToDestination),
    00643     distance * sin(angleToDestination));
    00644
    00645   double factor =
    00646     Range<double>::Range(-1, 1).
    00647       limit((distance - radius) / (radius));
    00648
    00649   destination *= factor;
    00650   destination.y += (leftRight > 0 ? 1 : -1)*(1000)*(1-factor);
    00651
    00652   if (destination.x < forwardComponent)
    00653     destination.x = forwardComponent;
    00654
    00655   double slowDownForMuchTurning =
    00656     Range<double>::Range(0.1, 1).limit
    00657     ((pi-fabs(angleToDestination))/pi);
    00658
    00659   motionRequest.motionType = MotionRequest::walk;
    00660   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00661   motionRequest.walkRequest.walkParams.translation.x = destination.x * slowDownForMuchTurning;
    00662   motionRequest.walkRequest.walkParams.translation.y = destination.y * slowDownForMuchTurning;
    00663
    00664   double angleFactor = 5;
    00665   //MODIFY("angle factor", angleFactor);
    00666
    00667   // set rotation speed in range
    00668   motionRequest.walkRequest.walkParams.rotation =
    00669     Range<double>::Range(-maxTurnSpeed, maxTurnSpeed).
    00670     limit(angleToDestination*angleFactor + (leftRight > 0 ? -1 : 1)); //*(factor));
    00671 }
    00672
    00673
    00674 void GT2005BasicBehaviorGoForwardToPoint::execute()
    00675 {
    00676   accelerationRestrictor.saveLastWalkParameters();
    00677
    00678   Vector2<double> destination(x,y);
    00679
    00680   if (maxSpeed == 0) maxSpeed = 350;
    00681
    00682   double maxTurnSpeed = fromDegrees(120);
    00683
    00684   double angleToDestination = Geometry::angleTo(robotPose,destination);
    00685
    00686
    00687   motionRequest.motionType = MotionRequest::walk;
    00688   motionRequest.walkRequest.walkType = WalkRequest::normal;
    00689
    00690   motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    00691   motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
    00692
    00693   motionRequest.walkRequest.walkParams.rotation = angleToDestination*2;
    00694   if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
    00695   {
    00696     motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;
    00697   }
    00698   if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed)
    00699   {
    00700     motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
    00701   }
    00702
    00703   accelerationRestrictor.restrictAccelerations(300,300,200);
    00704
    00705 }
    00706
    00707
    00708 void GT2005BasicBehaviorGoToPointAndAvoidObstacles::execute()
    00709 {
    00710   double
    00711     obstacleAvoidanceDistance,
    00712     distanceToDestination = Geometry::distanceTo(robotPose.getPose(),Vector2<double>(x,y)),
    00713     angleToDestination = Geometry::angleTo(robotPose.getPose(),Vector2<double>(x,y)),
    00714     targetSpeed, targetAngle,
    00715     freeSpaceInFront,
    00716     widthOfCorridor,
    00717     minSpeed,
    00718     distanceBelowWhichObstaclesAreAvoided;
    00719
    00720   // reset PIDs if last execution is long ago
    00721   if (SystemCall::getTimeSince(lastTimeExecuted) > 500)
    00722   {
    00723     speedX.resetTo(100);
    00724     speedY.resetTo(0);
    00725     turnSpeed.resetTo(0);
    00726   }
    00727   lastTimeExecuted = SystemCall::getCurrentSystemTime();
    00728
    00729   if (maxSpeed == 0) maxSpeed = 600;
    00730
    00731   // avoidance level:
    00732   // 0 = rugby (very little avoidance)
    00733   // 1 = football (medium...)
    00734   // 2 = standard, stay clear of obstacles
    00735
    00736 //  switch((int)avoidanceLevel)
    00737 //  {
    00738 //  case 0: widthOfCorridor = 150; break;
    00739 //  case 1: widthOfCorridor = 200; break;
    00740 //  case 2:
    00741 //  default: widthOfCorridor = 250; break;
    00742 //  }
    00743
    00744   widthOfCorridor = 350;
    00745
    00746   distanceBelowWhichObstaclesAreAvoided = 1000;
    00747   minSpeed = -150;
    00748
    00749   // perform clipping ...
    00750   obstacleAvoidanceDistance = Range<double>::Range(0, distanceBelowWhichObstaclesAreAvoided).limit(distanceToDestination);
    00751
    00752   // derive the forward speed of the robot
    00753   // from the free range in front of the robot
    00754   freeSpaceInFront = obstaclesModel.getDistanceInCorridor(0.0, widthOfCorridor);
    00755   targetSpeed = Range<double>::Range(minSpeed, maxSpeed).limit(freeSpaceInFront - 300);
    00756
    00757   // default: head to where there's more free space (necessary when close to obstacles!)
    00758   double leftForward = obstaclesModel.getTotalFreeSpaceInSector(.7, 1.39, obstacleAvoidanceDistance);
    00759   double rightForward = obstaclesModel.getTotalFreeSpaceInSector(-.7, 1.39, obstacleAvoidanceDistance);
    00760
    00761   // this angle is a good angle to head towards free space
    00762   targetAngle = (leftForward - rightForward) / (leftForward + rightForward);
    00763
    00764   // determine the next free sector in the desired direction
    00765   double nextFree = obstaclesModel.getAngleOfNextFreeSector(pi/4, angleToDestination, (int )obstacleAvoidanceDistance);
    00766
    00767   // if there is enough space in the direction of "nextFree"
    00768   // (which is a good direction heading towards the goal...) then
    00769   // head towards it
    00770   if (obstaclesModel.getDistanceInCorridor(nextFree, widthOfCorridor) > obstacleAvoidanceDistance)
    00771   {
    00772     //if (fabs(nextFree) < .1)
    00773       targetAngle = nextFree;
    00774   }
    00775   // double check: if there's enough space in the direction of the
    00776   // destination, directly head there
    00777   else if (obstaclesModel.getDistanceInCorridor(angleToDestination, widthOfCorridor) > obstacleAvoidanceDistance)
    00778   {
    00779     targetAngle = angleToDestination;
    00780   }
    00781   // there is something close and obstacle avoidance should be performed
    00782   else
    00783   {
    00784     double minTurnAngle = 0.3; // this angle is necessary for obstacles that are perpenticular in front of the robot and the target angle is therefore very small
    00785     if ((targetAngle < minTurnAngle) && (targetAngle > 0))
    00786       targetAngle = minTurnAngle;
    00787     if ((targetAngle < 0) && (targetAngle > -minTurnAngle))
    00788       targetAngle = -minTurnAngle;
    00789   }
    00790
    00791
    00792
    00793
    00794   // what is this good for?
    00795   //if(obstaclesModel.getDistanceInCorridor((double)sgn(targetAngle) * pi/2, distanceBelowWhichObstaclesAreAvoided/2) < 100)
    00796   //{
    00797   //  targetSpeed = maxSpeed;
    00798   //  targetAngle = 0;
    00799   //}
    00800
    00801   // adjust forward speed: if a lot of turning is required, reduce the forward speed!
    00802   if (targetSpeed == maxSpeed)
    00803     targetSpeed *= Range<double>::Range(0.3, 1).limit(
    00804       (pi-fabs(targetAngle))/pi );
    00805
    00806   if (targetSpeed < 150) targetSpeed = 150;
    00807
    00808   // if at destination, stop
    00809   if (distanceToDestination < 150)
    00810   {
    00811     motionRequest.motionType = MotionRequest::stand;
    00812   }
    00813   else
    00814   {
    00815     motionRequest.motionType = MotionRequest::walk;
    00816     motionRequest.walkRequest.walkType = WalkRequest::normal;
    00817     motionRequest.walkRequest.walkParams.translation.x = speedX.approximateVal(targetSpeed);
    00818     motionRequest.walkRequest.walkParams.translation.y = speedY.approximateVal(0);
    00819     motionRequest.walkRequest.walkParams.rotation = turnSpeed.approximateVal(2*targetAngle);
    00820   }
    00821 }
    00822
    00823 void GT2005BasicBehaviorGoToPoint::execute()
    00824 {
    00825   accelerationRestrictor.saveLastWalkParameters();
    00826
    00827   Vector2<double> destination(x,y);
    00828   const Vector2<double>& self = robotPose.translation;
    00829
    00830   if (maxSpeed==0) maxSpeed = 350;
    00831   if (maxSpeedY==0) maxSpeedY = 350;
    00832
    00833   double maxTurnSpeed = fromDegrees(40);
    00834
    00835   double distanceToDestination = Geometry::distanceTo(self,destination);
    00836
    00837   double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
    00838
    00839   double angleToDestination = Geometry::angleTo(robotPose,destination);
    00840
    00841
    00842   motionRequest.motionType = MotionRequest::walk;
    00843   motionRequest.walkRequest.walkType = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    00844
    00845   //this time does not necessarily include time for turning!:
    00846   double estimatedTimeToReachDestination;
    00847   if (distanceToDestination > 200)
    00848   {
    00849     estimatedTimeToReachDestination = (distanceToDestination+200) / maxSpeed;
    00850
    00851     motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    00852     motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
    00853   }
    00854   else
    00855   {
    00856     estimatedTimeToReachDestination = 2*distanceToDestination / maxSpeed + 2*fabs(angleDifference) / maxTurnSpeed;
    00857 //     OUTPUT (idText, text, "Time2ReachDestination previous: " <<
    00858 //      2*distanceToDestination / maxSpeed << "\nTime2ReachDestination now: " << estimatedTimeToReachDestination);
    00859
    00860     if (distanceToDestination > 30)
    00861     {
    00862       motionRequest.walkRequest.walkParams.translation.x =
    00863         cos(angleToDestination) * maxSpeed * distanceToDestination/200;
    00864       motionRequest.walkRequest.walkParams.translation.y =
    00865         sin(angleToDestination) * maxSpeed * distanceToDestination/200;
    00866     }
    00867     else
    00868     {
    00869       motionRequest.walkRequest.walkParams.translation.x = 0;
    00870       motionRequest.walkRequest.walkParams.translation.y = 0;
    00871     }
    00872   }
    00873
    00874   // If the estimated time is 0, position is already reached, so nothing has to be done anymore
    00875   if (estimatedTimeToReachDestination != 0)
    00876   {
    00877   /*
    00878   {
    00879     estimatedTimeToReachDestination = 0.001;
    00880   }
    00881   */
    00882
    00883     if (fabs(toDegrees(angleDifference)) > 20)
    00884     {
    00885       motionRequest.walkRequest.walkParams.rotation =
    00886         angleDifference / estimatedTimeToReachDestination;
    00887       motionRequest.walkRequest.walkParams.rotation =
    00888         min (maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    00889       motionRequest.walkRequest.walkParams.rotation =
    00890         max (-maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    00891     }
    00892     else
    00893     {
    00894       motionRequest.walkRequest.walkParams.rotation = 2*angleDifference;
    00895     }
    00896
    00897     /*
    00898     if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    00899     motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
    00900     */
    00901
    00902     motionRequest.walkRequest.walkParams.translation.y =
    00903       min (maxSpeedY, motionRequest.walkRequest.walkParams.translation.y);
    00904
    00905     if ((fabs(toDegrees(angleDifference))<angleRemain)&&(distanceToDestination<distanceRemain))
    00906     {
    00907       motionRequest.walkRequest.walkParams.translation.x = 0;
    00908       motionRequest.walkRequest.walkParams.translation.y = 0;
    00909       motionRequest.walkRequest.walkParams.rotation = 0;
    00910     }
    00911
    00912     accelerationRestrictor.restrictAccelerations(150,150,100);
    00913
    00914   } // if (estimatedTime != 0)
    00915   else  //only to be sure the robot won't move if desired position is reached
    00916   {
    00917     motionRequest.walkRequest.walkParams.translation.x = 0;
    00918     motionRequest.walkRequest.walkParams.translation.y = 0;
    00919     motionRequest.walkRequest.walkParams.rotation = 0;
    00920   }
    00921 }
    00922
    00923 void GT2005BasicBehaviorGoToRelativePoint::execute()
    00924 {
    00925   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00926   {
    00927     //save start positions for relative movement
    00928     startX = robotPose.translation.x;
    00929     startY = robotPose.translation.y;
    00930     startRot = robotPose.rotation;
    00931   }
    00932
    00933   accelerationRestrictor.saveLastWalkParameters();
    00934
    00935   Vector2<double> destination(x, y);
    00936
    00937   Vector2<double> self = robotPose.translation;
    00938   self.x -= startX;
    00939   self.y -= startY;
    00940
    00941   if (maxSpeed==0) maxSpeed = 450;
    00942   if (maxSpeedY==0) maxSpeedY = 450;
    00943
    00944   double maxTurnSpeed = fromDegrees(40);
    00945
    00946   double distanceToDestination = Geometry::distanceTo(self, destination);
    00947
    00948   double angleDifference = normalize(fromDegrees(destinationAngle) - (robotPose.rotation - startRot));
    00949
    00950   double angleToDestination = Geometry::angleTo(Pose2D((robotPose.rotation - startRot), self.x, self.y), destination);
    00951
    00952
    00953   motionRequest.motionType = MotionRequest::walk;
    00954   motionRequest.walkRequest.walkType = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    00955
    00956   //this time does not necessarily include time for turning!:
    00957   double estimatedTimeToReachDestination;
    00958   if (distanceToDestination > 200)
    00959   {
    00960     estimatedTimeToReachDestination = (distanceToDestination + 200) / maxSpeed;
    00961
    00962     motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    00963     motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
    00964   }
    00965   else
    00966   {
    00967     estimatedTimeToReachDestination = 2 * distanceToDestination / maxSpeed + 2 * fabs(angleDifference) / maxTurnSpeed;
    00968 //     OUTPUT (idText, text, "Time2ReachDestination previous: " <<
    00969 //      2 * distanceToDestination / maxSpeed << "\nTime2ReachDestination now: " << estimatedTimeToReachDestination);
    00970
    00971     if (distanceToDestination > 30)
    00972     {
    00973       motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed * distanceToDestination / 200;
    00974       motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed * distanceToDestination / 200;
    00975     }
    00976     else
    00977     {
    00978       motionRequest.walkRequest.walkParams.translation.x = 0;
    00979       motionRequest.walkRequest.walkParams.translation.y = 0;
    00980     }
    00981   }
    00982
    00983   // If the estimated time is 0, position is already reached, so nothing has to be done anymore
    00984   if (estimatedTimeToReachDestination != 0)
    00985   {
    00986     if (fabs(toDegrees(angleDifference)) > 20)
    00987     {
    00988       motionRequest.walkRequest.walkParams.rotation = angleDifference / estimatedTimeToReachDestination;
    00989       motionRequest.walkRequest.walkParams.rotation = min (maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    00990       motionRequest.walkRequest.walkParams.rotation = max (-maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    00991     }
    00992     else
    00993     {
    00994       motionRequest.walkRequest.walkParams.rotation = 2 * angleDifference;
    00995     }
    00996
    00997     motionRequest.walkRequest.walkParams.translation.y = min(maxSpeedY, motionRequest.walkRequest.walkParams.translation.y);
    00998
    00999     if ((fabs(toDegrees(angleDifference)) < angleRemain) && (distanceToDestination < distanceRemain))
    01000     {
    01001       motionRequest.walkRequest.walkParams.translation.x = 0;
    01002       motionRequest.walkRequest.walkParams.translation.y = 0;
    01003       motionRequest.walkRequest.walkParams.rotation = 0;
    01004     }
    01005   }
    01006   else  //only to be sure the robot won't move if desired position is reached
    01007   {
    01008     motionRequest.walkRequest.walkParams.translation.x = 0;
    01009     motionRequest.walkRequest.walkParams.translation.y = 0;
    01010     motionRequest.walkRequest.walkParams.rotation = 0;
    01011   }
    01012 }
    01013
    01014 void GT2005BasicBehaviorGoToPointFast::execute()
    01015 {
    01016   //Gotopoint without accelerationRestrictor
    01017   Vector2<double> destination(x,y);
    01018   const Vector2<double>& self = robotPose.translation;
    01019
    01020   if (maxSpeed==0) maxSpeed = 600;
    01021   if (maxSpeedY==0) maxSpeedY = 600;
    01022
    01023   double maxTurnSpeed = fromDegrees(40);
    01024   double distanceToDestination = Geometry::distanceTo(self,destination);
    01025   double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
    01026   double angleToDestination = Geometry::angleTo(robotPose,destination);
    01027
    01028   motionRequest.motionType = MotionRequest::walk;
    01029   motionRequest.walkRequest.walkType
    01030     = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    01031
    01032   //this time does not necessarily include time for turning!:
    01033   double estimatedTimeToReachDestination;
    01034   if (distanceToDestination > 200)
    01035   {
    01036     estimatedTimeToReachDestination = (distanceToDestination+200) / maxSpeed;
    01037
    01038     motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    01039     motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
    01040   }
    01041   else
    01042   {
    01043     estimatedTimeToReachDestination =   2*distanceToDestination
    01044                                       / maxSpeed + 2*fabs(angleDifference) / maxTurnSpeed;
    01045
    01046     if (distanceToDestination > 30)
    01047     {
    01048       motionRequest.walkRequest.walkParams.translation.x =
    01049         cos(angleToDestination) * maxSpeed * distanceToDestination/200;
    01050       motionRequest.walkRequest.walkParams.translation.y =
    01051         sin(angleToDestination) * maxSpeed * distanceToDestination/200;
    01052     }
    01053     else
    01054     {
    01055       motionRequest.walkRequest.walkParams.translation.x = 0;
    01056       motionRequest.walkRequest.walkParams.translation.y = 0;
    01057     }
    01058   }
    01059
    01060   // If the estimated time is 0, position is already reached, so nothing has to be done anymore
    01061   if (estimatedTimeToReachDestination != 0)
    01062   {
    01063     if (fabs(toDegrees(angleDifference)) > 20)
    01064     {
    01065       motionRequest.walkRequest.walkParams.rotation =
    01066         angleDifference / estimatedTimeToReachDestination;
    01067       motionRequest.walkRequest.walkParams.rotation =
    01068         min (maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    01069       motionRequest.walkRequest.walkParams.rotation =
    01070         max (-maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    01071     }
    01072     else
    01073     {
    01074       motionRequest.walkRequest.walkParams.rotation = 2*angleDifference;
    01075     }
    01076
    01077     motionRequest.walkRequest.walkParams.translation.y =
    01078       min (maxSpeedY, motionRequest.walkRequest.walkParams.translation.y);
    01079
    01080     if ((fabs(toDegrees(angleDifference))<angleRemain)&&(distanceToDestination<distanceRemain))
    01081     {
    01082       motionRequest.walkRequest.walkParams.translation.x = 0;
    01083       motionRequest.walkRequest.walkParams.translation.y = 0;
    01084       motionRequest.walkRequest.walkParams.rotation = 0;
    01085     }
    01086   } // if (estimatedTime != 0)
    01087   else  //only to be sure the robot won't move if desired position is reached
    01088   {
    01089     motionRequest.walkRequest.walkParams.translation.x = 0;
    01090     motionRequest.walkRequest.walkParams.translation.y = 0;
    01091     motionRequest.walkRequest.walkParams.rotation = 0;
    01092   }
    01093 }
    01094
    01095 void GT2005BasicBehaviorGoToInterceptionPoint::execute()
    01096 {
    01097   if (maxSpeed == 0) maxSpeed = 600;
    01098   if (maxSpeedY == 0) maxSpeedY = 600;
    01099
    01100   double maxTurnSpeed;
    01101   if (this->maxTurnSpeed == 0)
    01102     maxTurnSpeed = fromDegrees(180);
    01103   else
    01104     maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
    01105
    01106   Vector2<double> destination(x, y);
    01107
    01108   double angleToInterceptionPoint = Geometry::angleTo(Pose2D(0, 0, 0), destination);
    01109   double distanceToInterceptionPoint = Geometry::distanceTo(Pose2D(0, 0, 0), destination);
    01110
    01111   double factor;
    01112
    01113   // adjust forward speed:
    01114   // if a lot of turning is necessary, don't walk so fast!
    01115   factor = (pi-fabs(angleToInterceptionPoint))/pi;
    01116   if (factor > 1) factor = 1;
    01117   if (factor < 0) factor = 0;
    01118
    01119   destination.normalize();
    01120
    01121   if (distanceToInterceptionPoint < slowDownDistance)
    01122   {
    01123     destination *= maxSpeed*((distanceToInterceptionPoint/2)/slowDownDistance);
    01124   }
    01125   else
    01126     destination *= (maxSpeed*factor);
    01127
    01128   motionRequest.motionType = MotionRequest::walk;
    01129   motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
    01130   motionRequest.walkRequest.walkParams.translation.x = destination.x;
    01131   motionRequest.walkRequest.walkParams.translation.y = destination.y;
    01132
    01133   if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    01134     motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
    01135
    01136   if(angleFactor == 0)
    01137     angleFactor = 1.0;
    01138
    01139   factor = angleFactor;
    01140
    01141   motionRequest.walkRequest.walkParams.rotation = angleToInterceptionPoint * factor; /* this should also be factored!!!! */
    01142
    01143   // clip rotation speed
    01144   motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
    01145   motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);
    01146 }
    01147
    01148 //void GT2005BasicBehaviorDogAsJoystick::execute()
    01149 //{
    01150 //
    01151 ///*  headControlMode.pidData.setValues(JointData::legFR1,0,0,0);
    01152 //pidData.setValues(JointData::legFR2,0,0,0);
    01153 //pidData.setValues(JointData::legFL1,0,0,0);
    01154 //pidData.setValues(JointData::legFL2,0,0,0);
    01155 //  */
    01156 //  double tmpr1 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFR1]));
    01157 //  double tmpr2 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFR2]));
    01158 //  //double tmpr3 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFR3]));
    01159 //  double tmpl1 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFL1]));
    01160 //  double tmpl2 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFL2]));
    01161 //  //double tmpl3 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFL3]));
    01162 //
    01163 //  outgoingBehaviorTeamMessage.walkRequest.type = 0;
    01164 //  outgoingBehaviorTeamMessage.walkRequest.x = (tmpr1+tmpl1)*2.5;
    01165 //  outgoingBehaviorTeamMessage.walkRequest.y = (tmpr2-tmpl2)*2.5;
    01166 //  //  outgoingBehaviorTeamMessage.walkRequest.rotation = (tmpl1-tmpr1)/2  ;
    01167 //  outgoingBehaviorTeamMessage.walkRequest.rotation = (tmpl1-tmpr1)/20  ;
    01168 //}
    01169
    01170 void GT2005BasicBehaviorCalcWlanBearing::init()
    01171 {
    01172   OUTPUT (idText, text, "wlanbearing:init" << endl);
    01173
    01174   startTime = SystemCall::getCurrentSystemTime ();
    01175   frames = 0;
    01176   initExecuted = true;
    01177   filterExecuted = false;
    01178
    01179   for (int i = 0; i < WLANINFO; i++)
    01180   {
    01181     wlanInfo[i].link = 0.0;
    01182     wlanInfo[i].measures = 0;
    01183     wlanInfo[i].noise = 0.0;
    01184     wlanInfo[i].signal = 0.0;
    01185   }
    01186
    01187   slamPercept.wlanBearing.inProgress = false;
    01188   slamPercept.wlanBearing.finished = false;
    01189   slamPercept.wlanBearing.direction = 0;
    01190
    01191   rotationAngle = 0;
    01192 }
    01193
    01194 #define ROTATION_TIME 3750
    01195 //#define ROTATION_TIME 7900
    01196
    01197 #define TURNS 1
    01198
    01199 void GT2005BasicBehaviorCalcWlanBearing::execute()
    01200 {
    01201   unsigned char _state = (unsigned char) state;
    01202
    01203   headControlMode.headControlMode = headControlMode.lookStraightAhead;
    01204
    01205   motionRequest.motionType = motionRequest.stand;
    01206
    01207   motionRequest.walkRequest.walkParams.translation.x = 0;
    01208   motionRequest.walkRequest.walkParams.translation.y = 0;
    01209
    01210   Pose2D deltaOdometry = odometryData - lastOdometry;
    01211
    01212   lastOdometry = odometryData;
    01213
    01214   switch (_state)
    01215   {
    01216   case 0x00:
    01217     {
    01218       //init
    01219
    01220       if (!initExecuted)
    01221         init ();
    01222
    01223       motionRequest.walkRequest.walkParams.rotation = 0;
    01224
    01225       return;
    01226     };
    01227   case 0x01:
    01228     {
    01229       rotationAngle += deltaOdometry.rotation;
    01230
    01231       slamPercept.GPVal = rotationAngle;
    01232
    01233 //      OUTPUT (idText, text, "winkel : " << rotationAngle);
    01234
    01235 //      unsigned long timeStamp = SystemCall::getCurrentSystemTime ();
    01236
    01237 //      if ((timeStamp - startTime) < (ROTATION_TIME * TURNS))
    01238       if (rotationAngle <= pi2)
    01239       {
    01240         slamPercept.wlanBearing.inProgress = true;
    01241
    01242         motionRequest.walkRequest.walkParams.rotation = fromDegrees(45);
    01243
    01244         motionRequest.motionType = motionRequest.walk;
    01245
    01246     #ifdef APERIOS1_3_2
    01247 /*        unsigned long timeDiff = timeStamp - startTime;
    01248         timeDiff %= ROTATION_TIME;
    01249
    01250         double rotTime = (double)ROTATION_TIME;
    01251
    01252         unsigned char angle = (unsigned char) (((double)timeDiff / rotTime) * (double)WLANINFO);
    01253 */
    01254         unsigned int angle = (unsigned int)((rotationAngle / pi2) * (double)WLANINFO);
    01255
    01256 //        OUTPUT (idText, text, "angle : " << angle);
    01257
    01258         EtherDriverGetWLANStatisticsMsg myWLANStats;
    01259         ERA201D1_GetWLANStatistics(&myWLANStats);
    01260
    01261         wlanInfo[angle].measures++;
    01262         wlanInfo[angle].link += myWLANStats.statistics.link;
    01263         wlanInfo[angle].signal += myWLANStats.statistics.signal;
    01264         wlanInfo[angle].noise += myWLANStats.statistics.noise;
    01265
    01266         frames++;
    01267     #endif
    01268       }
    01269       else
    01270       {
    01271         motionRequest.walkRequest.walkParams.rotation = 0;
    01272         slamPercept.wlanBearing.finished = true;
    01273         slamPercept.wlanBearing.inProgress = false;
    01274       };
    01275
    01276       break;
    01277     };
    01278   case 0x02:
    01279     {
    01280       if (!filterExecuted)
    01281         Filter ();
    01282
    01283       break;
    01284     };
    01285   };
    01286 }
    01287
    01288 void GT2005BasicBehaviorCalcWlanBearing::Filter ()
    01289 {
    01290   OUTPUT (idText, text, "wlanbearing:filter" << endl);
    01291
    01292   unsigned int i = 0;
    01293 /*
    01294   FILE *file = fopen ("wlan.dat", "wt");
    01295
    01296   for (i = 0; i < WLANINFO; i++)
    01297   {
    01298     fprintf (file, "raw data %u : %f\n", i, (float)(wlanInfo[i].signal / wlanInfo[i].measures));
    01299   };
    01300
    01301   fclose (file);
    01302 */
    01303   char fileName[256];
    01304
    01305   sprintf (fileName, "/MS/wlan_%u.csv", numMeasure);
    01306
    01307   OutTextFile stream (fileName);
    01308
    01309   for (i = 0; i < WLANINFO; i++)
    01310     if (wlanInfo[i].measures > 0)
    01311       stream << "raw_data;" << i << ";" << (double)(wlanInfo[i].link / wlanInfo[i].measures) << ";" << (double)(wlanInfo[i].signal / wlanInfo[i].measures) << ";" << (double)(wlanInfo[i].noise / wlanInfo[i].measures) << ";" << "\n";
    01312
    01313 //    OUTPUT (idText, text, "raw data " << i << " : " << wlanInfo[i].signal / wlanInfo[i].measures);
    01314
    01315   double signalAverage = 0;
    01316
    01317   unsigned int count = 0;
    01318
    01319   //calculate signal average
    01320   for (i = 0; i < WLANINFO; i++)
    01321     if (wlanInfo[i].measures > 0)
    01322     {
    01323       signalAverage += wlanInfo[i].signal;
    01324
    01325       count++;
    01326     };
    01327
    01328   signalAverage /= count;
    01329
    01330   //calculate deltas
    01331   for (i = 0; i < WLANINFO; i++)
    01332     if (wlanInfo[i].measures > 0)
    01333       wlanInfo[i].dSignal = wlanInfo[i].signal - signalAverage;
    01334
    01335   double posSignalSum = 0;
    01336   double negSignalSum = 0;
    01337
    01338   for (i = 0; i < WLANINFO; i++)
    01339     if (wlanInfo[i].measures > 0)
    01340     {
    01341       if (wlanInfo[i].dSignal >= 0)
    01342         posSignalSum += wlanInfo[i].dSignal;
    01343       else
    01344         negSignalSum += wlanInfo[i].dSignal;
    01345     };
    01346
    01347   unsigned int position = WLANINFO / 2;
    01348
    01349   double currentPosSignalSum = 0;
    01350
    01351   while (currentPosSignalSum < posSignalSum)
    01352   {
    01353     if (wlanInfo[position % WLANINFO].measures > 0)
    01354       if (wlanInfo[position % WLANINFO].dSignal >= 0)
    01355         currentPosSignalSum += wlanInfo[position % WLANINFO].dSignal;
    01356
    01357     position++;
    01358   };
    01359
    01360   unsigned int posPoint = position;
    01361
    01362   slamPercept.wlanBearing.posCenter = position % WLANINFO;
    01363
    01364   position = WLANINFO / 2;
    01365
    01366   double currentNegSignalSum = 0;
    01367
    01368   while (currentNegSignalSum > negSignalSum)
    01369   {
    01370     if (wlanInfo[position % WLANINFO].measures > 0)
    01371       if (wlanInfo[position % WLANINFO].dSignal < 0)
    01372         currentNegSignalSum += wlanInfo[position % WLANINFO].dSignal;
    01373
    01374     position++;
    01375   };
    01376
    01377   unsigned int negPoint = position;
    01378
    01379   slamPercept.wlanBearing.negCenter = position % WLANINFO;
    01380
    01381   for (i = 0; i < 16; i++)
    01382   {
    01383     filteredInfo[i].measures = 0;
    01384     filteredInfo[i].link = 0;
    01385     filteredInfo[i].signal = 0;
    01386     filteredInfo[i].noise = 0;
    01387   };
    01388
    01389   //filter
    01390   for (i = 0; i < WLANINFO; i++)
    01391   {
    01392     //unsigned long timeDiff = wlanInfo[i].timeStamp - startTime;
    01393
    01394     //int angle = (int) (((double)(timeDiff % (ROTATION_TIME / 4)) / (double)ROTATION_TIME / 4.0) * 16.0);
    01395
    01396     unsigned int index = (unsigned int)(i / ((double) WLANINFO / (double)FILTEREDINFO));
    01397
    01398     filteredInfo[index].link += wlanInfo[i].link;
    01399     filteredInfo[index].noise += wlanInfo[i].noise;
    01400     filteredInfo[index].signal += wlanInfo[i].signal;
    01401     filteredInfo[index].measures += wlanInfo[i].measures;
    01402   }
    01403
    01404   for (i = 0; i < FILTEREDINFO; i++)
    01405   {
    01406     if (filteredInfo[i].measures > 0)
    01407     {
    01408       filteredInfo[i].link /= (double) filteredInfo[i].measures;
    01409       filteredInfo[i].noise /= (double) filteredInfo[i].measures;
    01410       filteredInfo[i].signal /= (double) filteredInfo[i].measures;
    01411     };
    01412   }
    01413
    01414   sprintf (fileName, "/MS/wlanf_%u.csv", numMeasure);
    01415
    01416   numMeasure++;
    01417
    01418   OutTextFile filteredStream (fileName);
    01419
    01420   for (i = 0; i < FILTEREDINFO; i++)
    01421     if (filteredInfo[i].measures > 0)
    01422       filteredStream  << "filtered_data;" << i << ";" << (double)(filteredInfo[i].link) << ";" << (double)(filteredInfo[i].signal) << ";" << (double)(filteredInfo[i].noise) << ";" << "\n";
    01423
    01424   int maxAngle = FILTEREDINFO + 1;
    01425   double maxVal = 0.0;
    01426
    01427   for (i = 0; i < FILTEREDINFO; i++)
    01428   {
    01429     if ((filteredInfo[i].measures > 0) && (filteredInfo[i].signal > maxVal))
    01430     {
    01431       maxVal = filteredInfo[i].signal;
    01432       maxAngle = i;
    01433     }
    01434   };
    01435
    01436   int minAngle = FILTEREDINFO + 1;
    01437   double minVal = 200.0;
    01438
    01439   for (i = 0; i < FILTEREDINFO; i++)
    01440   {
    01441     if ((filteredInfo[i].measures > 0) && (filteredInfo[i].signal < minVal))
    01442     {
    01443       minVal = filteredInfo[i].signal;
    01444       minAngle = i;
    01445     }
    01446   };
    01447
    01448   if (maxAngle < FILTEREDINFO/2)
    01449   {
    01450     std::cout << "facing yellow : " << maxAngle << ";" << minAngle << ";" << posPoint << ";" << negPoint << std::endl << std::flush;
    01451
    01452     ledRequest.backRearRedLED = LEDRequest::llll;
    01453     ledRequest.backFrontBlueLED = LEDRequest::oooo;
    01454   }
    01455   else
    01456   {
    01457     std::cout << "facing blue : " << maxAngle << ";" << minAngle << ";" << posPoint << ";" << negPoint << std::endl << std::flush;
    01458
    01459     ledRequest.backFrontBlueLED = LEDRequest::llll;
    01460     ledRequest.backRearRedLED = LEDRequest::oooo;
    01461   }
    01462
    01463   double bearing = (((double)maxAngle) / (double) FILTEREDINFO) * 360.0;
    01464
    01465   bearing -= 180.0;
    01466
    01467   slamPercept.wlanBearing.direction = bearing;
    01468
    01469   OUTPUT (idText, text, "ap in " << bearing << " degrees" << endl);
    01470
    01471   initExecuted = false;
    01472   filterExecuted = true;
    01473 };
    01474
    01475 void GT2005BasicBehaviorLocateMaxGreen::Init ()
    01476 {
    01477     slamPercept.greenLocation.greenLocationFinished = false;
    01478     slamPercept.greenLocation.angleToMaxGreenValid = false;
    01479     slamPercept.greenLocation.lengthOfMaxGreen = 0;
    01480     slamPercept.greenLocation.angleToMaxGreen = 0;
    01481
    01482     motionRequest.walkRequest.walkParams.rotation = 0;
    01483     motionRequest.walkRequest.walkParams.translation.x = 0;
    01484     motionRequest.walkRequest.walkParams.translation.y = 0;
    01485
    01486     lastOdometry = odometryData;
    01487
    01488     rotationAngle = 0;
    01489 };
    01490
    01491 void GT2005BasicBehaviorLocateMaxGreen::execute ()
    01492 {
    01493   Pose2D deltaOdometry = odometryData - lastOdometry;
    01494
    01495   lastOdometry = odometryData;
    01496
    01497   headControlMode.headControlMode = headControlMode.lookStraightAhead;
    01498
    01499   switch ((int)param)
    01500   {
    01501   case 0x00:
    01502     {
    01503       Init ();
    01504
    01505       break;
    01506     };
    01507   case 0x01:
    01508     {
    01509       if (!slamPercept.greenLocation.greenLocationFinished)
    01510       {
    01511           rotationAngle += deltaOdometry.rotation;
    01512
    01513           if (rotationAngle <= 2.0 * pi)
    01514           {
    01515             motionRequest.walkRequest.walkParams.rotation = fromDegrees(90);
    01516
    01517             motionRequest.motionType = motionRequest.walk;
    01518
    01519             if (slamPercept.greenLocation.impAngleToMaxGreenValid)
    01520             {
    01521               if (slamPercept.greenLocation.lengthOfMaxGreen < slamPercept.greenLocation.impLengthOfMaxGreen)
    01522               {
    01523                 slamPercept.greenLocation.angleToMaxGreen = rotationAngle + slamPercept.greenLocation.impAngleToMaxGreen;
    01524
    01525                 while (slamPercept.greenLocation.angleToMaxGreen > pi)
    01526                   slamPercept.greenLocation.angleToMaxGreen -= pi2;
    01527
    01528                 while (slamPercept.greenLocation.angleToMaxGreen < -pi)
    01529                   slamPercept.greenLocation.angleToMaxGreen += pi2;
    01530
    01531                 slamPercept.greenLocation.angleToMaxGreen = (slamPercept.greenLocation.angleToMaxGreen / pi) * 180.0;
    01532
    01533                 slamPercept.greenLocation.lengthOfMaxGreen = slamPercept.greenLocation.impLengthOfMaxGreen;
    01534               };
    01535             }
    01536           }
    01537           else
    01538           {
    01539             motionRequest.walkRequest.walkParams.rotation = 0;
    01540
    01541             slamPercept.greenLocation.greenLocationFinished = true;
    01542             slamPercept.greenLocation.angleToMaxGreenValid = true;
    01543
    01544             rotationAngle = 0;
    01545           };
    01546       }
    01547     };
    01548   };
    01549
    01550   return;
    01551 };
    01552
    01553 void GT2005BasicBehaviorTurn::Init ()
    01554 {
    01555     lastOdometry = odometryData;
    01556
    01557     motionRequest.walkRequest.walkParams.rotation = 0;
    01558     motionRequest.walkRequest.walkParams.translation.x = 0;
    01559     motionRequest.walkRequest.walkParams.translation.y = 0;
    01560
    01561     slamPercept.turnFinished = false;
    01562     rotationAngle = 0;
    01563
    01564     if (angle == 0.0)
    01565       targetRotation = radian;
    01566     else
    01567       targetRotation = (angle / 180.0) * pi;
    01568 };
    01569
    01570 void GT2005BasicBehaviorTurn::execute ()
    01571 {
    01572   Pose2D deltaOdometry = odometryData - lastOdometry;
    01573
    01574   lastOdometry = odometryData;
    01575
    01576   switch ((int)param)
    01577   {
    01578   case 0x00:
    01579     {
    01580       Init ();
    01581
    01582       break;
    01583     };
    01584   case 0x01:
    01585     {
    01586       rotationAngle += deltaOdometry.rotation;
    01587
    01588       if (fabs (targetRotation - rotationAngle) > pi / 18.0)
    01589       {
    01590         if (targetRotation > rotationAngle)
    01591           motionRequest.walkRequest.walkParams.rotation = fromDegrees(90);
    01592         else
    01593           motionRequest.walkRequest.walkParams.rotation = -fromDegrees(90);
    01594
    01595         motionRequest.motionType = motionRequest.walk;
    01596       }
    01597       else
    01598       {
    01599         motionRequest.walkRequest.walkParams.rotation = 0;
    01600
    01601         rotationAngle = 0;
    01602
    01603         slamPercept.turnFinished = true;
    01604       };
    01605
    01606       break;
    01607     };
    01608   };
    01609   return;
    01610 };