import org.usfirst.frc.team3501.robot.Constants.IntakeArm;
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;
* post-conditions: catapult is retracted, intake is extended
*/
public class ShootAtHighGoal extends CommandGroup {
+ private static final double WAIT_SECONDS = 0.4;
+ private static final double TIME_FOR_BALL_TO_CATAPULT_ROLLING = 1.0;
public ShootAtHighGoal() {
- // make sure catapult is down
- addSequential(new ResetCatapult());
// make sure intake is in up position
addSequential(new MoveIntakeArm(IntakeArm.RETRACT));
+ addSequential(new WaitCommand(WAIT_SECONDS));
+
+ // get ball onto catapult
+ addSequential(new TimeRunIntake(TIME_FOR_BALL_TO_CATAPULT_ROLLING));
+
// shoot catapult pistons
addSequential(new FireCatapult());
+ addSequential(new WaitCommand(WAIT_SECONDS));
+
// extend intake (ball actually shoots here)
addSequential(new MoveIntakeArm(IntakeArm.EXTEND));
- // wait a bit
- addSequential(new WaitCommand(1.0));
+ addSequential(new WaitCommand(WAIT_SECONDS));
// retract catapult pistons
addSequential(new ResetCatapult());
+ //
}
}