"Merged" from swerve-base repo

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

@@ -4,8 +4,13 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.numbers.N3;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
@@ -24,7 +29,7 @@ public final class Constants {
   /**
    * Input/Output constants
    */
-  public static class IOConstants {
+  public static final class IOConstants {
     public static final int kDriverControllerPort = 0;
 
     public static final double kControllerDeadband = 0.2;
@@ -72,7 +77,7 @@ public static final class DriveConstants {
     public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 configuration
 
     // TODO: Tune this PID before running on a robot on the ground
-    public static final double kPModuleTurningController = 0.3;
+    public static final double kPModuleTurningController = -0.3;
 
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
         new Translation2d(kWheelBase / 2, kTrackWidth / 2),
@@ -90,4 +95,25 @@ public static final class DriveConstants {
     // TODO: Tune this PID before running on a robot on the ground
     public static final double kPHeadingCorrectionController = 5;
   }
+
+  public static final class VisionConstants {
+    // TODO: Update cam pose relative to center of bot
+    public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
+    public static final double[] kLimelightCamPose = {
+        kCamPose.getX(),
+        kCamPose.getY(),
+        kCamPose.getZ(),
+        kCamPose.getRotation().getX(),
+        kCamPose.getRotation().getY(),
+        kCamPose.getRotation().getZ() };
+
+    // TODO: Experiment with different std devs in the pose estimator
+    public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);
+
+    // Field size in meters
+    public static final double kFieldWidth = 8.21055;
+    public static final double kFieldLength = 16.54175;
+  }
+
 }

src/main/java/frc/robot/Main.java

@@ -6,9 +6,19 @@
 
 import edu.wpi.first.wpilibj.RobotBase;
 
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
 public final class Main {
   private Main() {}
 
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
   public static void main(String... args) {
     RobotBase.startRobot(Robot::new);
   }

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

@@ -8,66 +8,96 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
   @Override
   public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
   }
 
+  /**
+   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
   @Override
   public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
   }
 
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
   public void disabledInit() {}
 
   @Override
   public void disabledPeriodic() {}
 
-  @Override
-  public void disabledExit() {}
-
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
+    // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
   }
 
+  /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {}
 
-  @Override
-  public void autonomousExit() {}
-
   @Override
   public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
   }
 
+  /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {}
 
-  @Override
-  public void teleopExit() {}
-
   @Override
   public void testInit() {
+    // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
   }
 
+  /** This function is called periodically during test mode. */
   @Override
   public void testPeriodic() {}
 
+  /** This function is called once when the robot is first started up. */
+  @Override
+  public void simulationInit() {}
+
+  /** This function is called periodically whilst in simulation. */
   @Override
-  public void testExit() {}
+  public void simulationPeriodic() {}
 }

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

@@ -16,7 +16,12 @@
 import frc.robot.Constants.IOConstants;
 import frc.robot.subsystems.DriveSubsystem;
 
-
+/*
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
@@ -69,6 +74,11 @@ private void configureBindings() {
         .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
   }
 
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.VisionConstants;
 import frc.robot.Robot;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -64,9 +65,9 @@ public class DriveSubsystem extends SubsystemBase {
       m_rearRight.getPosition()
   };
 
-  // TODO: Experiment with different std devs in the pose estimator
   private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics,
-      m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d());
+      m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
+      VisionConstants.kVisionSTDDevs);
 
   private final Field2d m_field = new Field2d();
 
@@ -88,7 +89,8 @@ public void periodic() {
         m_rearRight.getPosition()
     };
 
-    m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), m_swerveModulePositions);
+    m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
+        m_swerveModulePositions);
 
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
@@ -195,6 +197,10 @@ public void zeroHeading() {
     m_gyroAngle = 0;
   }
 
+  public void addVisionMeasurement(Pose2d pose, double timestamp) {
+    m_poseEstimator.addVisionMeasurement(pose, timestamp);
+  }
+
   /**
    * Sets the swerve ModuleStates.
    *

src/main/java/frc/robot/subsystems/SwerveModule.java

@@ -4,26 +4,26 @@
 
 package frc.robot.subsystems;
 
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CANcoderConfigurator;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.units.Units;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Robot;
 
-/** Add your docs here. */
 public class SwerveModule {
-  private final CANSparkFlex m_driveMotor;
-  private final CANSparkFlex m_turningMotor;
+  private final CANSparkMax m_driveMotor;
+  private final CANSparkMax m_turningMotor;
 
   private final CANcoder m_turningEncoder;
+  private final CANcoderConfigurator m_turningEncoderConfigurator;
 
   private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0,
       0);
@@ -49,9 +49,10 @@ public SwerveModule(
       int turningEncoderPort,
       boolean driveMotorReversed,
       double turningEncoderOffset) {
-    m_driveMotor = new CANSparkFlex(driveMotorPort, MotorType.kBrushless);
-    m_turningMotor = new CANSparkFlex(turningMotorPort, MotorType.kBrushless);
+    m_driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless);
+    m_turningMotor = new CANSparkMax(turningMotorPort, MotorType.kBrushless);
     m_turningEncoder = new CANcoder(turningEncoderPort);
+    m_turningEncoderConfigurator = m_turningEncoder.getConfigurator();
 
     // converts default units to meters per second
     m_driveMotor.getEncoder().setVelocityConversionFactor(
@@ -60,8 +61,10 @@ public SwerveModule(
     m_driveMotor.setInverted(driveMotorReversed);
 
     m_turningMotor.setIdleMode(IdleMode.kBrake);
-
-    m_turningEncoder.getConfigurator().apply(new CANcoderConfiguration().MagnetSensor.withMagnetOffset(-turningEncoderOffset));
+
+    // TODO: CANcoder offsets are now set on the device manually using Pheonix Tuner
+    // (or maybe Pheonix X)
+    m_turningEncoderConfigurator.apply(new MagnetSensorConfigs().withMagnetOffset(-turningEncoderOffset));
 
     m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
   }
@@ -78,12 +81,9 @@ public SwerveModulePosition getPosition() {
     // actual encoders
     // If the robot is simulated, then return the swerve module state using the
     // expected values
-
-
-    ;
     return Robot.isReal()
         ? new SwerveModulePosition(m_driveMotor.getEncoder().getPosition(),
-            new Rotation2d(Units.RadiansPerSecond.convertFrom(m_turningEncoder.getAbsolutePosition().getValueAsDouble(), Units.RotationsPerSecond)))
+            getEncoderAngle(m_turningEncoder))
         : new SwerveModulePosition(m_distance, m_state.angle);
   }
 
@@ -93,14 +93,26 @@ public SwerveModulePosition getPosition() {
    * @param desiredState Desired state with speed and angle.
    */
   public void setDesiredState(SwerveModuleState desiredState) {
-    m_state = SwerveModuleState.optimize(desiredState, new Rotation2d(Units.RadiansPerSecond.convertFrom(m_turningEncoder.getAbsolutePosition().getValueAsDouble(), Units.RotationsPerSecond)));
-
+    m_state = SwerveModuleState.optimize(desiredState, getEncoderAngle(m_turningEncoder));
     driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond;
 
-    turnOutput = m_turningPIDController.calculate(Units.RadiansPerSecond.convertFrom(m_turningEncoder.getAbsolutePosition().getValueAsDouble(), Units.RotationsPerSecond),
+    turnOutput = m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(),
         m_state.angle.getRadians());
 
     m_driveMotor.set(driveOutput);
     m_turningMotor.set(turnOutput);
   }
+
+  /**
+   * Returns the angle of a CANcoder
+   * 
+   * The CANcoder now gives values in rotations which is useless, so this method
+   * translates the CANcoder output into a Rotation2D
+   * 
+   * @param encoder The encoder to get the absolute angle of.
+   * @return A Rotation2d of the absolute angle.
+   */
+  public Rotation2d getEncoderAngle(CANcoder encoder) {
+    return new Rotation2d(encoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI);
+  }
 }