evasive rotation

QuackingBob · May 26, 2022 · 5107421 · 5107421
1 parent 02c6351
commit 5107421
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -57,6 +57,19 @@ public RobotContainer() {
    */
   private void configureButtonBindings() {
     new JoystickButton(driveJoystick, 2).whenPressed(() -> swerveDrivetrain.zeroHeading());
+    new JoystickButton(driveJoystick, 5)
+      .whenPressed(() -> swerveDrivetrain.setRotationPointIdx(1))
+      .whenReleased(() -> swerveDrivetrain.setRotationPointIdx(0));
+    new JoystickButton(driveJoystick, 6)
+      .whenPressed(() -> swerveDrivetrain.setRotationPointIdx(2))
+      .whenReleased(() -> swerveDrivetrain.setRotationPointIdx(0));
+    new JoystickButton(driveJoystick, 7)
+      .whenPressed(() -> swerveDrivetrain.setRotationPointIdx(3))
+      .whenReleased(() -> swerveDrivetrain.setRotationPointIdx(0));
+    new JoystickButton(driveJoystick, 8)
+      .whenPressed(() -> swerveDrivetrain.setRotationPointIdx(4))
+      .whenReleased(() -> swerveDrivetrain.setRotationPointIdx(0));
+
   }
 
   /**

diff --git a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java
@@ -67,7 +67,9 @@ public void execute() {
     }
 
     // convert to module states and apply to each wheel
-    SwerveModuleState[] moduleStates = drivetrain.getKinematics().toSwerveModuleStates(chassisSpeeds);
+    SwerveModuleState[] moduleStates = drivetrain.getKinematics().toSwerveModuleStates(
+      chassisSpeeds,
+      Constants.SwerveDrivetrain.rotatePoints[drivetrain.getRotationPointIdx()]);
     drivetrain.setModuleStates(moduleStates);
     SmartDashboard.putNumber("vX", vX);
     SmartDashboard.putNumber("vY", vY);

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -36,6 +36,7 @@ public static enum DrivetrainState {
   private SwerveDriveKinematics swerveKinematics;
   private SwerveModuleState[] desiredStates = new SwerveModuleState[4];
   private SwerveModule[] motors;
+  private int rotationPoint = 0;
 
   // Auton Stuff
   private Pose2d pose;
@@ -269,4 +270,12 @@ public void resetSimOdometry(Pose2d pose) {
   public Field2d getField() {
     return field;
   }
+
+  public int getRotationPointIdx() {
+    return rotationPoint;
+  }
+
+  public void setRotationPointIdx(int idx) {
+    rotationPoint = idx;
+  }
 }