00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "ArExport.h"
00028 #include "ariaOSDef.h"
00029 #include "ArActionLimiterBackwards.h"
00030 #include "ArRobot.h"
00031
00039 ArActionLimiterBackwards::ArActionLimiterBackwards(
00040 const char *name, double stopDistance, double slowDistance,
00041 double maxBackwardsSpeed, double widthRatio) :
00042 ArAction(name,
00043 "Slows the robot down so as not to hit anything in front of it.")
00044 {
00045 setNextArgument(ArArg("stop distance", &myStopDist,
00046 "Distance at which to stop. (mm)"));
00047 myStopDist = stopDistance;
00048
00049 setNextArgument(ArArg("slow distance", &mySlowDist,
00050 "Distance at which to slow down. (mm)"));
00051 mySlowDist = slowDistance;
00052
00053 setNextArgument(ArArg("maximum backwards speed", &myMaxBackwardsSpeed,
00054 "Maximum backwards speed, scales from this to 0 at stopDistance (-mm/sec)"));
00055 myMaxBackwardsSpeed = maxBackwardsSpeed;
00056
00057 setNextArgument(ArArg("width ratio", &myWidthRatio,
00058 "The ratio of robot width to how wide an area to check (ratio)"));
00059 myWidthRatio = widthRatio;
00060
00061 }
00062
00063 ArActionLimiterBackwards::~ArActionLimiterBackwards()
00064 {
00065
00066 }
00067
00068 ArActionDesired *
00069 ArActionLimiterBackwards::fire(ArActionDesired currentDesired)
00070 {
00071 double dist;
00072 double maxVel;
00073
00074 myDesired.reset();
00075 dist = myRobot->checkRangeDevicesCurrentBox(-myRobot->getRobotLength()/2,
00076 -(myRobot->getRobotWidth()/2.0 *
00077 myWidthRatio),
00078 mySlowDist + (-myRobot->getRobotLength()),
00079 (myRobot->getRobotWidth()/2.0 *
00080 myWidthRatio));
00081 dist -= myRobot->getRobotRadius();
00082 if (dist < -myStopDist)
00083 {
00084
00085 myDesired.setMaxNegVel(0);
00086 return &myDesired;
00087 }
00088 if (dist > -mySlowDist)
00089 {
00090
00091 myDesired.setMaxNegVel(-ArMath::fabs(myMaxBackwardsSpeed));
00092 return &myDesired;
00093 }
00094
00095
00096 maxVel = -ArMath::fabs(myMaxBackwardsSpeed) * ((-dist - myStopDist) / (mySlowDist - myStopDist));
00097
00098 myDesired.setMaxNegVel(maxVel);
00099 return &myDesired;
00100
00101 }