package org.usfirst.frc.team3501.robot.commands.shooter;
import org.usfirst.frc.team3501.robot.Constants.IntakeArm;
-import org.usfirst.frc.team3501.robot.Robot;
import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm;
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, and the intake is in the up position
+ * pre-conditions: a ball is in the intake
*
* post-conditions: catapult is retracted, intake is extended
*/
public class ShootAtHighGoal extends CommandGroup {
+ private static final double WAIT_SECONDS = 1.0;
public ShootAtHighGoal() {
- // (if photogate) check if ball is in intake
- // make sure intake is in up position and change accordingly
- if (!Robot.intakeArm.isExtended())
- addSequential(new MoveIntakeArm(IntakeArm.RETRACT));
+ // make sure intake is in up position
+ addSequential(new MoveIntakeArm(IntakeArm.RETRACT));
+
+ addSequential(new WaitCommand(WAIT_SECONDS));
+
+ // get ball onto catapult
// 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));
+
+ // wait a bit
+ addSequential(new WaitCommand(WAIT_SECONDS));
+
// retract catapult pistons
addSequential(new ResetCatapult());
+ //
}
}