Merge pull request #52 from frc-862/feature/PROG-234-new-auton-paths

PROG-234 working 5 ball better this time

src/main/java/com/lightningrobotics/voidrobot/RobotContainer.java

@@ -8,10 +8,8 @@
 import com.lightningrobotics.common.util.filter.JoystickFilter;
 import com.lightningrobotics.common.util.filter.JoystickFilter.Mode;
 import com.lightningrobotics.voidrobot.commands.ZeroTurretHood;
-import com.lightningrobotics.voidrobot.commands.auto.paths.FiveBallTerminalStopping;
+import com.lightningrobotics.voidrobot.commands.auto.paths.FiveBallTerminal;
 import com.lightningrobotics.voidrobot.commands.auto.paths.OneBall;
-import com.lightningrobotics.voidrobot.commands.auto.paths.ThreeBallTerminal;
-import com.lightningrobotics.voidrobot.commands.auto.paths.ThreeBallTerminalVision;
 import com.lightningrobotics.voidrobot.commands.auto.paths.TwoBall;
 import com.lightningrobotics.voidrobot.commands.climber.runClimb;
 import com.lightningrobotics.voidrobot.commands.hood.ResetHood;
@@ -68,10 +66,8 @@ protected void configureAutonomousCommands() {
         try {
 			Autonomous.register("Taxi", new Path("1-2Ball.path", false).getCommand(drivetrain));
 			Autonomous.register("2 Ball", new TwoBall(drivetrain, shooter, hood, turret, indexer, intake, targeting));
-			Autonomous.register("1 Ball", new OneBall(drivetrain, shooter, hood, turret, indexer, intake, targeting));
-			Autonomous.register("3 Ball Terminal Vision", new ThreeBallTerminalVision(drivetrain, indexer, intake, shooter, hood, turret, targeting));
-			// Autonomous.register("3 Ball Terminal", new ThreeBallTerminal(drivetrain, indexer, intake, shooter, hood, turret, targeting));
-            Autonomous.register("5 Ball Terminal Vision", new FiveBallTerminalStopping(drivetrain, indexer, intake, shooter, hood, turret, targeting));
+            Autonomous.register("1 Ball", new OneBall(drivetrain, shooter, hood, turret, indexer, intake, targeting));
+            Autonomous.register("5 Ball Terminal", new FiveBallTerminal(drivetrain, indexer, intake, shooter, hood, turret, targeting));
 		} catch (Exception e) {
 			System.err.println("I did an oopsie.");
             e.printStackTrace();

src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonVisionShooting.java

@@ -5,7 +5,6 @@
 import com.lightningrobotics.voidrobot.subsystems.HubTargeting;
 import com.lightningrobotics.voidrobot.subsystems.Indexer;
 import com.lightningrobotics.voidrobot.subsystems.Shooter;
-import com.lightningrobotics.voidrobot.subsystems.Turret;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;

src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java → src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminal.java

@@ -1,25 +1,20 @@
 package com.lightningrobotics.voidrobot.commands.auto.paths;
 
-import javax.sound.midi.MidiEvent;
-
 import com.lightningrobotics.common.auto.Path;
 import com.lightningrobotics.common.command.core.TimedCommand;
 import com.lightningrobotics.voidrobot.commands.auto.commands.AutonDeployIntake;
 import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIndexeCargo;
 import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake;
 import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting;
 import com.lightningrobotics.voidrobot.commands.turret.AimTurret;
-import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargo;
 import com.lightningrobotics.voidrobot.subsystems.*;
 
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-public class FiveBallTerminalStopping extends ParallelCommandGroup {
+public class FiveBallTerminal extends ParallelCommandGroup {
 
 
 	private static Path start5Ball = new Path("Start5Ball.path", false);
@@ -28,12 +23,12 @@ public class FiveBallTerminalStopping extends ParallelCommandGroup {
 
 	// private static Path oneMeter = new Path("1Meter.path", false);
 
-    public FiveBallTerminalStopping(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception {
+    public FiveBallTerminal(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception {
 		super(
 			new InstantCommand(() -> targeting.setState(0)),
 			new AimTurret(turret, targeting),
 			new AutonIntake(intake),
-			// new TimedCommand(new AutonDeployIntake(intake), 0.75d),
+			new TimedCommand(new AutonDeployIntake(intake), 0.75d),
 
 			new SequentialCommandGroup(
 				new InstantCommand(indexer::initializeBallsHeld),
@@ -45,7 +40,8 @@ public FiveBallTerminalStopping(Drivetrain drivetrain, Indexer indexer, Intake i
 					new SequentialCommandGroup(
 						new AutonVisionShooting(shooter, hood, indexer, targeting, 3d, 0.2d, 200d),
 						new InstantCommand(() -> targeting.setState(1)),
-						new TimedCommand(new AutonIndexeCargo(indexer, 2), start5Ball.getDuration(drivetrain))
+						new TimedCommand(new AutonIndexeCargo(indexer, 1), start5Ball.getDuration(drivetrain) - (start5Ball.getDuration(drivetrain) / 2)),
+						new TimedCommand(new AutonIndexeCargo(indexer, 1), start5Ball.getDuration(drivetrain) - (start5Ball.getDuration(drivetrain) / 2))
 					)
 				),
 				new InstantCommand(drivetrain::stop),

src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/OneBall.java

@@ -3,7 +3,8 @@
 import com.lightningrobotics.common.auto.Path;
 import com.lightningrobotics.common.command.core.TimedCommand;
 import com.lightningrobotics.voidrobot.commands.auto.commands.AutonDeployIntake;
-import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargoVision;
+import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake;
+import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting;
 import com.lightningrobotics.voidrobot.subsystems.*;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -12,26 +13,21 @@
 
 public class OneBall extends ParallelCommandGroup {
 
-	private static Path path = new Path("1-2Ball.path", false);
+	private static Path twoBallPath = new Path("1-2Ball.path", false);
 
 	public OneBall(Drivetrain drivetrain, Shooter shooter, Hood hood, Turret turret, Indexer indexer, Intake intake, HubTargeting targeting) throws Exception {
 		super(
-		new SequentialCommandGroup(
-
-			new InstantCommand(() -> shooter.setPower(0.4)),
-
-			// Set Initial Balls Held To 1
-			new InstantCommand(indexer::initializeBallsHeld, indexer),
-
-			// Deploy Intake
+			new AutonIntake(intake),
 			new TimedCommand(new AutonDeployIntake(intake), 0.75d),
 
-			path.getCommand(drivetrain),
-
-			// Shoot 2 (Preload & Collected)
-			new AutonShootCargoVision(shooter, hood, indexer, targeting)
-
-		));
+			new SequentialCommandGroup(
+				new InstantCommand(indexer::initializeBallsHeld),
+
+				twoBallPath.getCommand(drivetrain),
+
+				new AutonVisionShooting(shooter, hood, indexer, targeting, 0d, 0d, 0d)
+			)
+		);	
 	}
 
 }