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 org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
import edu.wpi.first.wpilibj.command.CommandGroup;
/**
- *
+ * // Robot starts in middle, goes to the hopper, then boiler,then shoots during
+ * auton
*/
public class AutonHopperShoot extends CommandGroup {
- public AutonHopperShoot() {
+ private static final double DISTANCE_TO_HOPPER = 224.569677;
+ private static final double DISTANCE_TO_TURNING_POSITION = 5.0;
+ public AutonHopperShoot() {
// Robot drives from center to front of airship
- addSequential(new DriveDistance(92.3, 1));
+ addSequential(new DriveDistance(DISTANCE_TO_TURNING_POSITION, 0.75));
// Robot turns towards hopper
- addSequential(new TurnForAngle(90.0, Constants.Direction.RIGHT, 1.0));
+ addSequential(new TurnForAngle(45.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());
+ addSequential(new DriveDistance(DISTANCE_TO_HOPPER, 1));
+ // Robot turns right towards hopper
+ addSequential(new TurnForAngle(45.0, Constants.Direction.RIGHT, 1.0));
+ // Robot drives into hopper switch
+ addSequential(new DriveDistance(2.0, 1.0));
// Robot backs up from switch
- addSequential(new DriveDistance());
+ addSequential(new DriveDistance(2.0, -1.0));
// Robot turns towards the boiler
- addSequential(new TurnForAngle(90));
+ addSequential(new TurnForAngle(90.0, Constants.Direction.RIGHT, 1.0));
// Robot drives to boiler
- addSequential(new DriveDistance());
+ addSequential(new DriveDistance(123.3, 1.0));
// Robot turns parallel to boiler
- addSequential(new TurnForAngle(45));
+ addSequential(new TurnForAngle(45.0, Constants.Direction.LEFT, 1.0));
// Shoot
- addSequential(new RunFlyWheel());
+ addSequential(new RunFlyWheelContinuous());
}
}