add code that puts ball onto catapult

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / shooter / ShootAtHighGoal.java

package org.usfirst.frc.team3501.robot.commands.shooter;

import org.usfirst.frc.team3501.robot.Constants.IntakeArm;
import org.usfirst.frc.team3501.robot.Constants.Shooter;
import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm;
import org.usfirst.frc.team3501.robot.commands.intakearm.TimeRunIntake;

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

/**
 * This command group performs the sequence of steps to shoot at the high goal
 *
 * pre-conditions: a ball is in the intake
 *
 * post-conditions: catapult is retracted, intake is extended
 */
public class ShootAtHighGoal extends CommandGroup {
  // TODO: test for this
  private static final double WAIT_SECONDS = 1.0;

  public ShootAtHighGoal() {

    // make sure intake is in up position
    addSequential(new MoveIntakeArm(IntakeArm.RETRACT));

    addSequential(new WaitCommand(WAIT_SECONDS));

    // get ball onto catapult
    addSequential(new TimeRunIntake(Shooter.TIME_FOR_BALL_TO_CATAPULT_ROLLING));

    addSequential(new WaitCommand(WAIT_SECONDS));

    // shoot catapult pistons
    addSequential(new FireCatapult());

    addSequential(new WaitCommand(WAIT_SECONDS));

    // extend intake (ball actually shoots here)
    addSequential(new MoveIntakeArm(IntakeArm.EXTEND));

    addSequential(new WaitCommand(WAIT_SECONDS));

    // retract catapult pistons
    addSequential(new ResetCatapult());

    //
  }
}