apriltag + bug fixes

src/main/kotlin/org/team4099/lib/apriltag/AprilTag.kt

@@ -0,0 +1,18 @@
+package com.team4099.apriltag
+
+import edu.wpi.first.apriltag.AprilTag
+import org.team4099.lib.geometry.Pose3d
+
+class AprilTag(val id: Int, val pose: Pose3d) {
+  constructor(apriltagWpilib: AprilTag) : this(apriltagWpilib.ID, Pose3d(apriltagWpilib.pose)) {}
+
+  val apriltagWpilib = AprilTag(id, pose.pose3d)
+
+  override fun equals(obj: Any?): Boolean {
+    if (obj is com.team4099.apriltag.AprilTag) {
+      val other = obj
+      return id == other.id && pose == other.pose
+    }
+    return false
+  }
+}

src/main/kotlin/org/team4099/lib/apriltag/AprilTagFieldLayout.kt

@@ -0,0 +1,54 @@
+package com.team4099.apriltag
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Translation3d
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.radians
+
+class AprilTagFieldLayout(
+  val aprilTags: List<AprilTag>,
+  val fieldLength: Length,
+  val fieldWidth: Length
+) {
+
+  init {
+    setOrigin(OriginPosition.kBlueAllianceWallRightSide)
+  }
+
+  val apriltagFieldLayoutWPILIB =
+    AprilTagFieldLayout(
+      aprilTags.map { it.apriltagWpilib }, fieldLength.inMeters, fieldWidth.inMeters
+    )
+
+  var origin = Pose3d()
+
+  fun getTagPose(id: Int): Pose3d {
+    if (id < aprilTags.size) {
+      return Pose3d((-1337).meters, (-1337).meters, (-1337).meters, Rotation3d())
+    }
+    return aprilTags[id].pose
+  }
+
+  fun setOrigin(newOrigin: OriginPosition) {
+    origin =
+      when (newOrigin) {
+        OriginPosition.kBlueAllianceWallRightSide -> Pose3d()
+        OriginPosition.kRedAllianceWallRightSide ->
+          Pose3d(
+            Translation3d(fieldLength, fieldWidth, 0.meters),
+            Rotation3d(0.radians, 0.radians, Math.PI.radians)
+          )
+      }
+  }
+
+  data class FieldDimensions(val fieldWidth: Length, val fieldLength: Length)
+
+  enum class OriginPosition {
+    kBlueAllianceWallRightSide,
+    kRedAllianceWallRightSide
+  }
+}

src/main/kotlin/org/team4099/lib/units/derived/Controller.kt

@@ -29,10 +29,10 @@ val Double.radiansPerSecondPerRadiansPerSecond
   get() = Value<Fraction<Velocity<Radian>, Velocity<Radian>>>(this)
 
 val ProportionalGain<Radian, Velocity<Radian>>.inDegreesPerSecondPerDegree: Double
-  get() = inRadiansPerSecondPerRadian / RADIANS_PER_DEGREES / RADIANS_PER_DEGREES
+  get() = value
 
 val IntegralGain<Radian, Velocity<Radian>>.inDegreesPerSecondPerDegreeSeconds: Double
-  get() = inRadiansPerSecondPerRadianSeconds / RADIANS_PER_DEGREES / RADIANS_PER_DEGREES
+  get() = value
 
 val DerivativeGain<Radian, Velocity<Radian>>.inDegreesPerSecondPerDegreesPerSecond: Double
   get() = value
@@ -176,7 +176,7 @@ val DerivativeGain<Radian, Volt>.inVoltsPerDegreePerSecond: Double
   get() = inVoltsPerRadianPerSecond * RADIANS_PER_DEGREES
 
 val DerivativeGain<Radian, Volt>.inVoltsPerRotationsPerMinute: Double
-  get() = inVoltsPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
+  get() = inVoltsPerRadianPerSecond * SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
 
 val DerivativeGain<Velocity<Radian>, Volt>.inVoltsPerRadiansPerSecondPerSecond: Double
   get() = value
@@ -233,4 +233,4 @@ val DerivativeGain<Radian, Ampere>.inAmpsPerDegreePerSecond: Double
   get() = inAmpsPerRadianPerSecond * RADIANS_PER_DEGREES
 
 val DerivativeGain<Radian, Ampere>.inAmpsPerRotationsPerMinute: Double
-  get() = inAmpsPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
+  get() = inAmpsPerRadianPerSecond * SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

src/test/kotlin/team4099/controller/PIDControllerTest.kt

@@ -2,7 +2,9 @@ package com.team4099.controller
 
 import org.junit.jupiter.api.Test
 import org.team4099.lib.controller.PIDController
+import org.team4099.lib.controller.ProfiledPIDController
 import org.team4099.lib.controller.SimpleMotorFeedforward
+import org.team4099.lib.controller.TrapezoidProfile
 import org.team4099.lib.units.Acceleration
 import org.team4099.lib.units.Fraction
 import org.team4099.lib.units.Value
@@ -12,7 +14,10 @@ import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond
 import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
+import org.team4099.lib.units.derived.radiansPerSecondPerRadiansPerSecond
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.perSecond
 
@@ -43,4 +48,30 @@ class PIDControllerTest {
     val aoutput: Value<Acceleration<Meter>> =
       aff.maxAchievableAcceleration(12.0.amps, 1.0.meters.perSecond)
   }
+
+  @Test
+  fun testPID() {
+    val AUTO_LEVEL_KP = 2.0.meters.perSecond / 1.0.degrees // tune this
+    val AUTO_LEVEL_KI = 2.0.meters.perSecond / (1.0.degrees * 1.seconds) // tune this
+    val AUTO_LEVEL_KD = 2.0.meters.perSecond / (1.0.degrees.perSecond) // tune this
+    val AUTO_LEVEL_MAX_VEL_SETPOINT = 5.degrees.perSecond
+    val AUTO_LEVEL_MAX_ACCEL_SETPOINT = 3.degrees.perSecond / 1.0.seconds
+
+    val levelPID =
+      ProfiledPIDController(
+        AUTO_LEVEL_KP,
+        AUTO_LEVEL_KI,
+        AUTO_LEVEL_KD,
+        TrapezoidProfile.Constraints(
+          AUTO_LEVEL_MAX_VEL_SETPOINT, AUTO_LEVEL_MAX_ACCEL_SETPOINT
+        )
+      )
+  }
+
+  @Test
+  fun testConversion() {
+    val kp = (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond
+
+    println(kp.inDegreesPerSecondPerDegreesPerSecond)
+  }
 }