Elevator (#11)

elevator + a bit of drive code

.vscode/settings.json

@@ -1,3 +1,3 @@
 {
-    "java.configuration.updateBuildConfiguration": "interactive"
+    "java.compile.nullAnalysis.mode": "automatic"
 }

README.md

@@ -1,2 +1,4 @@
 # 2024-offseason-robot-code
 AdvantageKit code
+
+Uses a modified version of Maple-sim code.

build.gradle

@@ -45,7 +45,7 @@ java {
     targetCompatibility = JavaVersion.VERSION_17
 }
 
-def ROBOT_MAIN_CLASS = "org.littletonrobotics.frc2024.Main"
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.
@@ -116,12 +116,12 @@ compileJava.finalizedBy checkAkitInstall
 // compileJava.finalizedBy generateDriveTrajectories
 
 // Check robot type when deploying
-task(checkRobot, dependsOn: "classes", type: JavaExec) {
-    main = "org.littletonrobotics.frc2024.Constants"
-    classpath = sourceSets.main.runtimeClasspath
-}
+// task(checkRobot, dependsOn: "classes", type: JavaExec) {
+//     main = "frc.robot.Constants"
+//     classpath = sourceSets.main.runtimeClasspath
+// }
 
-deployroborio.dependsOn(checkRobot)
+// deployroborio.dependsOn(checkRobot)
 
 // Create commit with working changes on event branches
 task(eventDeploy) {
@@ -199,7 +199,7 @@ test {
 }
 
 // Simulation configuration (e.g. environment variables).
-wpi.sim.addGui()
+wpi.sim.addGui().defaultEnabled = true
 wpi.sim.addDriverstation()
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

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

@@ -1,16 +1,20 @@
 package frc.robot;
 
-import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import frc.robot.subsystems.swerve.SwerveConstants.*;
 
 public final class Constants {
+
+  public class LogPaths {
+    public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/";
+    public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/";
+    public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/";
+  }
+
   public static final class HardwareConstants {
 
     // public static RobotType getRobot() {
@@ -43,128 +47,9 @@ public static enum Mode {
 
     public static final double MIN_FALCON_DEADBAND = 0.0001;
 
-    public static final PneumaticsModuleType PNEUMATICS_MODULE_TYPE = PneumaticsModuleType.CTREPCM;
-
     public static final double DEADBAND_VALUE = 0.05;
   }
 
-  public static final class DriveConstants {
-    public static final double X_POS_TRUST = 0.03; // Meters
-    public static final double Y_POS_TRUST = 0.03; // Meters
-    public static final double ANGLE_TRUST = Units.degreesToRadians(1); // Radians
-
-    // Wheel base and track width are measured by the center of the swerve modules, not the frame of
-    // the robot
-    // Distance between centers of right and left wheels on robot
-    public static final double TRACK_WIDTH = Units.inchesToMeters(21.25);
-    // Distance between front and back wheels on robot
-    public static final double WHEEL_BASE = Units.inchesToMeters(21.25);
-
-    public static final Translation2d[] MODULE_TRANSLATIONS =
-        new Translation2d[] {
-          new Translation2d(WHEEL_BASE / 2, TRACK_WIDTH / 2), // Front Left
-          new Translation2d(WHEEL_BASE / 2, -TRACK_WIDTH / 2), // Front Right
-          new Translation2d(-WHEEL_BASE / 2, TRACK_WIDTH / 2), // Rear Left
-          new Translation2d(-WHEEL_BASE / 2, -TRACK_WIDTH / 2) // Rear Right
-        };
-
-    public static final SwerveDriveKinematics DRIVE_KINEMATICS =
-        new SwerveDriveKinematics(MODULE_TRANSLATIONS);
-
-    public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 22;
-    public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 24;
-    public static final int REAR_LEFT_DRIVE_MOTOR_ID = 23;
-    public static final int REAR_RIGHT_DRIVE_MOTOR_ID = 21;
-
-    public static final int FRONT_LEFT_TURN_MOTOR_ID = 5;
-    public static final int FRONT_RIGHT_TURN_MOTOR_ID = 6;
-    public static final int REAR_LEFT_TURN_MOTOR_ID = 8;
-    public static final int REAR_RIGHT_TURN_MOTOR_ID = 7;
-
-    public static final int FRONT_LEFT_CANCODER_ID = 14;
-    public static final int FRONT_RIGHT_CANCODER_ID = 12;
-    public static final int REAR_LEFT_CANCODER_ID = 11;
-    public static final int REAR_RIGHT_CANCODER_ID = 13;
-
-    public static final double FRONT_LEFT_ZERO_ANGLE = 0.137939453125;
-    public static final double FRONT_RIGHT_ZERO_ANGLE = -0.420654296875;
-    public static final double REAR_LEFT_ZERO_ANGLE = -0.475341796875;
-    public static final double REAR_RIGHT_ZERO_ANGLE = -0.05078125;
-
-    public static final SensorDirectionValue FRONT_LEFT_CANCODER_REVERSED =
-        SensorDirectionValue.CounterClockwise_Positive;
-    public static final SensorDirectionValue FRONT_RIGHT_CANCODER_REVERSED =
-        SensorDirectionValue.CounterClockwise_Positive;
-    public static final SensorDirectionValue REAR_LEFT_CANCODER_REVERSED =
-        SensorDirectionValue.CounterClockwise_Positive;
-    public static final SensorDirectionValue REAR_RIGHT_CANCODER_REVERSED =
-        SensorDirectionValue.CounterClockwise_Positive;
-
-    public static final InvertedValue FRONT_LEFT_TURN_MOTOR_REVERSED =
-        InvertedValue.CounterClockwise_Positive;
-    public static final InvertedValue FRONT_RIGHT_TURN_MOTOR_REVERSED =
-        InvertedValue.CounterClockwise_Positive;
-    public static final InvertedValue REAR_LEFT_TURN_MOTOR_REVERSED =
-        InvertedValue.Clockwise_Positive;
-    public static final InvertedValue REAR_RIGHT_TURN_MOTOR_REVERSED =
-        InvertedValue.Clockwise_Positive;
-
-    public static final InvertedValue FRONT_LEFT_DRIVE_ENCODER_REVERSED =
-        InvertedValue.CounterClockwise_Positive;
-    public static final InvertedValue FRONT_RIGHT_DRIVE_ENCODER_REVERSED =
-        InvertedValue.Clockwise_Positive;
-    public static final InvertedValue REAR_LEFT_DRIVE_ENCODER_REVERSED =
-        InvertedValue.Clockwise_Positive;
-    public static final InvertedValue REAR_RIGHT_DRIVE_ENCODER_REVERSED =
-        InvertedValue.CounterClockwise_Positive;
-
-    public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 20;
-    public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 5;
-
-    public static final double MAX_SPEED_METERS_PER_SECOND = 6.95;
-    public static final double MAX_SHOOT_SPEED_METERS_PER_SECOND = 3;
-
-    public static final double HEADING_ACCEPTABLE_ERROR_RADIANS = Units.degreesToRadians(2.5);
-    public static final double HEADING_ACCEPTABLE_ERROR_MOVING_RADIANS = Units.degreesToRadians(4);
-
-    public static final double Y_RATE_LIMIT = 10.0;
-    public static final double X_RATE_LIMIT = 10.0;
-    public static final double ROT_RATE_LIMIT = 10.0;
-  }
-
-  public static final class ModuleConstants {
-    public static final double DRIVE_GEAR_RATIO = 4.59;
-    public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(3.774788522800778);
-
-    public static final double WHEEL_CIRCUMFERENCE_METERS = WHEEL_DIAMETER_METERS * Math.PI;
-    public static final double DRIVE_TO_METERS = WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO;
-    public static final double DRIVE_TO_METERS_PER_SECOND =
-        WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO;
-
-    public static final double DRIVE_SUPPLY_LIMIT = 45.0;
-    public static final double DRIVE_STATOR_LIMIT = 50.0;
-
-    public static final double TURN_P = 116;
-    public static final double TURN_I = 0.0;
-    public static final double TURN_D = 0.64;
-
-    public static final double MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND = 30;
-    public static final double MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED = 24;
-
-    public static final double DRIVE_P = 0.417;
-    public static final double DRIVE_I = 0.0;
-    public static final double DRIVE_D = 0.0;
-
-    public static final double DRIVE_S = 0.16;
-    // These values were gotten using recalc, then converted to the correct units & were confirmed
-    // through testing and characterization
-    // https://www.reca.lc/drive?appliedVoltageRamp=%7B%22s%22%3A1200%2C%22u%22%3A%22V%2Fs%22%7D&batteryAmpHours=%7B%22s%22%3A18%2C%22u%22%3A%22A%2Ah%22%7D&batteryResistance=%7B%22s%22%3A0.018%2C%22u%22%3A%22Ohm%22%7D&batteryVoltageAtRest=%7B%22s%22%3A12.6%2C%22u%22%3A%22V%22%7D&efficiency=97&filtering=1&gearRatioMax=%7B%22magnitude%22%3A15%2C%22ratioType%22%3A%22Reduction%22%7D&gearRatioMin=%7B%22magnitude%22%3A3%2C%22ratioType%22%3A%22Reduction%22%7D&maxSimulationTime=%7B%22s%22%3A4%2C%22u%22%3A%22s%22%7D&maxSpeedAccelerationThreshold=%7B%22s%22%3A0.15%2C%22u%22%3A%22ft%2Fs2%22%7D&motor=%7B%22quantity%22%3A4%2C%22name%22%3A%22Kraken%20X60%2A%22%7D&motorCurrentLimit=%7B%22s%22%3A60%2C%22u%22%3A%22A%22%7D&numCyclesPerMatch=24&peakBatteryDischarge=20&ratio=%7B%22magnitude%22%3A4.59%2C%22ratioType%22%3A%22Reduction%22%7D&sprintDistance=%7B%22s%22%3A25%2C%22u%22%3A%22ft%22%7D&swerve=1&targetTimeToGoal=%7B%22s%22%3A2%2C%22u%22%3A%22s%22%7D&throttleResponseMax=0.99&throttleResponseMin=0.5&weightAuxilliary=%7B%22s%22%3A24%2C%22u%22%3A%22lbs%22%7D&weightDistributionFrontBack=0.5&weightDistributionLeftRight=0.5&weightInspected=%7B%22s%22%3A125%2C%22u%22%3A%22lbs%22%7D&wheelBaseLength=%7B%22s%22%3A27%2C%22u%22%3A%22in%22%7D&wheelBaseWidth=%7B%22s%22%3A20%2C%22u%22%3A%22in%22%7D&wheelCOFDynamic=0.9&wheelCOFLateral=1.1&wheelCOFStatic=1.1&wheelDiameter=%7B%22s%22%3A4%2C%22u%22%3A%22in%22%7D
-    public static final double DRIVE_V =
-        1.73 * WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; // = 0.1203 V*s/m
-    public static final double DRIVE_A =
-        0.32 * WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; // = 0.02225 V*s^2/m
-  }
-
   public static final class VisionConstants {
 
     public static final double VISION_X_POS_TRUST = 0.5; // meters

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

@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants.HardwareConstants.Mode;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
@@ -22,6 +23,8 @@
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
   private RobotContainer m_robotContainer;
+  private static final Mode JAVA_SIM_MODE = Mode.SIM;
+  public static final Mode CURRENT_ROBOT_MODE = isReal() ? Mode.REAL : JAVA_SIM_MODE;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -119,4 +122,7 @@ public void testInit() {
   /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {}
+
+  @Override
+  public void simulationPeriodic() {}
 }

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

@@ -6,30 +6,31 @@
 import frc.robot.Constants.HardwareConstants;
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.extras.SmarterDashboardRegistry;
+import frc.robot.subsystems.swerve.SwerveConstants;
 // import frc.robot.extras.characterization.WheelRadiusCharacterization;
 // import frc.robot.extras.characterization.WheelRadiusCharacterization.Direction;
 // import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.swerve.Drive;
-import frc.robot.subsystems.swerve.GyroIO;
-import frc.robot.subsystems.swerve.ModuleIOSim;
+import frc.robot.subsystems.swerve.SwerveDrive;
+import frc.robot.subsystems.swerve.gyroIO.GyroIONavX;
+import frc.robot.subsystems.swerve.moduleIO.ModuleIOTalonFX;
 import java.util.function.DoubleSupplier;
 
 public class RobotContainer {
 
   // private final Vision visionSubsystem;
-  private final Drive driveSubsystem;
+  private final SwerveDrive driveSubsystem;
   private final XboxController driverController = new XboxController(0);
 
   public RobotContainer() {
     SmarterDashboardRegistry.initialize();
     // visionSubsystem = new Vision();
     driveSubsystem =
-        new Drive(
-            new GyroIO() {},
-            new ModuleIOSim(),
-            new ModuleIOSim(),
-            new ModuleIOSim(),
-            new ModuleIOSim());
+        new SwerveDrive(
+            new GyroIONavX(),
+            new ModuleIOTalonFX(SwerveConstants.moduleConfigs[0]),
+            new ModuleIOTalonFX(SwerveConstants.moduleConfigs[1]),
+            new ModuleIOTalonFX(SwerveConstants.moduleConfigs[2]),
+            new ModuleIOTalonFX(SwerveConstants.moduleConfigs[3]));
   }
 
   private static double deadband(double value, double deadband) {

src/main/java/frc/robot/commands/drive/DriveCommand.java

@@ -1,16 +1,16 @@
 package frc.robot.commands.drive;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.DriveConstants;
-import frc.robot.subsystems.swerve.Drive;
+import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
+import frc.robot.subsystems.swerve.SwerveDrive;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 // import frc.robot.subsystems.vision.VisionSubsystem;
 
 public class DriveCommand extends Command {
 
-  private final Drive driveSubsystem;
+  private final SwerveDrive driveSubsystem;
 
   private final DoubleSupplier leftJoystickY, leftJoystickX, rightJoystickX;
   private final BooleanSupplier isFieldRelative, isHighRotation;
@@ -28,7 +28,7 @@ public class DriveCommand extends Command {
    * @param isHighRotation The boolean supplier for if the robot should drive with a higher rotation
    */
   public DriveCommand(
-      Drive driveSubsystem,
+      SwerveDrive driveSubsystem,
       DoubleSupplier leftJoystickY,
       DoubleSupplier leftJoystickX,
       DoubleSupplier rightJoystickX,
@@ -69,7 +69,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     // When the command ends, it stops the robot
-    driveSubsystem.drive(0, 0, 0, true);
+    // driveSubsystem.drive(0, 0, 0, true);
   }
 
   @Override

src/main/java/frc/robot/extras/Alert.java

@@ -62,7 +62,7 @@ public Alert(String group, String text, AlertType type) {
    * Sets whether the alert should currently be displayed. When activated, the alert text will also
    * be sent to the console.
    */
-  public void set(boolean active) {
+  public void setActivated(boolean active) {
     if (active && !this.active) {
       activeStartTime = Timer.getFPGATimestamp();
       switch (type) {

src/main/java/frc/robot/extras/DeviceCANBus.java

@@ -0,0 +1,22 @@
+package frc.robot.extras;
+
+import frc.robot.Constants.HardwareConstants;
+
+/** CTRE Phoenix CAN bus */
+public enum DeviceCANBus {
+  /** roboRIO CAN bus */
+  RIO(HardwareConstants.RIO_CAN_BUS_STRING),
+  /**
+   * CANivore CAN bus
+   *
+   * <p>Only a single CANivore is supported, and MUST be named "canivore"
+   */
+  CANIVORE(HardwareConstants.CANIVORE_CAN_BUS_STRING);
+
+  /** CAN bus name */
+  public final String name;
+
+  private DeviceCANBus(String name) {
+    this.name = name;
+  }
+}

src/main/java/frc/robot/extras/LimelightHelpers.java

@@ -27,7 +27,6 @@
 public class LimelightHelpers {
 
   public static class LimelightTarget_Retro {
-
     @JsonProperty("t6c_ts")
     private double[] cameraPose_TargetSpace;
 
@@ -261,7 +260,6 @@ public LimelightTarget_Detector() {}
   }
 
   public static class Results {
-
     @JsonProperty("pID")
     public double pipelineID;
 
@@ -408,6 +406,17 @@ public static class PoseEstimate {
     public double avgTagArea;
     public RawFiducial[] rawFiducials;
 
+    public PoseEstimate() {
+      this.pose = new Pose2d();
+      this.timestampSeconds = 0;
+      this.latency = 0;
+      this.tagCount = 0;
+      this.tagSpan = 0;
+      this.avgTagDist = 0;
+      this.avgTagArea = 0;
+      this.rawFiducials = new RawFiducial[] {};
+    }
+
     public PoseEstimate(
         Pose2d pose,
         double timestampSeconds,
@@ -542,6 +551,10 @@ private static void printPoseEstimate(PoseEstimate pose) {
     }
   }
 
+  public static Boolean isValidPoseEstimate(PoseEstimate pose) {
+    return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
+  }
+
   public static NetworkTable getLimelightNTTable(String tableName) {
     return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
   }