Event gle

src/main/java/frc/robot/Autos.java

@@ -75,13 +75,15 @@ public enum Action {
     DEPOT,
     NOTHING,
     CLIMB_ONLY,
-    SCORE_AT_END;
+    SCORE_AT_END,
+    OUTPOST_NO_SCORE;
   }
 
   public enum Path {
     // OUTPOST
     RTrenchtoOutpost("RTrenchtoOutpost", Action.OUTPOST),
     RPreTrenchReversedtoOutpost("RPreTrenchReversedtoOutpost", Action.OUTPOST),
+    NoScoreRPreTrenchReversedtoOutpost("RPreTrenchReversedtoOutpost", Action.OUTPOST_NO_SCORE),
 
     PreOutposttoOutpost("PreOutposttoOutpost", Action.OUTPOST),
 
@@ -98,7 +100,7 @@ public enum Path {
     EndWScoreLNeutraltoLPreTrench("LNeutraltoLPreTrench", Action.SCORE_AT_END),
     EndWScoreLCleanuptoLPreTrench("LCleanuptoLPreTrench", Action.SCORE_AT_END),
 
-    RNeutraltoRPreTrenchReversed("RNeutraltoRPreTrenchReversed", Action.INTAKE),
+    RNeutraltoRPreTrenchReversed("RNeutraltoRPreTrenchReversed", Action.SCORE_AT_END),
 
     LPreTrenchtoLNeutral("LPreTrenchtoLNeutral", Action.INTAKE),
     LPreTrenchtoLCleanup("LPreTrenchtoLCleanup", Action.INTAKE),
@@ -118,7 +120,7 @@ public enum Path {
     OutposttoRPreTrench("OutposttoRPreTrench", Action.NOTHING),
     DepottoPreOutpost("DepottoPreOutpost", Action.DELAYED_SCORE),
     OutposttoPreDepot("OutposttoPreDepot", Action.NOTHING),
-    OutposttoPreOutpost("OutposttoPreOutpost", Action.SCORE),
+    OutposttoPreOutpost("OutposttoPreOutpost", Action.SCORE_AT_END),
     HubtoCenter("HubtoCenter", Action.SCORE),
 
     DepottoPreDepot("DepottoPreDepot", Action.SCORE_AT_END),
@@ -210,6 +212,8 @@ public Command runPath(Path path, AutoRoutine routine) {
         return scoreAtEndPath(path, routine);
       case NOTHING:
         return emptyPath(path, routine);
+      case OUTPOST_NO_SCORE:
+        return outpostNoScorePath(path, routine);
       default: // this should never happen
         return Commands.none();
     }
@@ -360,6 +364,33 @@ public Command outpostPath(Path path, AutoRoutine routine) {
         swerve.stopForTime(() -> 4)); // TODO tune time
   }
 
+  public Command outpostNoScorePath(Path path, AutoRoutine routine) {
+    return Commands.sequence(
+        stopScoring(),
+        stopFeeding(),
+        stopFlowing(),
+        stopIntaking(),
+        // holy chopped
+        // this is here because i suspect the time based at end will cause it to stop early if it
+        // hits something and messes up the time
+        // though this may be fixed by having a better path?? idk
+        path.getTrajectory(routine)
+            .cmd()
+            .until(
+                path.getTrajectory(routine)
+                    .atPose(
+                        path.getTrajectory(routine)
+                            .getFinalPose()
+                            .orElse(
+                                DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+                                    ? BLUE_OUTPOST
+                                    : RED_OUTPOST),
+                        0.25,
+                        Units.degreesToRadians(30))),
+        // TODO tune tolerance
+        swerve.stopForTime(() -> 4)); // TODO tune time
+  }
+
   public Command depotPath(Path path, AutoRoutine routine) {
     return Commands.sequence(
         stopScoring(),
@@ -671,19 +702,20 @@ public Command getRightNeutralOutpostScore() {
         new Path[] {
           Path.StartingRTrenchtoRNeutral,
           Path.RNeutraltoRPreTrenchReversed,
-          Path.RPreTrenchReversedtoOutpost
+          Path.NoScoreRPreTrenchReversedtoOutpost,
+          Path.OutposttoPreOutpost
         },
         setRightClimb());
   }
 
-  public Command getLeftNeutralOutpostScore() {
-    return createAuto(
-        "Left Neutral Outpost Score",
-        new Path[] {
-          Path.StartingLTrenchtoLNeutral, Path.LNeutraltoLPreTrench, Path.LPreTrenchtoDepot
-        },
-        setLeftClimb());
-  }
+  // public Command getLeftNeutralOutpostScore() {
+  //   return createAuto(
+  //       "Left Neutral Outpost Score",
+  //       new Path[] {
+  //         Path.StartingLTrenchtoLNeutral, Path.LNeutraltoLPreTrench, Path.LPreTrenchtoDepot
+  //       },
+  //       setLeftClimb());
+  // }
 
   public Command getLeftNeutralScoreTwice() {
     return createAuto(

src/main/java/frc/robot/Robot.java

@@ -72,6 +72,7 @@
 import frc.robot.utils.FieldUtils.FeedTargets;
 import frc.robot.utils.FieldUtils.TrenchPoses;
 import frc.robot.utils.FuelSim;
+import frc.robot.utils.autoaim.AutoAim;
 import frc.robot.utils.autoaim.NewAutoAim;
 import java.io.File;
 import java.util.Arrays;
@@ -151,7 +152,7 @@ public enum RobotEdition {
    * This is for when we're testing shot and extension numbers and should be FALSE once bring up is
    * complete
    */
-  public static final boolean TUNING_MODE = true;
+  public static final boolean TUNING_MODE = false;
 
   public boolean hasZeroedSinceStartup = false;
 
@@ -704,8 +705,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
         .rightBumper()
         .or(Autos.autoLeftClimbReq.negate())
         .onTrue(Commands.runOnce(() -> leftClimbTarget = false));
-    // I HATE THIS!
-    operator.leftStick().whileTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
     operator
         .rightStick()
         .onTrue(
@@ -768,6 +767,9 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
                 shooter::getTurretPosition,
                 () -> Superstructure.getFeedTarget()));
 
+    operator.povRight().onTrue(Commands.runOnce(() -> AutoAim.incrementFudgeFactor()));
+    operator.povLeft().onTrue(Commands.runOnce(() -> AutoAim.decrementFudgeFactor()));
+
     // create triggers for joystick disconnect alerts
     new Trigger(() -> DriverStation.isJoystickConnected(0))
         .negate()
@@ -803,7 +805,7 @@ private void addAutos() {
     autoChooser.addOption("Right Bump Outpost Center", autos.getRightBumpOutpostCenterAuto());
     autoChooser.addOption("Right Trench Double Dip Auto", autos.getDoubleDipRightTrench());
     autoChooser.addOption("Left Neutral Score Twice", autos.getLeftNeutralScoreTwice());
-    autoChooser.addOption("Left Neutral Outpost Score", autos.getLeftNeutralOutpostScore());
+    // autoChooser.addOption("Left Neutral Outpost Score", autos.getLeftNeutralOutpostScore());
     autoChooser.addOption("Hub Depot Outpost", autos.getHubDepotOutpostAuto());
     autoChooser.addOption("Hub Outpost Depot", autos.getHubOutpostDepotAuto());
 
@@ -884,6 +886,7 @@ public void robotPeriodic() {
         });
 
     updateAlerts();
+    Logger.recordOutput("Flywheel Fudge Factor", AutoAim.getFudgeFactor());
 
     // Log climb poses
     Logger.recordOutput(

src/main/java/frc/robot/Superstructure.java

@@ -406,7 +406,11 @@ private void addTransitions() {
     // Transition from any state to SPIT for anti jamming
     antiJamReq.onTrue(changeStateTo(SuperState.SPIT));
 
-    bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate());
+    bindTransition(
+        SuperState.SPIT,
+        SuperState.IDLE,
+        antiJamReq.negate(),
+        Commands.runOnce(() -> defense = false));
 
     defenseReq.onTrue(changeStateTo(SuperState.DEFENSE));
 
@@ -422,7 +426,10 @@ private void addTransitions() {
         SuperState.CLIMB,
         climbReq); // TODO maybe add transition out of climb in case we fall
     bindTransition(
-        SuperState.PRE_CLIMB, SuperState.IDLE, preClimbReq.negate().and(climbReq.negate()));
+        SuperState.PRE_CLIMB,
+        SuperState.IDLE,
+        preClimbReq.negate().and(climbReq.negate()),
+        Commands.runOnce(() -> defense = false));
 
     bindTransition(
         SuperState.SPIN_UP_SCORE_FLOW,
@@ -601,7 +608,7 @@ private void addCommands() {
     bindCommands(
         SuperState.PRE_CLIMB,
         intake.restRetracted(),
-        indexer.rest(),
+        indexer.stop(),
         shooter.rest(
             swerve::getPose,
             swerve::getVelocityRobotRelative,
@@ -611,7 +618,7 @@ private void addCommands() {
     bindCommands(
         SuperState.CLIMB,
         intake.restRetracted(),
-        indexer.rest(),
+        indexer.stop(),
         shooter.rest(
             swerve::getPose,
             swerve::getVelocityRobotRelative,
@@ -621,7 +628,7 @@ private void addCommands() {
     bindCommands(
         SuperState.POST_CLIMB,
         intake.restRetracted(),
-        indexer.rest(),
+        indexer.stop(),
         shooter.rest(
             swerve::getPose,
             swerve::getVelocityRobotRelative,
@@ -662,7 +669,7 @@ private void addCommands() {
     bindCommands(
         SuperState.DEFENSE,
         intake.restRetracted(),
-        indexer.rest(),
+        indexer.stop(),
         shooter.stopTurret(),
         climber.retract());
   }

src/main/java/frc/robot/components/pivot/PivotIO.java

@@ -82,7 +82,13 @@ public void setMotorVoltage(double voltage) {
 
   public void setMotorPositionSetpoint(Rotation2d setpoint) {
     this.setpoint = setpoint;
-    motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure()));
+    motor.setControl(motorMagicVoltage.withPosition(setpoint.getMeasure()).withFeedForward(0.0));
+  }
+
+  public void setMotorPositionSetpoint(Rotation2d setpoint, double ffVolts) {
+    this.setpoint = setpoint;
+    motor.setControl(
+        motorMagicVoltage.withPosition(setpoint.getMeasure()).withFeedForward(ffVolts));
   }
 
   public Rotation2d getSetpoint() {

src/main/java/frc/robot/subsystems/indexer/Indexer.java

@@ -5,6 +5,7 @@
 package frc.robot.subsystems.indexer;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
 /** Add your docs here. */
@@ -19,6 +20,10 @@ public interface Indexer extends Subsystem {
   /** Run both indexer and kicker towards the shooter */
   public Command kick();
 
-  /** Not running (set to 0) */
+  /** Not running (set spinner to 0 but idle kicker) */
   public Command rest();
+
+  public default Command stop() {
+    return Commands.none();
+  }
 }

src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java

@@ -67,16 +67,15 @@ public Command kick() {
     return Commands.sequence(
         this.run(
             () -> {
-              spinnerIO.setRollerVoltage(12);
-              kickerIO.setRollerVoltage(12);
-            })
-        //     .withTimeout(3),
-        // this.run(
-        //     () -> {
-        //       spinnerIO.setRollerVelocity(30);
-        //       kickerIO.setRollerVelocity(20);
-        //     })
-        );
+              //   spinnerIO.setRollerVoltage(12);
+              //   kickerIO.setRollerVoltage(12);
+              // })
+              //     .withTimeout(3),
+              // this.run(
+              //     () -> {
+              spinnerIO.setRollerVelocity(30);
+              kickerIO.setRollerVelocity(20);
+            }));
   }
 
   @Override
@@ -90,6 +89,15 @@ public Command spit() {
 
   @Override
   public Command rest() {
+    return this.run(
+        () -> {
+          spinnerIO.setRollerVoltage(0.0);
+          kickerIO.setRollerVoltage(-2);
+        });
+  }
+
+  @Override
+  public Command stop() {
     return this.run(
         () -> {
           spinnerIO.setRollerVoltage(0.0);

src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java

@@ -26,9 +26,9 @@
 
 public class SlapdownSubsystem extends SubsystemBase implements Intake {
   public static final Rotation2d PIVOT_MIN_POSITION =
-      Rotation2d.fromDegrees(-26.894531); // Rotation2d.fromRotations(-0.052002);
+      Rotation2d.fromDegrees(-28.894531); // Rotation2d.fromRotations(-0.052002);
   public static final Rotation2d PIVOT_MAX_POSITION =
-      Rotation2d.fromDegrees(122); // Not so sure abt this one...
+      Rotation2d.fromDegrees(111.445313); // 106.523438); // 115); // Not so sure abt this one...
   public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION;
   public static final Rotation2d PIVOT_RETRACTED_POSITION = PIVOT_MAX_POSITION;
   public static final double CURRENT_ZEROING_THRESHOLD = 30.0; // TODO: TUNE
@@ -113,17 +113,18 @@ public Command agitate() {
   @Override
   public Command intake() {
     return this.run(
-            () -> {
-              pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
-              rollerIO.setRollerVelocity(80);
-            })
-        .until(atExtensionTrigger)
-        .andThen(
-            this.run(
-                () -> {
-                  pivotIO.setMotorVoltage(0);
-                  rollerIO.setRollerVelocity(80);
-                }));
+        () -> {
+          pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION, -0.5);
+          rollerIO.setRollerVelocity(80);
+        })
+    // .until(atExtensionTrigger)
+    // .andThen(
+    //     this.run(
+    //         () -> {
+    //           pivotIO.setMotorVoltage(0);
+    //           rollerIO.setRollerVelocity(80);
+    //         }));
+    ;
   }
 
   @Override
@@ -138,17 +139,18 @@ public Command outtake() {
   @Override
   public Command restExtended() {
     return this.run(
-            () -> {
-              pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
-              rollerIO.setRollerVoltage(0.0);
-            })
-        .until(atExtensionTrigger)
-        .andThen(
-            this.run(
-                () -> {
-                  pivotIO.setMotorVoltage(0);
-                  rollerIO.setRollerVoltage(0);
-                }));
+        () -> {
+          pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION);
+          rollerIO.setRollerVoltage(0.0);
+        })
+    // .until(atExtensionTrigger)
+    // .andThen(
+    //     this.run(
+    //         () -> {
+    //           pivotIO.setMotorVoltage(0);
+    //           rollerIO.setRollerVoltage(0);
+    //         }));
+    ;
   }
 
   @Override
@@ -206,13 +208,13 @@ public static TalonFXConfiguration getPivotConfig() {
     config.Feedback.SensorToMechanismRatio = 1;
 
     config.Slot0.kS = 0.05;
-    config.Slot0.kV = 8.0; // Might suck\
+    config.Slot0.kV = 8.0; // Might suck
     config.Slot0.kA = 0.0;
-    config.Slot0.kG = 0.55;
+    config.Slot0.kG = 0.4;
     config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
     config.Slot0.GravityArmPositionOffset = 0.0; // Maybe need this??
-    config.Slot0.kP = 15.0;
-    config.Slot0.kD = 0.3;
+    config.Slot0.kP = 40.0;
+    config.Slot0.kD = 0.0;
 
     config.CurrentLimits.StatorCurrentLimit = 45.0; // glup
     config.CurrentLimits.StatorCurrentLimitEnable = true;