Merge branch 'AutoDev' of https://github.com/FRC1257/2024-Robot into AutoDev

Author: Swordman51

src/main/java/frc/robot/Constants.java

@@ -76,15 +76,6 @@ public static Mode getRobotMode() {
     return Mode.REAL;
   }
 
-  public static class BuildConstants {
-    public static int DIRTY = 1;
-    public static String MAVEN_NAME = "Snail";
-    public static String BUILD_DATE = "12/57";
-    public static String GIT_SHA = "Snail";
-    public static String GIT_DATE = "Snail";
-    public static String GIT_BRANCH = "PivotArm";
-  }
-
   public static final class NeoMotorConstants {
     public static final double kFreeSpeedRpm = 5676;
   }
@@ -141,15 +132,4 @@ public static class ElectricalLayout {
     {4.258293028	,0,   57},
     {5	          ,0,   60}
   };
-
-
-  /* public static final double[][] LookupTable = {
-    {0, 0, 38},
-    {1.22, 0, 42},
-    {1.45, 0, 38}, 
-    {3.49, 0, 53}
-    {6, 0, 55}
-  }; */
-
-
 }

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

@@ -13,7 +13,6 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.Constants.BuildConstants;
 
 /**
  * The VM is configured to automatically run this class, and to call the

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

@@ -77,13 +77,13 @@ public class RobotContainer {
   // Dashboard inputs
   private final LoggedDashboardChooser<Command> autoChooser;
 
+  // crete mode changers
   private LoggedDashboardNumber autoWait = new LoggedDashboardNumber("AutoWait", 0);
   private LoggedDashboardNumber rightShooterVolts = new LoggedDashboardNumber("RightShooter", ShooterConstants.SHOOTER_FULL_VOLTAGE);
   private LoggedDashboardNumber leftShooterVolts = new LoggedDashboardNumber("LeftShooter", ShooterConstants.SHOOTER_FULL_VOLTAGE);
-  private LoggedDashboardBoolean TurboMode = new LoggedDashboardBoolean("Turbo Mode",false);
-  private LoggedDashboardBoolean BrakeMode = new LoggedDashboardBoolean("Brake Mode", true);
-  private LoggedDashboardBoolean SetStartPosition = new LoggedDashboardBoolean("Set Start Position", false);
-  private LoggedDashboardBoolean ShootSide = new LoggedDashboardBoolean("ShootSide", false);
+  private LoggedDashboardBoolean brakeModeDashboard = new LoggedDashboardBoolean("Brake Mode", true);
+  private LoggedDashboardBoolean setStartPosition = new LoggedDashboardBoolean("Set Start Position", false);
+
   // Field
   private final Field2d field;
 
@@ -192,7 +192,7 @@ public RobotContainer() {
     System.out.println("[Init] Setting up Named Commands");
 
     NamedCommands.registerCommand("Shoot", shootSpeaker().andThen(zeroPosition()));
-    NamedCommands.registerCommand("ShootSide", shootSpeakerSide().andThen(zeroPosition()));
+    NamedCommands.registerCommand("shootSide", shootSpeakerSide().andThen(zeroPosition()));
     NamedCommands.registerCommand("ShootAnywhere", shootAnywhereAuto());
 
     NamedCommands.registerCommand("Intake",
@@ -203,6 +203,7 @@ public RobotContainer() {
     NamedCommands.registerCommand("ZeroPivot", pivot.bringDownCommand());
 
     NamedCommands.registerCommand("PrepShot", rotateArmSpeaker());
+    // TODO clean these up later
     NamedCommands.registerCommand("PrepShootAnywhere", rotateArmtoSpeakerForever()
                                                               .alongWith(shooter.runVoltageBoth(rightShooterVolts::get, leftShooterVolts::get)));
     NamedCommands.registerCommand("PrepPass", shooter.runVoltage(ShooterConstants.SHOOTER_UNJAM_VOLTAGE).alongWith(indexer.manualCommand(IndexerConstants.INDEXER_OUT_VOLTAGE)).withTimeout(0.1)
@@ -237,12 +238,11 @@ public RobotContainer() {
 
   }
 
-
+  // zero gyro
   public void reset() {
     drive.resetYaw();
   }
 
-
   /**
    * Use this method to define your button->command mappings. Buttons can be
    * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -344,12 +344,12 @@ private void configureButtonBindings() {
                                         .deadlineWith(shooter.runVoltage(ShooterConstants.SHOOTER_FULL_VOLTAGE))
                                         .deadlineWith(indexer.manualCommand(IndexerConstants.INDEXER_IN_VOLTAGE)));
 
+    // add controller rumble triggers
     new Trigger(() -> (int) Timer.getMatchTime() == 30.0).onTrue(getRumbleDriver());
     new Trigger(indexer::isIntaked).onTrue(getRumbleOperator());
     new Trigger(this::isAimedAtSpeaker).onTrue(getRumbleDriver());
-
 
-
+    // add extra commands in tuning mode
     if (Constants.tuningMode) {
       SmartDashboard.putData("Pivot Sysid", 
         new SequentialCommandGroup(
@@ -360,11 +360,10 @@ private void configureButtonBindings() {
         )
       );
     }
-
-    
   }
 
   public void setPivotPose3d() {
+    // log the component positions for viewing in advantage scope
     Logger.recordOutput("PivotPoseThing",
         new Pose3d(
             new Translation3d(0, 0, 0.28),
@@ -386,9 +385,11 @@ public void resetRobotPose(Pose2d pose) {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
+    // make sure choosers are configured for correct alliance poses
     AutoChooser.setupChoosers();
     drive.updateDeadzoneChooser();
 
+    // if we are running a custom auto create one
     if (autoChooser.getSendableChooser().getSelected().equals("Custom")) {
       Command custom = MakeAutos.makeAutoCommand(
         drive,
@@ -399,17 +400,20 @@ public Command getAutonomousCommand() {
         indexer::isIntaked
       );
 
+      // add auto wait if needed
       if (autoWait.get() > 0) {
         return new WaitCommand(autoWait.get()).andThen(custom);
       }
 
       return custom;
     }
 
+    // add auto wait if needed
     if (autoWait.get() > 0) {
       return new WaitCommand(autoWait.get()).andThen(autoChooser.get());
     }
 
+    // get path planner auto
     return autoChooser.get();    
   }
 
@@ -427,6 +431,9 @@ public Command joystickAmpPoint() {
     );
   }
 
+  /* 
+   * Compound Commands
+   */
   public Command zeroPosition() {
     return pivot.bringDownCommand()
         .deadlineWith(
@@ -462,14 +469,6 @@ public Command altShootAnywhere() {
             DRIVE_STRAFE)); 
   }
 
-  public boolean isPointedAtSpeaker() {
-    return DriveCommands.pointedAtSpeaker(drive);
-  }
-
-  public boolean isAimedAtSpeaker() {
-    return DriveCommands.pointedAtSpeaker(drive) && pivot.atSetpoint();
-  }
-
   public Command shootAnywhere() {
     return (new WaitUntilCommand(this::isPointedAtSpeaker).andThen(shootNote()))
               .deadlineWith(lockOnSpeakerFull());
@@ -545,13 +544,9 @@ public Command shootNote() {
         .withTimeout(1.9));//.alongWith(new InstantCommand(() -> {NoteVisualizer.shoot().schedule();})); // run the visualizer
   }
 
-  /* // Brings the note forward and back for 0.5 seconds each to center it
-  public Command intakeShimmyCommand() {
-    return (indexer.manualCommand(IndexerConstants.INDEXER_IN_VOLTAGE)
-      .alongWith(groundIntake.manualCommand(GroundIntakeConstants.GROUND_INTAKE_IN_VOLTAGE)))
-      .withTimeout(ShooterConstants.SHOOTER_SPINUP_TIME)
-      .andThen(indexer.manualCommand(IndexerConstants.INDEXER_OUT_VOLTAGE).withTimeout(ShooterConstants.SHOOTER_SPINUP_TIME));
-  } */
+  public Command intakeUntilIntaked(){
+    return indexer.IntakeLoopCommand(IndexerConstants.INDEXER_IN_VOLTAGE_WEAK).deadlineWith(groundIntake.manualCommand(GroundIntakeConstants.GROUND_INTAKE_IN_VOLTAGE)).deadlineWith(shooter.runVoltage(0));
+  }
 
   // Returns the estimated transformation over the next tick (The change in
   // position)
@@ -573,20 +568,30 @@ private double getEstimatedDistance() {
     return targetTransform.getTranslation().getNorm();
   }
 
-  // Gets RPM based on distance from speaker, taking into account the actual
-  // shooting position
-  /* private double getRPM() {
-    return Lookup.getRPM(getEstimatedDistance());
-  } */
+  @AutoLogOutput(key = "PointedAtSpeaker")
+  public boolean isPointedAtSpeaker() {
+    return DriveCommands.pointedAtSpeaker(drive);
+  }
+
+  @AutoLogOutput(key = "AimedAtSpeaker")
+  public boolean isAimedAtSpeaker() {
+    return DriveCommands.pointedAtSpeaker(drive) && pivot.atSetpoint();
+  }
+
 
   // Gets angle based on distance from speaker, taking into account the actual
   // shooting position
+  @AutoLogOutput(key = "ShootAnywhereAngle")
   private double getAngle() {
     double angle = Lookup.getAngle(getEstimatedDistance());
     Logger.recordOutput("ShootAnywhereAngle", angle);
     return angle;
   }
 
+  /* 
+   * Other Periodic Methods
+   */
+
   public void LEDPeriodic() {
     BlinkinLEDController.isEndgame = DriverStation.getMatchTime() <= 30;
     BlinkinLEDController.isEnabled = DriverStation.isEnabled();
@@ -598,26 +603,22 @@ public void LEDPeriodic() {
 
   public void disabledPeriodic() {
     LEDPeriodic();
-    if (BrakeMode.get() != brakeMode) {
+    if (brakeModeDashboard.get() != brakeMode) {
       brakeMode = !brakeMode;
       pivot.setBrake(brakeMode);
     }
 
-    if (SetStartPosition.get()) {
+    if (setStartPosition.get()) {
       AutoChooser.setupChoosers();
       drive.updateDeadzoneChooser();
       resetRobotPose(AutoChooser.getStartPose());
-      SetStartPosition.set(false);
+      setStartPosition.set(false);
     }
 
     setPivotPose3d();
     field.setRobotPose(drive.getPose());
     Logger.recordOutput("DriveAimed", DriveCommands.pointedAtSpeaker(drive));
     Logger.recordOutput("PivotAimed", pivot.atSetpoint());
   }
-
-  public Command intakeUntilIntaked(){
-    return indexer.IntakeLoopCommand(IndexerConstants.INDEXER_IN_VOLTAGE_WEAK).deadlineWith(groundIntake.manualCommand(GroundIntakeConstants.GROUND_INTAKE_IN_VOLTAGE)).deadlineWith(shooter.runVoltage(0));
-  }
 
 }

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

@@ -22,6 +22,7 @@
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
@@ -45,12 +46,10 @@ public class DriveCommands {
     private static double slowMode = 1;
     // kSlowModeConstant;
 
-    private static PIDController angleController = new PIDController(kTurnSpeakerP,
-            kTurnSpeakerI, kTurnSpeakerD);
-
-    private DriveCommands() {
-    }
+    private static PIDController angleController = new PIDController(kTurnSpeakerP, kTurnSpeakerI, kTurnSpeakerD);
+    private static LoggedDashboardBoolean shootSide = new LoggedDashboardBoolean("ShootSide", false);
 
+    
     /**
      * Field relative drive command using two joysticks (controlling linear and
      * angular velocities).
@@ -347,7 +346,7 @@ public static boolean pointedAtSpeaker(Drive drive) {
     }
 
     public static boolean getPivotSideAngle() {
-        if (SmartDashboard.getBoolean("ShootSide", false)) {
+        if (shootSide.get()) {
             return false;
         }
         return true;

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -26,6 +26,7 @@
 
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.GoalEndState;
@@ -51,7 +52,6 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -107,6 +107,8 @@ public class Drive extends SubsystemBase {
 
   private DeadzoneChooser deadzoneChooser = new DeadzoneChooser("Deadzone");
 
+  private LoggedDashboardBoolean useVisionDashboard = new LoggedDashboardBoolean("UseVision", true);
+
   public Drive(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
@@ -174,7 +176,7 @@ public Drive(
 
             }, null, this));
 
-    SmartDashboard.putBoolean("UseVision", useVision);
+    useVisionDashboard.set(useVision);
   }
 
   public void periodic() {
@@ -186,7 +188,7 @@ public void periodic() {
     odometryLock.unlock();
     Logger.processInputs("Drive/Gyro", gyroInputs);
 
-    if (SmartDashboard.getBoolean("UseVision", useVision)) {
+    if (useVisionDashboard.get()) {
       visionIO.updateInputs(visionInputs, getPose());
       Logger.processInputs("Vision", visionInputs);
       if (visionInputs.hasEstimate) {
@@ -448,36 +450,4 @@ public Command pathfindToTrajectory(PathPlannerPath path) {
     return AutoBuilder.pathfindThenFollowPath(path, kPathConstraints);
   }
 
-  public Command goToThaPose(Pose2d endPose) {
-    List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
-      getPose(),
-      endPose
-    );
-
-    // Create the path using the bezier points created above
-    PathPlannerPath path = new PathPlannerPath(
-      bezierPoints,
-      kPathConstraints, // The constraints for this path. If using a differential drivetrain, the angular constraints have no effect.
-      new GoalEndState(0.0, Rotation2d.fromDegrees(-90)) // Goal end state. You can set a holonomic rotation here. If using a differential drivetrain, the rotation will have no effect.
-    );
-    return AutoBuilder.followPath(path);
-  }
-  
-  public Translation2d FindMidPose(Pose2d start, Pose2d end){
-    return new Translation2d((start.getX()+end.getX())/2, (start.getY()+end.getY())/2);
-  }
-
-  public Command driveFromPoseToPose(Pose2d start, Pose2d end) {
-    return AutoBuilder.followPath(
-      PathPlannerPath.fromPathPoints(
-        List.of(
-          new PathPoint(start.getTranslation(), new RotationTarget(0, start.getRotation(), true)),
-          new PathPoint(FindMidPose(start, end), new RotationTarget(0, start.getRotation(), true)),
-          new PathPoint(FindMidPose(new Pose2d(FindMidPose(start, end), new Rotation2d()), end), new RotationTarget(0, start.getRotation(), true)),
-          new PathPoint(end.getTranslation(), new RotationTarget(0, end.getRotation(), true))
-        ), 
-        kPathConstraints, 
-        new GoalEndState(0, end.getRotation())));
-  }
-
 }

src/main/java/frc/robot/subsystems/drive/GyroIOReal.java

@@ -212,20 +212,6 @@ public boolean navXConnected() {
         return navx.isConnected();
     }
 
-    /**
-     * Displays the angles on {@code SmartDashboard}.
-     */
-    public void outputValues() {
-        SmartDashboard.putNumber("/Gyro/Yaw Angle", getYawAngle());
-        SmartDashboard.putNumber("/Gyro/Roll Angle", getRollAngle());
-        SmartDashboard.putNumber("/Gyro/Pitch Angle", getPitchAngle());
-
-        SmartDashboard.putNumber("Robot Angle", getRobotAngle());
-        SmartDashboard.putNumber("Robot Angle Vel", getRobotAngleVelocity());
-        // SmartDashboard.putNumber("Tilt Angle", getTiltAngle());
-        SmartDashboard.putBoolean("Gyro Connected", navXConnected());
-    }
-
     public static GyroIOReal getInstance() {
         if (instance == null) {
             instance = new GyroIOReal();