Merge pull request #93 from strykeforce/swerve-module-rotation

Add get/set Rotation2d to SwerveModule

Author: jhh

build.gradle

@@ -6,12 +6,12 @@ plugins {
     id "maven-publish"
     id "org.jetbrains.kotlin.jvm" version "1.6.10"
     id "org.jetbrains.kotlin.kapt" version "1.6.10"
-    id "edu.wpi.first.GradleRIO" version "2022.1.1"
+    id "edu.wpi.first.GradleRIO" version "2022.3.1"
     id 'com.diffplug.spotless' version '6.2.0'
 }
 
 group = "org.strykeforce"
-version = "22.0.1"
+version = "22.1.0"
 
 sourceCompatibility = JavaVersion.VERSION_11
 targetCompatibility = JavaVersion.VERSION_11

src/main/java/org/strykeforce/swerve/SwerveModule.java

@@ -1,5 +1,6 @@
 package org.strykeforce.swerve;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import org.strykeforce.telemetry.Registrable;
@@ -32,7 +33,7 @@ public interface SwerveModule extends Registrable {
    * Command the swerve module motors to the desired state.
    *
    * @param desiredState the desired swerve module speed and angle
-   * @param isDriveOpenLoop true if drive should set speed using closed-loop velocity control
+   * @param isDriveOpenLoop false if drive should set speed using closed-loop velocity control
    */
   void setDesiredState(SwerveModuleState desiredState, boolean isDriveOpenLoop);
 
@@ -45,6 +46,23 @@ default void setDesiredState(SwerveModuleState desiredState) {
     this.setDesiredState(desiredState, false);
   }
 
+  /**
+   * Command the swerve module azimuth rotation to the desired angle with a drive speed of zero. If
+   * setting the module to an angle with a non-zero drive speed, use {@link
+   * SwerveModule#setDesiredState(SwerveModuleState)} or {@link
+   * SwerveModule#setDesiredState(SwerveModuleState, boolean)}.
+   *
+   * @param rotation the desired absolute azimuth angle
+   */
+  void setAzimuthRotation2d(Rotation2d rotation);
+
+  /**
+   * Get the angle of the swerve drive azimuth.
+   *
+   * @return the angle of the azimuth rotation.
+   */
+  Rotation2d getAzimuthRotation2d();
+
   /** Resets the drive encoders to currently read a position of 0. */
   void resetDriveEncoder();
 

src/main/java/org/strykeforce/swerve/TalonSwerveModule.java

@@ -91,11 +91,9 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isDriveOpenL
     if (desiredState.speedMetersPerSecond < driveDeadbandMetersPerSecond) {
       desiredState = new SwerveModuleState(0.0, previousAngle);
     }
-    previousAngle = desiredState.angle;
 
-    Rotation2d currentAngle = getAzimuthRotation2d();
-    SwerveModuleState optimizedState = SwerveModuleState.optimize(desiredState, currentAngle);
-    setAzimuthRotation2d(optimizedState.angle);
+    SwerveModuleState optimizedState = setAzimuthOptimizedState(desiredState);
+
     if (isDriveOpenLoop) {
       setDriveOpenLoopMetersPerSecond(optimizedState.speedMetersPerSecond);
     } else {
@@ -155,17 +153,35 @@ private int getAzimuthAbsoluteEncoderCounts() {
     return azimuthTalon.getSensorCollection().getPulseWidthPosition() & 0xFFF;
   }
 
+  @Override
   public Rotation2d getAzimuthRotation2d() {
     double azimuthCounts = azimuthTalon.getSelectedSensorPosition();
     double radians = 2.0 * Math.PI * azimuthCounts / azimuthCountsPerRev;
     return new Rotation2d(radians);
   }
 
+  @Override
   public void setAzimuthRotation2d(Rotation2d angle) {
+    setAzimuthOptimizedState(new SwerveModuleState(0.0, angle));
+    logger.info("azimuth {}: set rotation to: {}", azimuthTalon.getDeviceID(), angle);
+  }
+
+  @NotNull
+  private SwerveModuleState setAzimuthOptimizedState(SwerveModuleState desiredState) {
+    // minimize change in heading by potentially reversing the drive direction
+    Rotation2d currentAngle = getAzimuthRotation2d();
+    SwerveModuleState optimizedState = SwerveModuleState.optimize(desiredState, currentAngle);
+
+    // set the azimuth wheel position
     double countsBefore = azimuthTalon.getSelectedSensorPosition();
-    double countsFromAngle = angle.getRadians() / (2.0 * Math.PI) * azimuthCountsPerRev;
+    double countsFromAngle =
+        optimizedState.angle.getRadians() / (2.0 * Math.PI) * azimuthCountsPerRev;
     double countsDelta = Math.IEEEremainder(countsFromAngle - countsBefore, azimuthCountsPerRev);
     azimuthTalon.set(MotionMagic, countsBefore + countsDelta);
+
+    // save previous angle for use if inside deadband in setDesiredState()
+    previousAngle = optimizedState.angle;
+    return optimizedState;
   }
 
   private double getDriveMetersPerSecond() {

src/test/java/org/strykeforce/swerve/TalonSwerveModuleTest.java

@@ -3,7 +3,6 @@
 import static org.assertj.core.api.Assertions.assertThat;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.mockito.ArgumentMatchers.anyDouble;
 import static org.mockito.ArgumentMatchers.anyInt;
 import static org.mockito.Mockito.eq;
 import static org.mockito.Mockito.mock;
@@ -165,7 +164,6 @@ void shouldSetAzimuthZero(int absoluteEncoderPosition, int zeroReference, double
       module.loadAndSetAzimuthZeroReference();
       verify(azimuthTalon).setSelectedSensorPosition(setpoint, 0, 10);
     }
-
   }
 
   /*