Add command group for robot to go to hopper and shoot

[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.RunFlyWheel;

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

/**
 *
 */
public class AutonHopperShoot extends CommandGroup {

  public AutonHopperShoot() {

    // Robot drives from center to front of airship
    addSequential(new DriveDistance(92.3, 1));
    // Robot turns towards hopper
    addSequential(new TurnForAngle(90.0, Constants.Direction.RIGHT, 1.0));
    // Robot drives near hopper
    addSequential(new DriveDistance());
    // Robot turns left towards hopper
    addSequential(new TurnForAngle(90));
    // Robot drives in front of hopper
    addSequential(new DriveDistance());
    // Robot turns to face hopper
    addSequential(new TurnForAngle(90));
    // Robot hits hopper switch
    addSequential(new DriveDistance());
    // Robot backs up from switch
    addSequential(new DriveDistance());
    // Robot turns towards the boiler
    addSequential(new TurnForAngle(90));
    // Robot drives to boiler
    addSequential(new DriveDistance());
    // Robot turns parallel to boiler
    addSequential(new TurnForAngle(45));
    // Shoot
    addSequential(new RunFlyWheel());

  }
}