GoToXYPath.java 文件源码

/**
 * Init- calculates the drive distance and turn degrees, plugs them into a
 * path command, and adds it to the command group.
 */
@Override
protected void initialize()
{
    mCurrentX = mPositioner.getXPosition();
    double ChangeInX = mFinalXCoor - mCurrentX;

    mCurrentY = mPositioner.getYPosition();
    double ChangeInY = mFinalYCoor - mCurrentY;

    mDriveDistance = Math.sqrt((Math.pow((ChangeInX), 2)) + (Math.pow((ChangeInY), 2)));
    mTurnDegrees = Math.toDegrees(Math.atan2((ChangeInX), (ChangeInY)));

    mTurnPathConfig = new PathConfig(mTurnDegrees, mMaxTurnVel, mMaxTurnAccel, .02);
    mTurnSetpointIterator = new StaticSetpointIterator(mTurnPathConfig);
    mDriveTurnPath = new DriveTurnPath(mDriveTrain, mPositioner, mTurnSetpointIterator);

    mDrivePathConfig = new PathConfig(mDriveDistance, mMaxDriveVel, mMaxDriveAccel, .02);
    mDriveSetpointIterator = new StaticSetpointIterator(mDrivePathConfig);
    mDriveStraightPath = new DriveStraightPath(mDriveTrain, mPositioner, mDriveSetpointIterator);

    mCommandGroup = new CommandGroup();
    mCommandGroup.addSequential(mDriveTurnPath);
    mCommandGroup.addSequential(mDriveStraightPath);
}