feat: swerve code from swerve-base

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

@@ -0,0 +1,93 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide
+ * numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants
+ * should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+
+  /**
+   * Input/Output constants
+   */
+  public static class IOConstants {
+    public static final int kDriverControllerPort = 0;
+
+    public static final double kControllerDeadband = 0.2;
+    public static final double kSlowModeScalar = 0.8;
+  }
+
+  public static final class DriveConstants {
+    // TODO: set motor and encoder constants
+    public static final int kFrontLeftDriveMotorPort = 1;
+    public static final int kRearLeftDriveMotorPort = 3;
+    public static final int kFrontRightDriveMotorPort = 5;
+    public static final int kRearRightDriveMotorPort = 7;
+
+    public static final int kFrontLeftTurningMotorPort = 2;
+    public static final int kRearLeftTurningMotorPort = 4;
+    public static final int kFrontRightTurningMotorPort = 6;
+    public static final int kRearRightTurningMotorPort = 8;
+
+    public static final int kFrontLeftTurningEncoderPort = 9;
+    public static final int kRearLeftTurningEncoderPort = 10;
+    public static final int kFrontRightTurningEncoderPort = 11;
+    public static final int kRearRightTurningEncoderPort = 12;
+
+    public static final double kFrontLeftTurningEncoderOffset = 0;
+    public static final double kRearLeftTurningEncoderOffset = 0;
+    public static final double kFrontRightTurningEncoderOffset = 0;
+    public static final double kRearRightTurningEncoderOffset = 0;
+
+    // TODO: Test motor orientations before driving on an actual robot
+    public static final boolean kFrontLeftDriveMotorReversed = false;
+    public static final boolean kRearLeftDriveMotorReversed = false;
+    public static final boolean kFrontRightDriveMotorReversed = true;
+    public static final boolean kRearRightDriveMotorReversed = true;
+
+    /** Distance between centers of right and left wheels on robot (in meters). */
+    public static final double kTrackWidth = 0.5;
+
+    /** Distance between front and back wheels on robot (in meters). */
+    public static final double kWheelBase = 0.5;
+
+    /** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
+    public static final double kWheelDiameterMeters = 0.1;
+
+    /** Gear ratio between the motor and the wheel. */
+    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 SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+    /** For a a SDS Mk4i L1 swerve base with Neos */
+    public static final double kMaxSpeedMetersPerSecond = 3.6576;
+    /** For a a SDS Mk4i L1 swerve base with Neos */
+    public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3;
+
+    /** Heading Correction */
+    public static final double kHeadingCorrectionTurningStopTime = 0.2;
+    // TODO: Tune this PID before running on a robot on the ground
+    public static final double kPHeadingCorrectionController = 5;
+  }
+}

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

@@ -4,15 +4,70 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.IOConstants;
+import frc.robot.subsystems.DriveSubsystem;
+
 
 public class RobotContainer {
+  // The robot's subsystems and commands are defined here
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+  private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
+
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
+    // Configure the trigger bindings
     configureBindings();
+
+    m_robotDrive.setDefaultCommand(
+        new RunCommand(
+            () -> m_robotDrive.drive(
+                MathUtil.applyDeadband(
+                    -m_driverController.getLeftY(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxSpeedMetersPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    * 0.8,
+                MathUtil.applyDeadband(
+                    -m_driverController.getLeftX(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxSpeedMetersPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    * 0.8,
+                MathUtil.applyDeadband(
+                    m_driverController.getRightX(),
+                    IOConstants.kControllerDeadband)
+                    * DriveConstants.kMaxAngularSpeedRadiansPerSecond
+                    * (1 - m_driverController
+                        .getLeftTriggerAxis()
+                        * IOConstants.kSlowModeScalar)
+                    / 2,
+                !m_driverController.getRightBumper()),
+            m_robotDrive));
   }
 
-  private void configureBindings() {}
+  /**
+   * Use this method to define your button->command mappings.
+   */
+  private void configureBindings() {
+    new JoystickButton(m_driverController, Button.kStart.value)
+        .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
+  }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");

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

@@ -0,0 +1,226 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.Robot;
+
+public class DriveSubsystem extends SubsystemBase {
+  private final SwerveModule m_frontLeft = new SwerveModule(
+      DriveConstants.kFrontLeftDriveMotorPort,
+      DriveConstants.kFrontLeftTurningMotorPort,
+      DriveConstants.kFrontLeftTurningEncoderPort,
+      DriveConstants.kFrontLeftDriveMotorReversed,
+      DriveConstants.kFrontLeftTurningEncoderOffset);
+
+  private final SwerveModule m_rearLeft = new SwerveModule(
+      DriveConstants.kRearLeftDriveMotorPort,
+      DriveConstants.kRearLeftTurningMotorPort,
+      DriveConstants.kRearLeftTurningEncoderPort,
+      DriveConstants.kRearLeftDriveMotorReversed,
+      DriveConstants.kRearLeftTurningEncoderOffset);
+
+  private final SwerveModule m_frontRight = new SwerveModule(
+      DriveConstants.kFrontRightDriveMotorPort,
+      DriveConstants.kFrontRightTurningMotorPort,
+      DriveConstants.kFrontRightTurningEncoderPort,
+      DriveConstants.kFrontRightDriveMotorReversed,
+      DriveConstants.kFrontRightTurningEncoderOffset);
+
+  private final SwerveModule m_rearRight = new SwerveModule(
+      DriveConstants.kRearRightDriveMotorPort,
+      DriveConstants.kRearRightTurningMotorPort,
+      DriveConstants.kRearRightTurningEncoderPort,
+      DriveConstants.kRearRightDriveMotorReversed,
+      DriveConstants.kRearRightTurningEncoderOffset);
+
+  private final AHRS m_gyro = new AHRS();
+  private double m_gyroAngle;
+
+  private final Timer m_headingCorrectionTimer = new Timer();
+  private final PIDController m_headingCorrectionPID = new PIDController(DriveConstants.kPHeadingCorrectionController,
+      0, 0);
+  private SwerveModulePosition[] m_swerveModulePositions = new SwerveModulePosition[] {
+      m_frontLeft.getPosition(),
+      m_frontRight.getPosition(),
+      m_rearLeft.getPosition(),
+      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());
+
+  private final Field2d m_field = new Field2d();
+
+  /** Creates a new DriveSubsystem. */
+  public DriveSubsystem() {
+    SmartDashboard.putData("Field", m_field);
+    m_headingCorrectionTimer.restart();
+    m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+
+    m_swerveModulePositions = new SwerveModulePosition[] {
+        m_frontLeft.getPosition(),
+        m_frontRight.getPosition(),
+        m_rearLeft.getPosition(),
+        m_rearRight.getPosition()
+    };
+
+    m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle), m_swerveModulePositions);
+
+    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
+
+    SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
+    SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX());
+    SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY());
+
+    // AdvantageScope Logging
+    double[] logData = {
+        m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput,
+        m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput,
+        m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput,
+        m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput,
+    };
+    SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
+  }
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   *
+   * @return The pose.
+   */
+  public Pose2d getPose() {
+    return m_poseEstimator.getEstimatedPosition();
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed        Speed of the robot in the x direction (forward).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rotation      Angular rotation speed of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the
+   *                      field.
+   */
+  public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
+    // If we are rotating, reset the timer
+    if (rotation != 0) {
+      m_headingCorrectionTimer.reset();
+    }
+
+    /*
+     * Heading correction helps maintain the same heading and
+     * prevents rotational drive while our robot is translating
+     * 
+     * For heading correction we use a timer to ensure that we
+     * lose all rotational momentum before saving the heading
+     * that we want to maintain
+     */
+
+    // TODO: Test heading correction without timer
+    // TODO: Test heading correction using gyro's rotational velocity (if it is 0
+    // then set heading instead of timer)
+
+    // Save our desired rotation to a variable we can add our heading correction
+    // adjustments to
+    double calculatedRotation = rotation;
+
+    double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians());
+
+    // If we are not translating or if not enough time has passed since the last
+    // time we rotated
+    if ((xSpeed == 0 && ySpeed == 0)
+        || m_headingCorrectionTimer.get() < DriveConstants.kHeadingCorrectionTurningStopTime) {
+      // Update our desired angle
+      m_headingCorrectionPID.setSetpoint(currentAngle);
+    } else {
+      // If we are translating or if we have not rotated for a long enough time
+      // then maintain our desired angle
+      calculatedRotation = m_headingCorrectionPID.calculate(currentAngle);
+    }
+
+    // Depending on whether the robot is being driven in field relative, calculate
+    // the desired states for each of the modules
+    SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
+        fieldRelative
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
+                Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
+            : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));
+
+    setModuleStates(swerveModuleStates);
+  }
+
+  /**
+   * Resets the odometry to the specified pose.
+   *
+   * @param pose The pose to which to set the odometry.
+   */
+  public void resetOdometry(Pose2d pose) {
+    m_poseEstimator.resetPosition(
+        Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
+        new SwerveModulePosition[] {
+            m_frontLeft.getPosition(),
+            m_frontRight.getPosition(),
+            m_rearLeft.getPosition(),
+            m_rearRight.getPosition()
+        },
+        pose);
+  }
+
+  /** Zeroes the heading of the robot. */
+  public void zeroHeading() {
+    m_gyro.reset();
+    m_gyroAngle = 0;
+  }
+
+  /**
+   * Sets the swerve ModuleStates.
+   *
+   * @param desiredStates The desired SwerveModule states.
+   */
+  public void setModuleStates(SwerveModuleState[] desiredStates) {
+    SwerveDriveKinematics.desaturateWheelSpeeds(
+        desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
+    m_frontLeft.setDesiredState(desiredStates[0]);
+    m_frontRight.setDesiredState(desiredStates[1]);
+    m_rearLeft.setDesiredState(desiredStates[2]);
+    m_rearRight.setDesiredState(desiredStates[3]);
+
+    // AdvantageScope Logging
+    double[] logData = {
+        desiredStates[0].angle.getDegrees(), desiredStates[0].speedMetersPerSecond,
+        desiredStates[1].angle.getDegrees(), desiredStates[1].speedMetersPerSecond,
+        desiredStates[2].angle.getDegrees(), desiredStates[2].speedMetersPerSecond,
+        desiredStates[3].angle.getDegrees(), desiredStates[3].speedMetersPerSecond,
+    };
+    SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logData);
+
+    // Takes the integral of the rotation speed to find the current angle for the
+    // simulator
+    m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond
+        * Robot.kDefaultPeriod;
+  }
+
+}