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()); }