finished Auton strat for peg close to boiler in any alliance

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commandgroups / AutonGearThenBaselinePegCloseToBoiler.java

package org.usfirst.frc.team3501.robot.commandgroups;

import org.usfirst.frc.team3501.robot.Constants.Direction;
import org.usfirst.frc.team3501.robot.Robot;
import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 *
 */
public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup {
 
 
    private static final int MOTOR_VALUE_TURN = 60;
    private static final int MOTOR_VALUE_FORWARD = 60;
    private static final double DISTANCE_FROM_BOILER_WALL = 111.5676;

    public AutonGearThenBaselinePegCloseToBoiler() {
  
      
      requires(Robot.getDriveTrain());
      addSequential(new DriveDistance(DISTANCE_FROM_BOILER_WALL/Math.sqrt(3)+52.4614,MOTOR_VALUE_FORWARD));
      addSequential(new TurnForAngle(60, Direction.LEFT , MOTOR_VALUE_TURN));
      addSequential(new DriveDistance(143.3272-2*DISTANCE_FROM_BOILER_WALL/Math.sqrt(3)-10.5,MOTOR_VALUE_FORWARD));
      //does not include drift distance
      
    }
}