batb winning code

src/main/java/team3647/frc2024/auto/AutoCommands.java

@@ -165,7 +165,9 @@ public class AutoCommands implements AllianceObserver {
 
     public final AutonomousMode doNothing;
 
-    public final AutonomousMode preloadOnly;
+    public final AutonomousMode redPreloadOnly;
+
+    public final AutonomousMode bluePreloadOnly;
 
     public final AutonomousMode preloadMove;
 
@@ -299,28 +301,31 @@ public AutoCommands(
                 new AutonomousMode(testForward(), AllianceFlip.flipForPP(getInitial(s15_straight_forward)));
         this.doNothing = 
                 new AutonomousMode(Commands.none(), AllianceFlip.flipForPP(getInitial(s15_straight_forward), color == Alliance.Red));
-        this.preloadOnly = 
-                new AutonomousMode(preloadOnly(), AllianceFlip.flipForPP(getInitial(s15_straight_forward), color == Alliance.Red));
+        this.redPreloadOnly = 
+                new AutonomousMode(preloadOnly(), AllianceFlip.flipForPP(getInitial(s1_to_n1)), "red preload only");
+        this.bluePreloadOnly = 
+                new AutonomousMode(preloadOnly(), getInitial(s1_to_n1), "blue preload only");
         this.preloadMove = 
                 new AutonomousMode(preloadMove(), AllianceFlip.flipForPP(getInitial(s3_preload_move), color == Alliance.Red));
 
 
         redAutoModes =
                 new ArrayList<AutonomousMode>(
                         Set.of(
+                                redPreloadOnly,
                                 redFullCenterS1,
                                 redFullCenterS3,
                                 redFour_S1F1F2F3,
                                 redFour_S1N1N2N3,
                                 redFour_S3F5F4F3,
                                 redSix_S1F1F2N1N2N3,
                                 redTwo_S2F3,
-                                doNothing,
-                                preloadOnly));
+                                doNothing));
 
         blueAutoModes =
                 new ArrayList<AutonomousMode>(
                         Set.of(
+                                bluePreloadOnly,
                                 blueFour_S1F1F2F3,
                                 blueFour_S1N1N2N3,
                                 blueFour_S3F5F4F3,
@@ -329,8 +334,7 @@ public AutoCommands(
                                 blueSix_S1F1F2N1N2N3,
                                 blueForwardTest,
                                 blueRightTest,
-                                doNothing,
-                                preloadOnly));
+                                doNothing));
 
     }
 
@@ -383,49 +387,57 @@ public Command seven_S1N1F1F2F3N2N3(Alliance color) {
     public Command getScoringSequenceF1F2(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverride(f1_to_shoot1, color),
+                target().withTimeout(0.5),
                 followChoreoPathWithOverrideFast(shoot1_to_f2, color));
     }
 
     public Command getScoringSequenceF2F3(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverride(f2_to_shoot1, color),
+                target().withTimeout(0.8),
                 followChoreoPathWithOverrideFast(shoot1_to_f3, color));
     }
 
     public Command getScoringSequenceF3F4(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverride(f3_to_shoot2, color),
+                target().withTimeout(0.8),
                 followChoreoPathWithOverrideFast(shoot2_to_f4, color));
     }
 
     public Command getScoringSequenceF4F5(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverrideLongTimer(f4_to_shoot2, color),
+                target().withTimeout(0.8),
                 followChoreoPathWithOverrideFast(shoot2_to_f5, color));
     }
 
     public Command getScoringSequenceF2F1(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverrideLongTimer(f2_to_shoot3, color),
+                target().withTimeout(0.8),
                 followChoreoPathWithOverrideFast(shoot3_to_f1, color));
     }
 
     public Command getScoringSequenceF3F2(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverride(f3_to_shoot3, color),
+                target().withTimeout(0.8),
                 followChoreoPathWithOverrideFast(shoot3_to_f2, color));
     }
 
     public Command getScoringSequenceF4F3(Alliance color) {
         return Commands.sequence(
-                followChoreoPathWithOverride(f4_to_shoot3, color),
+                followChoreoPathWithOverrideLongTimer(f4_to_shoot3, color),
+                target().withTimeout(0.8),
                 followChoreoPathWithOverrideFast(shoot3_to_f3, color));
     }
 
     public Command getScoringSequenceF5F4(Alliance color) {
         return Commands.sequence(
                 followChoreoPathWithOverride(f5_to_shoot3, color),
-                followChoreoPathWithOverrideFast(shoot3_to_f4, color));
+                target().withTimeout(0.8),
+                followChoreoPathWithOverrideNoverrideFast(shoot3_to_f4, color));
     }
 
     public Command testForward() {
@@ -452,7 +464,10 @@ public Command fullCenterS1(Alliance color) {
     }
 
     public Command preloadOnly(){
-        return scorePreload();
+        return Commands.parallel(
+            target().withTimeout(10),
+            scorePreload()
+        );
     }
 
     public Command preloadMove(){
@@ -547,8 +562,11 @@ public Command four_S1N1N2N3(Alliance color) {
                 masterSuperstructureSequence(color),
                 Commands.sequence(
                         followChoreoPathWithOverride(s1_to_n1, color),
+                        target().withTimeout(1),
                         followChoreoPathWithOverride(n1_to_n2, color),
-                        followChoreoPathWithOverride(n2_to_n3, color)));
+                        target().withTimeout(1),
+                        followChoreoPathWithOverride(n2_to_n3, color),
+                        target().withTimeout(1)));
     }
 
     public Command pathToTrapTest() {
@@ -684,7 +702,7 @@ public Command pathfindToTrap() {
 
     public Command masterSuperstructureSequence(Alliance color) {
         return Commands.sequence(
-                scorePreload(),
+                poopPreload(),
                 Commands.parallel(
                         // superstructure.spinUp(),
                         superstructure.prep(),
@@ -702,14 +720,22 @@ public boolean goodToGo(Alliance color) {
         }
     }
 
-    public Command scorePreload() {
+    public Command poopPreload() {
         return Commands.parallel(
                         superstructure.spinUpPreload(),
                         superstructure.prep(),
                         Commands.sequence(Commands.waitSeconds(1), superstructure.feed()))
                 .withTimeout(1.4);
     }
 
+    public Command scorePreload(){
+        return Commands.parallel(
+                        superstructure.spinUp(),
+                        superstructure.prep(),
+                        Commands.sequence(Commands.waitSeconds(1), superstructure.feed()))
+                .withTimeout(1.4);      
+    }
+
     public Pose2d getInitial(String path) {
         ChoreoTrajectory traj = Choreo.getTrajectory(path);
         return traj.getInitialPose();
@@ -890,7 +916,7 @@ public Command followChoreoPathWithOverride(String path, Alliance color) {
                 .andThen(Commands.runOnce(() -> swerve.drive(0, 0, 0), swerve));
     }
 
-    public Command followChoreoPathWithOverrideNoverride(String path, Alliance color) {
+    public Command followChoreoPathWithOverrideNoverrideFast(String path, Alliance color) {
         ChoreoTrajectory traj = Choreo.getTrajectory(path);
         boolean mirror = color == Alliance.Red;
         PathPlannerLogging.logActivePath(PathPlannerPath.fromChoreoTrajectory(path));
@@ -911,13 +937,15 @@ public Command followChoreoPathWithOverrideNoverride(String path, Alliance color
                                     var posexthresholdlow = color == Alliance.Blue ? 5 : FieldConstants.kFieldLength - 8.75;
                                     var posexThresholdHigh = color == Alliance.Blue ? 8.75 : FieldConstants.kFieldLength - 5;
 
-                                    if(!this.hasPiece && hasTarget.getAsBoolean()){
+                                    var isInPose = swerve.getOdoPose().getX() > posexthresholdlow && swerve.getOdoPose().getX() < posexThresholdHigh;
+
+                                    if(!this.hasPiece && hasTarget.getAsBoolean() && isInPose){
                                         motionXComponent = autoDriveVelocities.get().dx;
                                         motionYComponent = autoDriveVelocities.get().dy;
                                         motionRotComponent = autoDriveVelocities.get().dtheta;
                                     }
 
-                                    if(swerve.getOdoPose().getX() > posexthresholdlow && swerve.getOdoPose().getX() < posexThresholdHigh){
+                                    if(isInPose){
                                         motionRotComponent = speeds.omegaRadiansPerSecond;
                                     }
                                     swerve.drive(motionXComponent, motionYComponent, motionRotComponent);},

src/main/java/team3647/frc2024/commands/KickerCommands.java

@@ -9,11 +9,11 @@
 public class KickerCommands {
 
     public Command kick() {
-        return Commands.run(() -> kicker.openLoop(0.8), kicker);
+        return Commands.run(() -> kicker.openLoop(1), kicker);
     }
 
     public Command fastKick() {
-        return Commands.run(() -> kicker.openLoop(0.8), kicker);
+        return Commands.run(() -> kicker.openLoop(1), kicker);
     }
 
     public Command slowFeed() {

src/main/java/team3647/frc2024/constants/PivotConstants.java

@@ -104,7 +104,9 @@ public class PivotConstants {
         kMasterSpeakerMap.put(2.0, 49.0); //
         kMasterSpeakerMap.put(2.5, 45.0); //
         kMasterSpeakerMap.put(3.0, 35.0); //
+        kMasterSpeakerMap.put(3.343, 32.946 + 0.4);
         kMasterSpeakerMap.put(3.5, 31.7); //
+        kMasterSpeakerMap.put(3.93, 30.309 + 0.2);
         kMasterSpeakerMap.put(4.0, 30.0); //
         kMasterSpeakerMap.put(4.5, 27.9); //
         kMasterSpeakerMap.put(5.0, 26.5); //

src/main/java/team3647/frc2024/constants/ShooterConstants.java

@@ -98,9 +98,9 @@ public class ShooterConstants {
         kRightMap.put(20.0, 18.0);
 
         kFeedMap.put(0.0, 3.0);
-        kFeedMap.put(8.0, 3.5);
-        kFeedMap.put(12.0, 4.0);
-        kFeedMap.put(20.0, 4.0);
+        kFeedMap.put(8.0, 4.0);
+        kFeedMap.put(12.0, 4.4);
+        kFeedMap.put(20.0, 4.4);
 
         Slot0Configs kRightSlot0 = new Slot0Configs();
         Slot0Configs kLeftSlot0 = new Slot0Configs();

src/main/java/team3647/frc2024/constants/TunerConstants.java

@@ -50,7 +50,7 @@ public class TunerConstants {
 
     public static final double kDriveGearRatio = 5.357142857142857;
     public static final double kSteerGearRatio = 21.428571428571427;
-    public static final double kWheelRadiusInches = 1.925;
+    public static final double kWheelRadiusInches = 1.953;
 
     public static final boolean kSteerMotorReversed = true;
     public static final boolean kInvertLeftSide = false;

src/main/java/team3647/frc2024/robot/RobotContainer.java

@@ -247,6 +247,12 @@ private void configureButtonBindings() {
         climbing.onTrue(superstructure.setIsClimbing());
         climbing.onFalse(superstructure.setIsNotClimbing());
 
+        coController.leftJoyStickPress.and(coController.rightJoyStickPress)
+            .onTrue(superstructure.stowChurro());
+
+        coController.leftTrigger.and(coController.rightTrigger)
+            .onTrue(superstructure.stowAll());
+
         // characterization
 
         // swerve
@@ -295,7 +301,7 @@ private void configureDefaultCommands() {
         pivot.setDefaultCommand(superstructure.prep());
         intake.setDefaultCommand(superstructure.intakeCommands.kill());
         kicker.setDefaultCommand(superstructure.kickerCommands.kill());
-        churro.setDefaultCommand(superstructure.churroCommands.holdPositionAtCall());
+        churro.setDefaultCommand(superstructure.stowChurro());
         wrist.setDefaultCommand(superstructure.wristCommands.holdPositionAtCall());
         shooterRight.setDefaultCommand(superstructure.shooterCommands.killRight());
         shooterLeft.setDefaultCommand(superstructure.shooterCommands.killLeft());

src/main/java/team3647/frc2024/subsystems/Superstructure.java

@@ -165,11 +165,11 @@ public Command stowChurro() {
     }
 
     public Command spinUpAmp() {
-        return shooterCommands.setVelocity(() -> 4, () -> 1);
+        return shooterCommands.setVelocity(() -> 0.0005, () -> 1);
     }
 
     public Command setShootModeStationary() {
-        return Commands.runOnce(() -> this.wantedShootingMode = DriveMode.SHOOT_STATIONARY);
+        return Commands.runOnce(() -> this.wantedShootingMode= DriveMode.SHOOT_STATIONARY);
     }
 
     public Command setShootModeMoving() {
@@ -204,6 +204,21 @@ public boolean getPiece() {
         return hasPiece;
     }
 
+    public Command stowAll(){
+        return Commands.parallel(
+             wristCommands
+                        .setAngle(wristStowAngle)
+                        .until(() -> wrist.angleReached(wristStowAngle, 5)),
+                Commands.parallel(
+                                pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()),
+                                shooterCommands.kill(),
+                                kickerCommands.kill(),
+                                intakeCommands.kill(),
+                                stowChurro())
+
+        );
+    }
+
     public boolean currentYes() {
         return intake.getMasterCurrent() > currentLimit && wrist.getAngle() < 5; // 41
     }
@@ -394,11 +409,13 @@ public Command stowNoShoot() {
     public Command stowFromAmpShoot() {
         return Commands.sequence(
                 Commands.parallel(
+                                deployChurro(),
                                 prepAmp(),
                                 spinUpAmp(),
                                 Commands.sequence(Commands.waitSeconds(0.3), feed()))
                         .withTimeout(1),
                 Commands.deadline(
+                        stowChurro(),
                         pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()),
                         shooterCommands.kill(),
                         kickerCommands.kill()));

src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java

@@ -111,10 +111,10 @@ public Optional<VisionMeasurement> QueueToInputs() {
             return Optional.empty();
         }
 
-        if (result.getBestTarget().getFiducialId() == 5
-                || result.getBestTarget().getFiducialId() == 6) {
-            return Optional.empty();
-        }
+        // if (result.getBestTarget().getFiducialId() == 5
+        //         || result.getBestTarget().getFiducialId() == 6) {
+        //     return Optional.empty();
+        // }
         // Logger.recordOutput(
         //         "Cams/" + this.getName(), update.get().estimatedPose.transformBy(robotToCam));
         double numTargets = result.getTargets().size();
@@ -124,8 +124,8 @@ public Optional<VisionMeasurement> QueueToInputs() {
         final var stdDevs = baseStdDevs.times(targetDistance).times(8 / Math.pow(numTargets, 3));
         double ambiguityScore =
                 1 / (numTargets * 100 + (1 - result.getBestTarget().getPoseAmbiguity()));
-        // final double priorityScore = this.hasPriority ? 50 : 0;
-        // ambiguityScore += priorityScore;
+        final double priorityScore = this.hasPriority ? 50 : 0;
+        ambiguityScore += priorityScore;
         if (result.targets.stream().anyMatch(target -> target.getPoseAmbiguity() > 0.2)) {
             return Optional.empty();
         }

src/main/java/team3647/frc2024/util/RobotTracker.java

@@ -45,7 +45,7 @@ public RobotTracker(
             Supplier<Pose2d> drivePose,
             Supplier<ChassisSpeeds> driveSpeeds,
             InterpolatingDoubleTreeMap shootSpeedMap) {
-        this.color = Alliance.Blue;
+        this.color = Alliance.Red;
 
 
 

src/main/java/team3647/frc2024/util/TargetingUtil.java

@@ -258,15 +258,15 @@ public Pose2d pose() {
     }
 
     public double rotToAmp() {
-        if (robotTracker.getPose().getRotation().getRadians() > Math.PI / 2) {
+        if (robotTracker.getPose().getRotation().getRadians() >= Math.PI / 2) {
             return -Math.PI / 2 - (robotTracker.getPose().getRotation().getRadians() - 2 * Math.PI);
         } else {
             return -Math.PI / 2 - robotTracker.getPose().getRotation().getRadians();
         }
     }
 
     public double rotToOther90() {
-        if (robotTracker.getPose().getRotation().getRadians() < -Math.PI / 2) {
+        if (robotTracker.getPose().getRotation().getRadians() <= -Math.PI / 2) {
             return Math.PI / 2 - (robotTracker.getPose().getRotation().getRadians() + 2 * Math.PI);
         } else {
             return Math.PI / 2 - robotTracker.getPose().getRotation().getRadians();

src/main/java/team3647/lib/team9442/AutoChooser.java

@@ -32,7 +32,7 @@ public AutoChooser(AutoCommands commands, Consumer<Pose2d> setStartPose) {
                     SmartDashboard.putString("sellected", getSelected().getName());
                 });
         autosList = autoCommands.redAutoModes;
-        setDefaultOption("rDEFAULT AUTO CHANE THIS red full center", autosList.get(0));
+        setDefaultOption("DEFAULT AUTO CHANE THIS red preload", autosList.get(0));
     }
 
     @Override