Merge pull request #28 from frc937/heading-snapping

Heading snapping

src/main/deploy/swerve/swervedrive.json

@@ -4,7 +4,7 @@
     "id": 0,
     "canbus": null
   },
-  "invertedIMU": true,
+  "invertedIMU": false,
   "modules": [
     "frontleft.json",
     "frontright.json",

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

@@ -16,11 +16,11 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.DeployPneumatics;
-import frc.robot.commands.DriveFieldOriented;
-import frc.robot.commands.DriveRobotOriented;
 import frc.robot.commands.EnterXMode;
 import frc.robot.commands.RunBelts;
 import frc.robot.commands.RunIntake;
+import frc.robot.commands.drive.DriveFieldOriented;
+import frc.robot.commands.drive.DriveRobotOriented;
 import frc.robot.subsystems.Drive;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.mailbox.MailboxBelts;

src/main/java/frc/robot/commands/DriveFieldOriented.java → src/main/java/frc/robot/commands/drive/DriveFieldOriented.java

@@ -9,9 +9,11 @@
  * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
  */
 
-package frc.robot.commands;
+package frc.robot.commands.drive;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.Drive;
 
@@ -32,10 +34,12 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    drive.driveFieldOriented(
-        RobotContainer.getScaledControllerLeftYAxis(),
-        RobotContainer.getScaledControllerLeftXAxis(),
-        RobotContainer.getControllerRightXAxis());
+    double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED;
+    double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED;
+    double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED;
+    Translation2d translation = new Translation2d(x, y);
+
+    drive.driveFieldOriented(translation, z);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java

@@ -0,0 +1,88 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+import swervelib.SwerveController;
+
+/**
+ * Drives robot in field oriented mode with shortcuts to snap to field relative angles in increments
+ * of 90º using dpad
+ */
+public class DriveFieldOrientedHeadingSnapping extends Command {
+  private Drive drive;
+  private XboxController controller;
+
+  /** Creates a new DriveFieldOrientedHeadingSnapping. */
+  public DriveFieldOrientedHeadingSnapping() {
+    this.controller = RobotContainer.driverController.getHID();
+    this.drive = RobotContainer.drive;
+    addRequirements(RobotContainer.drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double headingX = 0;
+    double headingY = 0;
+    if (controller.getPOV() == 0) {
+      headingY = -1;
+    }
+    if (controller.getPOV() == 90) {
+      headingX = 1;
+    }
+    if (controller.getPOV() == 180) {
+      headingY = 1;
+    }
+    if (controller.getPOV() == 270) {
+      headingX = -1;
+    }
+
+    ChassisSpeeds desiredSpeeds =
+        drive.getTargetSpeeds(
+            RobotContainer.getScaledControllerLeftXAxis(),
+            RobotContainer.getScaledControllerLeftYAxis(),
+            headingX,
+            headingY);
+
+    Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
+    if (headingX == 0
+        && headingY == 0
+        && Math.abs(RobotContainer.getScaledControllerRightXAxis()) > 0) {
+      drive.driveFieldOriented(
+          translation,
+          (RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED));
+    } else {
+      drive.driveFieldOriented(translation, desiredSpeeds.omegaRadiansPerSecond);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/DriveRobotOriented.java → src/main/java/frc/robot/commands/drive/DriveRobotOriented.java

@@ -9,9 +9,11 @@
  * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
  */
 
-package frc.robot.commands;
+package frc.robot.commands.drive;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.RobotContainer;
 import frc.robot.subsystems.Drive;
 
@@ -32,10 +34,12 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    drive.driveRobotOriented(
-        RobotContainer.getScaledControllerLeftYAxis(),
-        RobotContainer.getScaledControllerLeftXAxis(),
-        RobotContainer.getScaledControllerRightXAxis());
+    double x = RobotContainer.getScaledControllerLeftYAxis() * Constants.Drive.MAX_SPEED;
+    double y = RobotContainer.getScaledControllerLeftXAxis() * Constants.Drive.MAX_SPEED;
+    double z = RobotContainer.getScaledControllerRightXAxis() * Constants.Drive.MAX_ANGULAR_SPEED;
+    Translation2d translation = new Translation2d(x, y);
+
+    drive.driveRobotOriented(translation, z);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -12,6 +12,7 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -45,35 +46,24 @@ public Drive() {
   /**
    * Drives the robot in robot-oriented mode.
    *
-   * @param x Robot velocity left to right in m/s. Left is positive.
-   * @param y Robot velocity forward and backward in m/s. Forward is positive.
+   * @param translation {@link Translation2d} that represents the commanded robot velocities on the
+   *     x and y axes. Front-left postitive.
    * @param z Robot angular velocity around the z-axis in radians per second. Counter-clockwise is
    *     positive.
    */
-  public void driveRobotOriented(double x, double y, double z) {
-    x = x * getMaximumSpeed();
-    y = y * getMaximumSpeed();
-    z = z * getMaximumAngularSpeed();
-    Translation2d translation = new Translation2d(x, y);
-
+  public void driveRobotOriented(Translation2d translation, double z) {
     drive.drive(translation, z, false, false);
   }
 
   /**
    * Drives the robot in field-oriented mode.
    *
-   * @param x Robot velocity left to right in m/s. Left is positive. Relative to the field.
-   * @param y Robot velocity forward and backward in m/s. Toward the opposing alliance wall is
-   *     positive.
+   * @param translation {@link Translation2d} that represents the commanded robot velocities on the
+   *     x and y axes. Front-left postitive. Reletive to the field.
    * @param z Robot angular velocity around the z-axis in radians per second. Counter-clockwise is
    *     positive.
    */
-  public void driveFieldOriented(double x, double y, double z) {
-    x = x * getMaximumSpeed();
-    y = y * getMaximumSpeed();
-    z = z * getMaximumAngularSpeed();
-    Translation2d translation = new Translation2d(x, y);
-
+  public void driveFieldOriented(Translation2d translation, double z) {
     drive.drive(translation, z, true, false);
   }
 
@@ -88,12 +78,26 @@ public void enterXMode() {
     drive.lockPose();
   }
 
-  private double getMaximumSpeed() {
-    return Constants.Drive.MAX_SPEED;
-  }
-
-  private double getMaximumAngularSpeed() {
-    return Constants.Drive.MAX_ANGULAR_SPEED;
+  /**
+   * Takes [-1, 1] joystick-like inputs and converts them to a {@link ChassisSpeeds} object that
+   * represents the commanded robot velocities
+   *
+   * @param translationX joystick input for the left to right axis. [-1, 1], left is positive.
+   * @param translationY joystick input for the forward to backward axis. [-1, 1], forward is
+   *     positive.
+   * @param headingX x component of the cartesian angle of the robot's heading
+   * @param headingY y component of the cartesian angle of the robot's heading
+   * @return {@link ChassisSpeeds} object that represents the commanded robot velocities
+   */
+  public ChassisSpeeds getTargetSpeeds(
+      double translationX, double translationY, double headingX, double headingY) {
+    return drive.swerveController.getTargetSpeeds(
+        translationX,
+        translationY,
+        headingX,
+        headingY,
+        drive.getPose().getRotation().getRadians(),
+        Constants.Drive.MAX_SPEED);
   }
 
   /** Runs every scheduler run. */