Adjust vision autos

Co-Authored-By: Alex Duan <[email protected]>
Co-Authored-By: Ayaka Kawano <[email protected]>
Co-Authored-By: Brianna Adewinmbi <[email protected]>
Co-Authored-By: Yaj Duan <[email protected]>

Author: 4 people

src/main/java/frc/robot/RobotContainer.java

@@ -294,6 +294,7 @@ private void initAutoChoosers() {
     // autoChooser.addOption("Vision Two Piece Cable (Alliance)", () -> VisionAutos.cableZoomTwoPieceAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance));
     autoChooser.addOption("Smooth Low Cube Auto", () -> VisionAllLowAuto.ThreeCubesAutoFast(swerveDrive, vision, arm, elevator, motorClaw, alliance));
     autoChooser.addOption("Cable Low Cube Auto", () -> VisionCableSideAuto.LowAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance));
+    autoChooser.addOption("Cable High Cube Auto", () -> VisionCableSideAuto.HighAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance));
 
 
     // Have alliance parameter but do not use it.

src/main/java/frc/robot/commands/VisionAllLowAuto.java

@@ -266,8 +266,8 @@ public static CommandBase ThreeCubesAutoFast(SwerveDrivetrain swerveDrive, VROOO
                 new Pose2d(3.8, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), 
                 new Pose2d(1.5, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)),
                 new Pose2d(-0.3, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)),
-                new Pose2d(-0.3, 0.45 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)),
-                new Pose2d(-0.45, 0.45 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9))
+                new Pose2d(-0.3, 0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)),
+                new Pose2d(-0.45, 0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9))
             ),
             trajectoryConfig);
 

src/main/java/frc/robot/commands/VisionCableSideAuto.java

@@ -295,70 +295,26 @@ public static CommandBase HighAuto(SwerveDrivetrain swerveDrive, VROOOOM vision,
             //init
             Commands.sequence(
 
-                Commands.race(
-                    waitSeconds(5),
-                    Commands.sequence(
-                        parallel(
-                            sequence(
-                                claw.setPower(-0.35),
-                                waitSeconds(0.25),
-                                claw.setPower(-0.25)
-                            ),
-                            deadline(
-                                waitSeconds(2),
-                                runOnce(() -> SmartDashboard.putString("Stage", "Score")),
-                                sequence(
-                                    runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeHigh)),
-                                    waitSeconds(0.5),
-                                    waitUntil(arm.atTargetPosition)
-                                ),
-                                sequence(
-                                    waitSeconds(0.5),
-                                    runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreHigh)),
-                                    waitSeconds(0.5),
-                                    waitUntil(elevator.atTargetPosition)
-                                )
-                            )
-                        ),
-
-                        waitSeconds(0.25),
-                        claw.setPower(0.3),
-                        waitSeconds(0.5),
-                        claw.setPowerZero(),
-                
-                        deadline(
-                            waitSeconds(0.5),
-                            runOnce(() -> SmartDashboard.putString("Stage", "Stow")),
-                            sequence(
-                                runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)),
-                                waitSeconds(0.5),
-                                waitUntil(elevator.atTargetPosition)
-                            ),
-                            sequence(
-                                runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)),
-                                waitSeconds(0.5),
-                                waitUntil(arm.atTargetPosition)
-                            )
-                        )
-                    )
-                ),
+                ChargeAutos.preloadHigh(arm, elevator, claw),
 
                 //TODO confirm if we need it!!!!!!
                 Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())),
 
                 //trajectory to cube
                 Commands.parallel(
-                    zoooomToCubeCommand,
-                    Commands.sequence(
-                        Commands.waitSeconds(1.5),
-                        runOnce(() -> arm.setTargetTicks((ArmConstants.kArmStow) )) // to be safe
-                    ),
-                    Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE))
+                    zoooomToCubeCommand
+                    // Commands.sequence(
+                    //     Commands.waitSeconds(1.5),
+                    //     runOnce(() -> arm.setTargetTicks((ArmConstants.kArmStow) )) // to be safe
+                    // ),
+                    
                 ),
                 new TurnToAngle(179.9, swerveDrive),
+                Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)),
 
                 Commands.race(
-                    new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier),
+                    
+                    new RunCommand(() -> vision.driveToCubeOnGround(claw, 4), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier),
                     Commands.waitSeconds(20) // kill this auto
                     // TODO need add protection here!!!!!!
                 ),
@@ -393,22 +349,22 @@ public static CommandBase HighAuto(SwerveDrivetrain swerveDrive, VROOOOM vision,
 
                 new TurnToAngle(0, swerveDrive),
 
-                parallel (
-                    Commands.race(
-                        new RunCommand(() -> vision.driveToGridTag(claw, atagIdFinal), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier),
-                        Commands.waitSeconds(3) 
-                    )/* ,
-
-                    //Drop arm high drop off
-                    Commands.deadline( // TODO: Fix this and the other two Deadline arm Commands
-                        Commands.waitSeconds(0.5),
-                        sequence(
-                            runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeMid)),
-                            waitSeconds(0.5),
-                            waitUntil(arm.atTargetPosition)
-                        )
-                    )*/
-                ),
+                // parallel (
+                //     Commands.race(
+                //         new RunCommand(() -> vision.driveToGridTag(claw, atagIdFinal), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier),
+                //         Commands.waitSeconds(3) 
+                //     )/* ,
+
+                //     //Drop arm high drop off
+                //     Commands.deadline( // TODO: Fix this and the other two Deadline arm Commands
+                //         Commands.waitSeconds(0.5),
+                //         sequence(
+                //             runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeMid)),
+                //             waitSeconds(0.5),
+                //             waitUntil(arm.atTargetPosition)
+                //         )
+                //     )*/
+                // ),
 
                 claw.setPower(0.3)