void DpExecMachine::rotateToHeading(bool first) {
float rotationSpeed = 0;
static int lineUpCount = 0;
static float leftDist = 0 , rightDist = 0;
float delta = getDeltaHeading(centerPoleHeading);
static unsigned long startTime;
arm.commandSwingArm(SA_DOWN);
if(abs(delta) < 5){
leftDist = ultraSonicMgr.getSensor(LEFT)->getCalculatedDistanceValue();
rightDist = ultraSonicMgr.getSensor(RIGHT)->getCalculatedDistanceValue();
if(lineUpCount++ >= 10){
lineUpCount = 0;
DirectionToDPRight = leftDist - 3.5 < rightDist;
currentState = (state) &DpExecMachine::backUpToWall;
}
}
else {
rotationSpeed = getToHeading(centerPoleHeading);
lineUpCount = 0;
leftDist = rightDist = 0;
}
wheels.updateCommand(0,0,rotationSpeed);
if (first) startTime = micros();
if (micros() - startTime > 1500000) {
lineUpCount = 0;
DirectionToDPRight = leftDist - 3.5 < rightDist;
currentState = (state) &DpExecMachine::backUpToWall;
}
}
AREXPORT void ArActionDesired::log(void) const
{
// all those maxes and movement parameters
if (getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransVel %.0f", getMaxVel());
if (getMaxNegVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransNegVel %.0f",
getMaxNegVel());
if (getTransAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransAccel %.0f", getTransAccel());
if (getTransDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransDecel %.0f", getTransDecel());
if (getMaxRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tMaxRotVel %.0f", "",
getMaxRotVel());
if (getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotAccel %.0f", "",
getRotAccel());
if (getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotDecel %.0f", "",
getRotDecel());
if (getMaxLeftLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxLeftLatVel %.0f", "",
getMaxLeftLatVel());
if (getMaxRightLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxRightLatVel %.0f", "",
getMaxRightLatVel());
if (getLatAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatAccel %.0f", "",
getLatAccel());
if (getLatDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatDecel %.0f", "",
getLatDecel());
// the actual movement part
if (getVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tVel %.0f", getVel());
if (getHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tHeading %.0f", "",
getHeading());
if (getDeltaHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tDeltaHeading %.0f", "",
getDeltaHeading());
if (getRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotVel %.0f", "",
getRotVel());
if (getLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatVel %.0f", "",
getLatVel());
}