AREXPORT ArActionJoydrive::ArActionJoydrive(const char *name,
double transVelMax,
double turnAmountMax,
bool stopIfNoButtonPressed,
bool useOSCalForJoystick) :
ArAction(name, "This action reads the joystick and sets the translational and rotational velocities based on this.")
{
if ((myJoyHandler = Aria::getJoyHandler()) == NULL)
{
myJoyHandler = new ArJoyHandler;
myJoyHandler->init();
Aria::setJoyHandler(myJoyHandler);
}
setSpeeds(transVelMax, turnAmountMax);
setNextArgument(ArArg("trans vel max", &myTransVelMax, "The full speed to go when the joystick is maxed forwards/backwards (mm/sec)"));
setNextArgument(ArArg("turn amount max", &myTurnAmountMax, "The full amount to turn if the joystick is pushed all the way right/left (deg)"));
setNextArgument(ArArg("stop if no button pressed", &myStopIfNoButtonPressed, "If this is true, then joydrive will stop the robot if there is no button pressed, otherwise it will just let whatever lower priority things go."));
myStopIfNoButtonPressed = stopIfNoButtonPressed;
setNextArgument(ArArg("use os calibration for joystick", &myUseOSCal, "If this is true then the os calibration for the joystick will be used, otherwise autocalibration will be used."));
myUseOSCal = useOSCalForJoystick;
myPreviousUseOSCal = myUseOSCal;
myUseThrottle = false;
}
/**
@param name name of the action instance
@param backOffSpeed speed at which to back away (mm/sec)
@param backOffTime number of milisecons to back up for (msec)
@param turnTime number of milisecons to alow for turn (msec)
@param setMaximums if true, set desired maximum translation velocity limits to backOffSpeed while performing the action; if false, retain existing limits.
*/
AREXPORT ArActionBumpers::ArActionBumpers(const char *name,
double backOffSpeed,
int backOffTime, int turnTime,
bool setMaximums) :
ArAction(name, "Reacts to the bumpers triggering")
{
setNextArgument(ArArg("back off speed", &myBackOffSpeed,
"Speed at which to back away (mm/sec)"));
myBackOffSpeed = backOffSpeed;
setNextArgument(ArArg("back off time", &myBackOffTime,
"Number of msec to back up for (msec)"));
myBackOffTime = backOffTime;
//myStopTime = 1000;
setNextArgument(ArArg("turn time", &myTurnTime,
"Number of msec to allow for turn (msec)"));
myTurnTime = turnTime;
setNextArgument(ArArg("set maximums", &mySetMaximums,
"Whether to set maximum vels or not (bool)"));
mySetMaximums = setMaximums;
myFiring = false;
mySpeed = 0.0;
myHeading = 0.0;
myBumpMask = (ArUtil::BIT1 | ArUtil::BIT2 | ArUtil::BIT3 | ArUtil::BIT4 |
ArUtil::BIT5 | ArUtil::BIT6 | ArUtil::BIT7 | ArUtil::BIT8);
}
AREXPORT ArActionKeydrive::ArActionKeydrive(const char *name,
double transVelMax,
double turnAmountMax,
double velIncrement,
double turnIncrement)
:
ArAction(name, "This action reads the keyboard arrow keys and sets the translational and rotational velocities based on this."),
myUpCB(this, &ArActionKeydrive::up),
myDownCB(this, &ArActionKeydrive::down),
myLeftCB(this, &ArActionKeydrive::left),
myRightCB(this, &ArActionKeydrive::right),
mySpaceCB(this, &ArActionKeydrive::space)
{
setNextArgument(ArArg("trans vel max", &myTransVelMax, "The maximum speed to go (mm/sec)"));
myTransVelMax = transVelMax;
setNextArgument(ArArg("turn amount max", &myTurnAmountMax, "The maximum amount to turn (deg/cycle)"));
myTurnAmountMax = turnAmountMax;
setNextArgument(ArArg("vel increment per keypress", &myVelIncrement, "The amount to increment velocity by per keypress (mm/sec)"));
myVelIncrement = velIncrement;
setNextArgument(ArArg("turn increment per keypress", &myVelIncrement, "The amount to turn by per keypress (deg)"));
myTurnIncrement = turnIncrement;
myDesiredSpeed = 0;
myDeltaVel = 0;
myTurnAmount = 0;
mySpeedReset = true;
}
AREXPORT ArActionGoto::ArActionGoto(const char *name, ArPose goal,
double closeDist, double speed,
double speedToTurnAt, double turnAmount) :
ArAction(name, "Goes to the given goal.")
{
myDirectionToTurn = myCurTurnDir = 1;
myTurnedBack = false;
myPrinting = false;
setNextArgument(ArArg("goal", &myGoal, "ArPose to go to. (ArPose)"));
setGoal(goal);
setNextArgument(ArArg("close dist", &myCloseDist,
"Distance that is close enough to goal. (mm)"));
myCloseDist = closeDist;
setNextArgument(ArArg("speed", &mySpeed,
"Speed to travel to goal at. (mm/sec)"));
mySpeed = speed;
setNextArgument(ArArg("speed to turn at", &mySpeedToTurnAt,
"Speed to start obstacle avoiding at (mm/sec)"));
mySpeedToTurnAt = speedToTurnAt;
setNextArgument(ArArg("amount to turn", &myTurnAmount,
"Amount to turn when avoiding (deg)"));
myTurnAmount = turnAmount;
}
/**
@param name name of the action
@param stopDistance distance at which to stop (mm)
@param slowDistance distance at which to slow down (mm)
@param maxBackwardsSpeed maximum backwards speed, speed allowed scales
from this to 0 at the stop distance (mm/sec)
@param widthRatio The ratio of robot width to the width of the region this action checks for sensor readings.
@param avoidLocationDependentObstacles If true, stop as the robot nears location-dependent sensed obstacles, if false, ignore them.
*/
AREXPORT ArActionLimiterBackwards::ArActionLimiterBackwards(
const char *name, double stopDistance, double slowDistance,
double maxBackwardsSpeed, double widthRatio,
bool avoidLocationDependentObstacles) :
ArAction(name,
"Slows the robot down so as not to hit anything behind it.")
{
setNextArgument(ArArg("stop distance", &myStopDist,
"Distance at which to stop. (mm)"));
myStopDist = stopDistance;
setNextArgument(ArArg("slow distance", &mySlowDist,
"Distance at which to slow down. (mm)"));
mySlowDist = slowDistance;
setNextArgument(ArArg("maximum backwards speed", &myMaxBackwardsSpeed,
"Maximum backwards speed, scales from this to 0 at stopDistance (-mm/sec)"));
myMaxBackwardsSpeed = maxBackwardsSpeed;
setNextArgument(ArArg("width ratio", &myWidthRatio,
"The ratio of robot width to how wide an area to check (ratio)"));
myWidthRatio = widthRatio;
setNextArgument(ArArg("avoid location dependent obstacles",
&myAvoidLocationDependentObstacles,
"Whether to avoid location dependent obstacles or not"));
myAvoidLocationDependentObstacles = avoidLocationDependentObstacles;
}
/**
@param name name of the action
@param stopDistance distance at which to stop (mm)
@param slowDistance distance at which to slow down (mm)
@param slowSpeed speed allowed at slowDistance, scales to 0 at slow
distance (mm/sec)
@param widthRatio Ratio of the width of the box to look at to the robot radius (multiplier)
*/
AREXPORT ArActionLimiterForwards::ArActionLimiterForwards(const char *name,
double stopDistance,
double slowDistance,
double slowSpeed,
double widthRatio) :
ArAction(name,
"Slows the robot down so as not to hit anything in front of it.")
{
setNextArgument(ArArg("stop distance", &myStopDist,
"Distance at which to stop. (mm)"));
myStopDist = stopDistance;
setNextArgument(ArArg("slow distance", &mySlowDist,
"Distance at which to slow down. (mm)"));
mySlowDist = slowDistance;
setNextArgument(ArArg("slow speed", &mySlowSpeed,
"Speed at which to slow to at the slow distance, (mm/sec)"));
mySlowSpeed = slowSpeed;
setNextArgument(ArArg("width ratio", &myWidthRatio,
"Ratio of the width of the box to look at to the robot radius (multiplier)"));
myWidthRatio = widthRatio;
myLastStopped = false;
}
/**
@param name the name of the action
@param obstacleDistance distance at which to turn. (mm)
@param avoidVelocity Speed at which to go while avoiding an obstacle.
(mm/sec)
@param turnAmount Degrees to turn relative to current heading while
avoiding obstacle (deg)
@param useTableIRIfAvail Whether to use the table sensing IR if they are
available
*/
AREXPORT ArActionAvoidFront::ArActionAvoidFront(const char *name,
double obstacleDistance,
double avoidVelocity,
double turnAmount,
bool useTableIRIfAvail) :
ArAction(name, "Slows down and avoids obstacles in front of the robot.")
{
setNextArgument(ArArg("obstacle distance", &myObsDist,
"Distance at which to turn. (mm)"));
myObsDist = obstacleDistance;
setNextArgument(ArArg("avoid speed", &myAvoidVel,
"Speed at which to go while avoiding an obstacle. (mm/sec)"));
myAvoidVel = avoidVelocity;
setNextArgument(ArArg("turn ammount", &myTurnAmountParam,
"Degrees to turn relative to current heading while avoiding obstacle (deg)"));
myTurnAmountParam = turnAmount;
setNextArgument(ArArg("use table IR", &myUseTableIRIfAvail,
"true to use table sensing IR for avoidance if the robot has them, false otherwise"));
myUseTableIRIfAvail = useTableIRIfAvail;
myTurning = 0;
}
/*
Note the use of constructor chaining with
ArAction(actionName). Also note how it uses setNextArgument, which makes it so that
other parts of the program could find out what parameters this action has, and possibly modify them.
*/
ActionGo::ActionGo(double maxSpeed, double stopDistance) :
ArAction("Go")
{
mySonar = NULL;
myMaxSpeed = maxSpeed;
myStopDistance = stopDistance;
setNextArgument(ArArg("maximum speed", &myMaxSpeed, "Maximum speed to go."));
setNextArgument(ArArg("stop distance", &myStopDistance, "Distance at which to stop."));
}
/*
This is the ActionTurn constructor, note the use of constructor chaining
with the ArAction. also note how it uses setNextArgument, which makes
it so that other things can see what parameters this action has, and
set them. It also initializes the classes variables.
*/
ActionTurn::ActionTurn(double turnThreshold, double turnAmount) :
ArAction("Turn")
{
myTurnThreshold = turnThreshold;
myTurnAmount = turnAmount;
setNextArgument(ArArg("turn threshold (mm)", &myTurnThreshold, "The number of mm away from obstacle to begin turnning."));
setNextArgument(ArArg("turn amount (deg)", &myTurnAmount, "The number of degress to turn if turning."));
myTurning = 0;
}
TestAction3(const char *name, const char *description = "") :
ArAction(name, description)
{
setNextArgument(ArArg("first", &myDouble, "That infamous double"));
setNextArgument(ArArg("second", &myInt));
setNextArgument(ArArg("third", myString, "An infamous string", sizeof(myString)));
myDouble = 3;
myInt = 3;
strcpy(myString, "3");
};
AREXPORT ArActionAvoidSide::ArActionAvoidSide(const char *name,
double obstacleDistance,
double turnAmount) :
ArAction(name, "Avoids side obstacles, ie walls")
{
setNextArgument(ArArg("obstacle distance", &myObsDist,
"Distance at which to start avoiding (mm)"));
myObsDist = obstacleDistance;
setNextArgument(ArArg("turn amount", &myTurnAmount,
"Degrees at which to turn (deg)"));
myTurnAmount = turnAmount;
myTurning = false;
}
AREXPORT ArActionDriveDistance::ArActionDriveDistance(const char *name,
double speed,
double deceleration) :
ArAction(name, "Drives a given distance.")
{
myPrinting = false;
setNextArgument(ArArg("speed", &mySpeed,
"Speed to travel to at. (mm/sec)"));
setNextArgument(ArArg("deceleration", &myDeceleration,
"Speed to decelerate at. (mm/sec/sec)"));
mySpeed = speed;
myDeceleration = deceleration;
myState = STATE_NO_DISTANCE;
}
/**
@param name name of the action
@param velocity velocity to travel at (mm/sec)
*/
AREXPORT ArActionConstantVelocity::ArActionConstantVelocity(const char *name,
double velocity) :
ArAction(name, "Sets the robot to travel straight at a constant velocity.")
{
setNextArgument(ArArg("velocity", &myVelocity,
"The velocity to make the robot travel at. (mm/sec)"));
myVelocity = velocity;
}
AREXPORT ArActionRobotJoydrive::ArActionRobotJoydrive(
const char *name, bool requireDeadmanPushed) :
ArAction(name, "This action reads the joystick on the robot and sets the translational and rotational velocities based on this."),
myHandleJoystickPacketCB(this, &ArActionRobotJoydrive::handleJoystickPacket),
myConnectCB(this, &ArActionRobotJoydrive::connectCallback)
{
myRequireDeadmanPushed = requireDeadmanPushed;
setNextArgument(ArArg("whether to require the deadman to be pushed or not", &myRequireDeadmanPushed, "If this is true then deadman will need to be pushed to drive, if false we'll drive based on the joystick all the time"));
myDeadZoneLast = false;
myHandleJoystickPacketCB.setName("ArActionRobotJoydrive");
}
/**
@param name name of the action
@param stopDistance distance at which to stop (mm)
@param slowDistance distance at which to slow down (mm)
@param maxBackwardsSpeed maximum backwards speed, speed allowed scales
from this to 0 at the stop distance (mm/sec)
@param widthRatio The ratio of robot width to the width of the region this action checks for sensor readings.
*/
AREXPORT ArActionLimiterBackwards::ArActionLimiterBackwards(
const char *name, double stopDistance, double slowDistance,
double maxBackwardsSpeed, double widthRatio) :
ArAction(name,
"Slows the robot down so as not to hit anything in front of it.")
{
setNextArgument(ArArg("stop distance", &myStopDist,
"Distance at which to stop. (mm)"));
myStopDist = stopDistance;
setNextArgument(ArArg("slow distance", &mySlowDist,
"Distance at which to slow down. (mm)"));
mySlowDist = slowDistance;
setNextArgument(ArArg("maximum backwards speed", &myMaxBackwardsSpeed,
"Maximum backwards speed, scales from this to 0 at stopDistance (-mm/sec)"));
myMaxBackwardsSpeed = maxBackwardsSpeed;
setNextArgument(ArArg("width ratio", &myWidthRatio,
"The ratio of robot width to how wide an area to check (ratio)"));
myWidthRatio = widthRatio;
}
AREXPORT Avoid::Avoid(const char *name,
ArPose goal,
double obstacleFrontDistance,
double obstacleSideDistance,
double minDistance) :
ArAction(name, "Avoids side obstacles, ie walls")
{
setNextArgument(ArArg("goal", &myGoal, "ArPose to go to. (ArPose)"));
myGoal = goal;
myOldGoal = myGoal;
setNextArgument(ArArg("obstacle front distance", &myObsDist_front,
"Distance at which to start avoiding (mm)"));
myObsDist_front = obstacleFrontDistance;
setNextArgument(ArArg("obstacle side distance", &myObsDist_side,
"Distance at which to start avoiding (mm)"));
myObsDist_side = obstacleSideDistance;
setNextArgument(ArArg("turn amount", &minimum_Dist,
"Degrees at which to turn (deg)"));
minimum_Dist=minDistance;
//雷达数据大突变，保持
leftDist_before = 0;
rightDist_before = 0;
//标志区//
//到达目标，停止机器人标志
STOP_CAR_FIRST = false;
//新定义目标点后，车体指向目标点完毕标志
ACHIEVE_ANGLE = false;
//转向目标点标志，
TURN_TO_GOAL = false;
//当前位置到目标位置角度
poseAngle = 0;
}
AREXPORT ArActionGotoStraight::ArActionGotoStraight(const char *name,
double speed) :
ArAction(name, "Goes to the given goal.")
{
myPrinting = false;
setNextArgument(ArArg("speed", &mySpeed,
"Speed to travel to goal at. (mm/sec)"));
mySpeed = speed;
myState = STATE_NO_GOAL;
myUseEncoderGoal = false;
myBacking = false;
setCloseDist();
}
TestAction1(const char *name, const char *description = "") :
ArAction(name, description)
{
setNextArgument(ArArg("first", &myDouble));
myDouble = 1;
};
