Swerve Code

build.gradle

@@ -68,7 +68,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+    //testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 }
 
 test {

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,18 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

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

@@ -0,0 +1,126 @@
+// 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.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
+ * 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 final 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 = 32;
+    public static final int kRearLeftDriveMotorPort = 29;
+    public static final int kFrontRightDriveMotorPort = 38;
+    public static final int kRearRightDriveMotorPort = 34;
+
+    public static final int kFrontLeftTurningMotorPort = 28;
+    public static final int kRearLeftTurningMotorPort = 22;
+    public static final int kFrontRightTurningMotorPort = 37;
+    public static final int kRearRightTurningMotorPort = 26;
+
+    public static final int kFrontLeftTurningEncoderPort = 19;
+    public static final int kRearLeftTurningEncoderPort = 20;
+    public static final int kFrontRightTurningEncoderPort = 18;
+    public static final int kRearRightTurningEncoderPort = 17;
+
+    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.57785;
+
+    /** Distance between front and back wheels on robot (in meters). */
+    public static final double kWheelBase = 0.57785;
+
+    /** 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
+    // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 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 Neo Vortexes */
+    public static final double kMaxSpeedMetersPerSecond = 4.4196;
+    /** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
+    public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
+    // ^^ Calculated using the method taken from the old SDS github example
+
+    /** 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;
+  }
+
+  public static final class ShooterConstants {
+    public static final int kTopShooterMotorPort = 35;
+    public static final int kBottomShooterMotorPort = 20;
+  }
+
+  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() {}
 }