Add DISTANCE_TO_TURNING_POINT to AutonHopperShoot

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

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

import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;

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

/**
 * // Robot starts in middle, goes to the hopper, then boiler,then shoots during
 * auton
 */
public class AutonHopperShoot extends CommandGroup {

  private static final double DISTANCE_TO_HOPPER = 224.569677;
  private static final double DISTANCE_TO_TURNING_POSITION = 5.0;

  public AutonHopperShoot() {
    // Robot drives from center to front of airship
    addSequential(new DriveDistance(DISTANCE_TO_TURNING_POSITION, 1));
    // Robot turns towards hopper
    addSequential(new TurnForAngle(45.0, Constants.Direction.RIGHT, 1.0));
    // Robot drives near hopper
    addSequential(new DriveDistance(DISTANCE_TO_HOPPER, 1));
    // Robot turns left towards hopper
    addSequential(new TurnForAngle(45.0, Constants.Direction.RIGHT, 1.0));
    // Robot drives in front of hopper
    addSequential(new DriveDistance(2.0, 1.0));
    // Robot backs up from switch
    addSequential(new DriveDistance(2.0, -1.0));
    // Robot turns towards the boiler
    addSequential(new TurnForAngle(90.0, Constants.Direction.RIGHT, 1.0));
    // Robot drives to boiler
    addSequential(new DriveDistance(123.3, 1.0));
    // Robot turns parallel to boiler
    addSequential(new TurnForAngle(45.0, Constants.Direction.LEFT, 1.0));
    // Shoot
    addSequential(new RunFlyWheelContinuous(1.0));

  }
}