add variables for both sides

[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.Constants.Direction;
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 {

  // If red, direction is right; if blue, direction is left
  private static final Direction DIRECTION_TO_HOPPER = Constants.Direction.RIGHT;
  // If red, direction is left; if blue, direction is right
  private static final Direction DIRECTION_TO_BOILER = Constants.Direction.LEFT;

  public AutonHopperShoot() {
    // Robot drives from center to front of airship
    addSequential(new DriveDistance(94.75, 0.75));
    // Robot turns towards hopper
    addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 1.0));
    // Robot drives into hopper switch
    addSequential(new DriveDistance(44.0, 1));
    // Robot backs up from switch
    addSequential(new DriveDistance(5.0, -1.0));
    // Robot turns towards the boiler
    addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 1.0));
    // Robot drives to boiler
    addSequential(new DriveDistance(75.7, 0.75));
    // Robot turns parallel to boiler
    addSequential(new TurnForAngle(46.6, Constants.Direction.LEFT, 1.0));
    // Shoot
    addSequential(new RunFlyWheelContinuous());

  }
}