GoToXY.java 文件源码

/**
 * Calculates distance to travel and proper orientation then creates
 * DriveStraightADistance and TurnWithDegrees Commands, adds them to a
 * CommandGroup, then starts the CommandGroup.
 */
@Override
protected void initialize()
{

    mCommandGroup = new CommandGroup();
    mCurrentX = mPositioner.getXPosition();
    mCurrentY = mPositioner.getYPosition();
    mDriveDistance = Math.sqrt((Math.pow((mFinalXCoor - mCurrentX), 2)) + (Math.pow((mFinalYCoor - mCurrentY), 2)));
    mTurnDegrees = Math.toDegrees(Math.atan2((mFinalXCoor - mCurrentX), (mFinalYCoor - mCurrentY)));
    mTurnWithDegrees = new TurnWithDegrees(mDriveTrain, mPositioner, mTurnDegrees, mSpeed);
    System.out.println(mTurnDegrees);
    mDriveStraightADistance = new DriveStraightADistance(mDriveTrain, mPositioner, mDriveDistance, mSpeed);
    mCommandGroup.addSequential(mTurnWithDegrees);
    mCommandGroup.addSequential(mDriveStraightADistance);
    mCommandGroup.start();

}