Merge branch 'offseason-tuning' into auto-scoring-with-zones

team4099 · Sep 22, 2023 · 79fd13e · 79fd13e
2 parents 3da8b63 + 95b9282
commit 79fd13e
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 18 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt
@@ -0,0 +1,28 @@
+package com.team4099.robot2023.commands.drivetrain
+
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import edu.wpi.first.math.kinematics.SwerveModuleState
+import edu.wpi.first.wpilibj2.command.CommandBase
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.inRotation2ds
+
+class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: Angle) :
+  CommandBase() {
+  init {
+    addRequirements(drivetrain)
+  }
+
+  override fun execute() {
+    for (module in drivetrain.swerveModules) {
+      module.setPositionClosedLoop(
+        SwerveModuleState(0.0, steeringPosition.inRotation2ds),
+        SwerveModuleState(0.0, steeringPosition.inRotation2ds),
+        false
+      )
+    }
+  }
+
+  override fun isFinished(): Boolean {
+    return false
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt
@@ -46,15 +46,7 @@ object FieldConstants {
           Rotation3d(0.0.radians, 0.0.radians, 0.0.radians)
         )
       ),
-      AprilTag(
-        2,
-        Pose3d(
-          (43.125).inches,
-          (40.875).inches,
-          (18.22).inches,
-          Rotation3d(0.0.radians, 0.0.radians, 0.0.radians)
-        )
-      ),
+      AprilTag(5, Pose3d((1).centi.meters, (87.5).inches, (89.3).centi.meters, Rotation3d())),
       AprilTag(
         3,
         Pose3d(
@@ -64,15 +56,6 @@ object FieldConstants {
           Rotation3d(0.0.radians, 0.0.radians, 0.0.radians)
         )
       ),
-      AprilTag(
-        5,
-        Pose3d(
-          (43.125).inches,
-          (173.375).inches,
-          (18.22).inches,
-          Rotation3d(0.0.radians, 0.0.radians, 0.0.radians)
-        )
-      ),
       AprilTag(
         4,
         Pose3d(

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt
@@ -30,6 +30,8 @@ import org.team4099.lib.units.derived.perMeterPerSecond
 import org.team4099.lib.units.derived.perMeterPerSecondPerSecond
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inMetersPerSecond
+import org.team4099.lib.units.inMetersPerSecondPerSecond
 import org.team4099.lib.units.perSecond
 import kotlin.math.IEEErem
 import kotlin.math.withSign
@@ -146,6 +148,17 @@ class SwerveModule(val io: SwerveModuleIO) {
     //        "${io.label}/steeringValueDegreesWithMod",
     //        inputs.steeringPosition.inDegrees.IEEErem(360.0)
     //      )
+
+    Logger.getInstance().recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond)
+    Logger.getInstance().recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians)
+    Logger.getInstance()
+      .recordOutput(
+        "SwerveModule/AccelerationSetPoint", accelerationSetPoint.inMetersPerSecondPerSecond
+      )
+    Logger.getInstance()
+      .recordOutput(
+        "SwerveModule/SteeringError", (steeringSetPoint - inputs.steeringPosition).inRadians
+      )
   }
 
   /**