First pass through the swerve drive subsystem

Moving things around, moving constants to Constants, adding various logging
that AdvantageKit sketches usually produces.

Also, modify the ``flywheel_example`` to conform to some of the other changes
being made.

Next step: see about implementing the CTRE Pro functionality, if licensed.

	modified:   src/main/java/frc/robot/Constants.java
	modified:   src/main/java/frc/robot/RobotContainer.java
	modified:   src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
	modified:   src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java
	modified:   src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
	modified:   src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
	modified:   src/main/java/frc/robot/subsystems/vision/Vision.java

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

@@ -112,8 +112,6 @@ public static final class PhysicalConstants {
     public static final Matter kChassis =
         new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), PhysicalConstants.kRobotMass);
     public static final double kLoopTime = 0.13; // s, 20ms + 110ms sprk max velocity lag
-    public static final double kMaxSpeed = Units.feetToMeters(14.5);
-    // Maximum speed of the robot in meters per second, used to limit acceleration.
   }
 
   /** Power Distribution Module Constants ********************************** */
@@ -138,15 +136,69 @@ public static final class PowerConstants {
 
   /** Autonomous Action Constants ****************************************** */
   public static final class AutonConstants {
+
+    // Translation PID constants
     public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0);
+    // Rotation PID constants
     public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01);
   }
 
   /** Drive Base Constants ************************************************* */
   public static final class DrivebaseConstants {
 
+    // Physical size of the drive base
+    private static final double TRACK_WIDTH_X = Units.inchesToMeters(20.75);
+    private static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.75);
+    public static final double DRIVE_BASE_RADIUS =
+        Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
+
+    // Maximum chassis speeds desired for robot motion -- metric / radians
+    public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18); // ft/s
+    public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
+    // Maximum chassis accelerations desired for robot motion  -- metric / radians
+    public static final double MAX_LINEAR_ACCEL = 4.0; // m/s/s
+    public static final double MAX_ANGULAR_ACCEL = Units.degreesToRadians(720); // deg/s/s
+
+    // Wheel radius
+    public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
+
+    // ** Gear ratios for SDS MK4i L2, adjust as necessary **
+    public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+    public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
+
     // Hold time on motor brakes when disabled
     public static final double WHEEL_LOCK_TIME = 10; // seconds
+
+    // SysID characterization constants
+    public static final double kMaxV = 12.0; // Max volts
+    public static final double kDelay = 3.0; // seconds
+    public static final double kQuasiTimeout = 5.0; // seconds
+    public static final double kDynamicTimeout = 3.0; // seconds
+  }
+
+  /** Example Flywheel Mechanism Constants ********************************* */
+  public static final class FlywheelConstants {
+
+    // Mechanism motor gear ratio
+    public static final double GEAR_RATIO = 1.5;
+
+    // MODE == REAL / REPLAY
+    // Feedforward constants
+    public static final double kStaticGainReal = 0.1;
+    public static final double kVelocityGainReal = 0.05;
+    // Feedback (PID) constants
+    public static final double kPReal = 1.0;
+    public static final double kIReal = 0.0;
+    public static final double kDReal = 0.0;
+
+    // MODE == SIM
+    // Feedforward constants
+    public static final double kStaticGainSim = 0.0;
+    public static final double kVelocityGainSim = 0.03;
+    // Feedback (PID) constants
+    public static final double kPSim = 1.0;
+    public static final double kISim = 0.0;
+    public static final double kDSim = 0.0;
   }
 
   /** Operator Constants *************************************************** */

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

@@ -248,7 +248,8 @@ public static class Ports {
     /* SUBSYSTEM CAN DEVICE IDS */
     // This is where mechanism subsystem devices are defined
     // Example:
-    // public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(3, "");
+    public static final CanDeviceId FLYWHEEL_LEADER = new CanDeviceId(3, "");
+    public static final CanDeviceId FLYWHEEL_FOLLOWER = new CanDeviceId(4, "");
 
     /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */
     // This is where digital I/O feedback devices are defined

src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java

@@ -14,6 +14,7 @@
 package frc.robot.subsystems.flywheel_example;
 
 import static edu.wpi.first.units.Units.*;
+import static frc.robot.Constants.FlywheelConstants.*;
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.util.Units;
@@ -39,12 +40,12 @@ public Flywheel(FlywheelIO io) {
     switch (Constants.getMode()) {
       case REAL:
       case REPLAY:
-        ffModel = new SimpleMotorFeedforward(0.1, 0.05);
-        io.configurePID(1.0, 0.0, 0.0);
+        ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal);
+        io.configurePID(kPReal, kIReal, kDReal);
         break;
       case SIM:
-        ffModel = new SimpleMotorFeedforward(0.0, 0.03);
-        io.configurePID(0.5, 0.0, 0.0);
+        ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim);
+        io.configurePID(kPSim, kISim, kDSim);
         break;
       default:
         ffModel = new SimpleMotorFeedforward(0.0, 0.0);
@@ -98,7 +99,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
   }
 
   /** Returns the current velocity in RPM. */
-  @AutoLogOutput
+  @AutoLogOutput(key = "Mechanism/Flywheel")
   public double getVelocityRPM() {
     return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec);
   }

src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSparkMax.java

@@ -13,23 +13,28 @@
 
 package frc.robot.subsystems.flywheel_example;
 
+import static frc.robot.Constants.FlywheelConstants.*;
+
 import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.SparkPIDController.ArbFFUnits;
 import edu.wpi.first.math.util.Units;
+import frc.robot.RobotContainer.Ports;
 
 /**
  * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
  * "CANSparkFlex".
  */
 public class FlywheelIOSparkMax implements FlywheelIO {
-  private static final double GEAR_RATIO = 1.5;
 
-  private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless);
-  private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless);
+  // Define the leader / follower motors from the Ports section of RobotContainer
+  private final CANSparkMax leader =
+      new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless);
+  private final CANSparkMax follower =
+      new CANSparkMax(Ports.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless);
   private final RelativeEncoder encoder = leader.getEncoder();
   private final SparkPIDController pid = leader.getPIDController();
 

src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java

@@ -13,6 +13,8 @@
 
 package frc.robot.subsystems.flywheel_example;
 
+import static frc.robot.Constants.FlywheelConstants.*;
+
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Slot0Configs;
@@ -23,12 +25,15 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.util.Units;
+import frc.robot.RobotContainer.Ports;
 
 public class FlywheelIOTalonFX implements FlywheelIO {
-  private static final double GEAR_RATIO = 1.5;
 
-  private final TalonFX leader = new TalonFX(0);
-  private final TalonFX follower = new TalonFX(1);
+  // Define the leader / follower motors from the Ports section of RobotContainer
+  private final TalonFX leader =
+      new TalonFX(Ports.FLYWHEEL_LEADER.getDeviceNumber(), Ports.FLYWHEEL_LEADER.getBus());
+  private final TalonFX follower =
+      new TalonFX(Ports.FLYWHEEL_FOLLOWER.getDeviceNumber(), Ports.FLYWHEEL_FOLLOWER.getBus());
 
   private final StatusSignal<Double> leaderPosition = leader.getPosition();
   private final StatusSignal<Double> leaderVelocity = leader.getVelocity();