Remove two steps from AutonHopperShooter and decrease time it takes to execute commands

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

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

  public AutonHopperShoot() {

    // Robot drives from center to front of airship
    addSequential(new DriveDistance(5.0, 1));
    // Robot turns towards hopper
    addSequential(new TurnForAngle(45.0, Constants.Direction.RIGHT, 1.0));
    // Robot drives near hopper
    addSequential(new DriveDistance(224.569677, 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));

  }
}