Merge pull request #2 from ThePinkAlliance/develop

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

@@ -6,6 +6,7 @@
 
 import com.ThePinkAlliance.core.drivetrain.swerve.SwerveModuleConfig;
 import com.ThePinkAlliance.swervelib.Mk4SwerveModuleHelper;
+import com.ThePinkAlliance.swervelib.Mk4iSwerveModuleHelper;
 import com.ThePinkAlliance.swervelib.SdsModuleConfigurations;
 import edu.wpi.first.math.util.Units;
 
@@ -46,8 +47,8 @@ public final class Constants {
     GLOBAL_SWERVE_POD_RAMP_RATE
   );
 
-  public static Mk4SwerveModuleHelper.GearRatio gearRatio =
-    Mk4SwerveModuleHelper.GearRatio.L1;
+  public static Mk4iSwerveModuleHelper.GearRatio gearRatio =
+    Mk4iSwerveModuleHelper.GearRatio.L1;
 
   public static final double MAX_VOLTAGE = 12.0;
 

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

@@ -6,15 +6,18 @@
 
 import com.ThePinkAlliance.core.joystick.Joystick;
 import com.ThePinkAlliance.core.joystick.JoystickAxis;
+import com.ThePinkAlliance.core.limelight.Limelight;
 import com.ThePinkAlliance.core.pathweaver.PathChooser;
 import com.ThePinkAlliance.core.selectable.CommandSelectable;
 import com.ThePinkAlliance.core.selectable.SelectableBuilder;
+import com.ctre.phoenix.motion.MotionProfileStatus;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.commands.Drive;
-import frc.robot.selectables.auto.DriveStraight;
 import frc.robot.subsystems.Base;
 
 /**
@@ -27,31 +30,26 @@ public class RobotContainer {
 
   private final Joystick mainJS = new Joystick(0);
 
-  private JoystickAxis x = new JoystickAxis(mainJS, Joystick.Axis.LEFT_X)
-    .withAxisCubed()
-    .withDeadband()
-    .withLimit(55);
+  private JoystickAxis x = new JoystickAxis(mainJS, Joystick.Axis.LEFT_X);
 
-  private JoystickAxis y = new JoystickAxis(mainJS, Joystick.Axis.LEFT_Y)
-    .withAxisCubed()
-    .withDeadband()
-    .withLimit(55);
+  private JoystickAxis y = new JoystickAxis(mainJS, Joystick.Axis.LEFT_Y);
 
-  private JoystickAxis rot = new JoystickAxis(mainJS, Joystick.Axis.RIGHT_X)
-    .withAxisCubed()
-    .withDeadband()
-    .withLimit(55);
+  private JoystickAxis rot = new JoystickAxis(mainJS, Joystick.Axis.RIGHT_X);
 
   private final PathChooser m_pathChooser = new PathChooser("drivers", 2, 0);
 
-  private final CommandSelectable defaultSelectable = SelectableBuilder.buildCommand(
+  private final DataLog log = new DataLog("logs");
+
+  private final CommandSelectable defaultSelectable = SelectableBuilder.build(
     "Drive Straight",
     new InstantCommand()
   );
 
   // The robot's subsystems and commands are defined here...
   private final Base m_base = new Base();
 
+  private final Limelight m_limelight = new Limelight(33.3, 50);
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings

src/main/java/frc/robot/commands/Drive.java

@@ -6,6 +6,7 @@
 
 import com.ThePinkAlliance.core.joystick.JoystickAxis;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Base;
 
@@ -36,9 +37,13 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double x = this.x.getSuppliedValue().get();
-    double y = this.y.getSuppliedValue().get();
-    double rot = this.rot.getSuppliedValue().get();
+    double x = this.x.get();
+    double y = this.y.get();
+    double rot = this.rot.get();
+
+    SmartDashboard.putNumber("x", x);
+    SmartDashboard.putNumber("y", y);
+    SmartDashboard.putNumber("rot", rot);
 
     m_base.drive(new ChassisSpeeds(x, y, rot));
   }

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

@@ -5,6 +5,7 @@
 package frc.robot.subsystems;
 
 import com.ThePinkAlliance.swervelib.Mk4SwerveModuleHelper;
+import com.ThePinkAlliance.swervelib.Mk4iSwerveModuleHelper;
 import com.ThePinkAlliance.swervelib.SwerveModule;
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -79,55 +80,43 @@ public Base() {
     this.states = kinematics.toSwerveModuleStates(new ChassisSpeeds());
 
     this.frontLeftModule =
-      Mk4SwerveModuleHelper.createFalcon500(
+      Mk4iSwerveModuleHelper.createFalcon500(
         tab
           .getLayout("Front Left Module", BuiltInLayouts.kList)
           .withSize(2, 4)
           .withPosition(0, 0),
         Constants.gearRatio,
-        Constants.frontLeftConfig.getMotorDrivePort(),
-        Constants.frontLeftConfig.getMotorSteerPort(),
-        Constants.frontLeftConfig.getCanCoderPort(),
-        Constants.frontLeftConfig.getSteerOffset()
+        Constants.frontLeftConfig
       );
 
     this.frontRightModule =
-      Mk4SwerveModuleHelper.createFalcon500(
+      Mk4iSwerveModuleHelper.createFalcon500(
         tab
           .getLayout("Front Right Module", BuiltInLayouts.kList)
           .withSize(2, 4)
           .withPosition(0, 2),
         Constants.gearRatio,
-        Constants.frontRightConfig.getMotorDrivePort(),
-        Constants.frontRightConfig.getMotorSteerPort(),
-        Constants.frontRightConfig.getCanCoderPort(),
-        Constants.frontRightConfig.getSteerOffset()
+        Constants.frontRightConfig
       );
 
     this.backRightModule =
-      Mk4SwerveModuleHelper.createFalcon500(
+      Mk4iSwerveModuleHelper.createFalcon500(
         tab
           .getLayout("Back Right Module", BuiltInLayouts.kList)
           .withSize(2, 4)
           .withPosition(0, 4),
         Constants.gearRatio,
-        Constants.backRightConfig.getMotorDrivePort(),
-        Constants.backRightConfig.getMotorSteerPort(),
-        Constants.backRightConfig.getCanCoderPort(),
-        Constants.backRightConfig.getSteerOffset()
+        Constants.backRightConfig
       );
 
     this.backLeftModule =
-      Mk4SwerveModuleHelper.createFalcon500(
+      Mk4iSwerveModuleHelper.createFalcon500(
         tab
           .getLayout("Back Left Module", BuiltInLayouts.kList)
           .withSize(2, 4)
           .withPosition(0, 6),
         Constants.gearRatio,
-        Constants.backLeftConfig.getMotorDrivePort(),
-        Constants.backLeftConfig.getMotorSteerPort(),
-        Constants.backLeftConfig.getCanCoderPort(),
-        Constants.backLeftConfig.getSteerOffset()
+        Constants.backLeftConfig
       );
 
     this.configurePods();
@@ -268,6 +257,15 @@ public Supplier<Pose2d> getPoseSupplier() {
     return () -> odometry.getPoseMeters();
   }
 
+  /**
+   * Calculates the desired angle relative to the robot's heading.
+   * @param desiredAngle The desired rotation in degress
+   * @return The angle in degress the drivetrain needs to move.
+   */
+  public Rotation2d calculateDesiredAngle(Rotation2d desiredAngle) {
+    return new Rotation2d(this.getSensorYaw() - desiredAngle.getDegrees());
+  }
+
   /**
    * Returns the current direction of the robot.
    */

vendordeps/SdsSwerveLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "SdsSwerveLib.json",
     "name": "swerve-lib",
-    "version": "1.1.3",
+    "version": "1.1.5",
     "uuid": "9619F7EA-7F96-4236-9D94-02338DFED592",
     "mavenUrls": [
         "https://jitpack.io"
@@ -11,7 +11,7 @@
         {
             "groupId": "com.github.ThePinkAlliance",
             "artifactId": "swerve-lib",
-            "version": "1.1.3"
+            "version": "1.1.5"
         }
     ],
     "jniDependencies": [],

vendordeps/ThePinkAlliance.json

@@ -1,7 +1,7 @@
 {
     "fileName": "ThePinkAlliance.json",
     "name": "ThePinkAlliance",
-    "version": "2.0.11",
+    "version": "2.0.12",
     "uuid": "9619F7EA-7F96-4236-9D94-02338DFED572",
     "mavenUrls": [
         "https://jitpack.io",
@@ -13,7 +13,7 @@
         {
             "groupId": "com.github.ThePinkAlliance",
             "artifactId": "core",
-            "version": "2.0.11"
+            "version": "2.0.12"
         }
     ],
     "jniDependencies": [],

vendordeps/navx_frc.json

@@ -1,7 +1,7 @@
 {
     "fileName": "navx_frc.json",
     "name": "KauaiLabs_navX_FRC",
-    "version": "4.0.442",
+    "version": "4.0.447",
     "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
     "mavenUrls": [
         "https://repo1.maven.org/maven2/"
@@ -11,15 +11,15 @@
         {
             "groupId": "com.kauailabs.navx.frc",
             "artifactId": "navx-java",
-            "version": "4.0.442"
+            "version": "4.0.447"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.kauailabs.navx.frc",
             "artifactId": "navx-cpp",
-            "version": "4.0.442",
+            "version": "4.0.447",
             "headerClassifier": "headers",
             "sourcesClassifier": "sources",
             "sharedLibrary": false,