Cleanup and correctly calculate single tag angle error

blair-robot-project · Dec 27, 2023 · f384f7b · f384f7b
1 parent 3a8714d
commit f384f7b
Show file tree

Hide file tree

Showing 7 changed files with 45 additions and 44 deletions.
diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt
@@ -66,8 +66,6 @@ class RobotLoop : TimedRobot(), Logged {
   override fun robotPeriodic() {
     CommandScheduler.getInstance().run()
 
-//    Logger.updateEntries()
-
     robot.field.robotPose = robot.drive.pose
 
     robot.field.getObject("bumpers").pose = robot.drive.pose

diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt
@@ -8,7 +8,10 @@ import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.geometry.Transform2d
 import edu.wpi.first.math.geometry.Translation2d
-import edu.wpi.first.math.kinematics.*
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics
+import edu.wpi.first.math.kinematics.SwerveModulePosition
+import edu.wpi.first.math.kinematics.SwerveModuleState
 import edu.wpi.first.util.sendable.SendableBuilder
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotBase.isReal

diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveSim.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveSim.kt
@@ -58,7 +58,7 @@ class SwerveSim(
         numTargets[index] = presentResult.targetsUsed.size.toDouble()
         tagDistance[index] = 0.0
         avgAmbiguity[index] = 0.0
-        heightError[index] = abs(presentResult.estimatedPose.z - camera.robotToCam.z)
+        heightError[index] = abs(presentResult.estimatedPose.z)
 
         for (tag in presentResult.targetsUsed) {
           val tagPose = camera.estimator.fieldTags.getTagPose(tag.fiducialId)
@@ -78,8 +78,8 @@ class SwerveSim(
         if (presentResult.timestampSeconds > 0 &&
           avgAmbiguity[index] <= VisionConstants.MAX_AMBIGUITY &&
           numTargets[index] < 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_SINGLE_TAG ||
-          numTargets[index] >= 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_MULTI_TAG * (1 + (numTargets[index] - 2) * VisionConstants.TAG_MULTIPLIER)
-//          heightError[index] < VisionConstants.MAX_HEIGHT_ERR_METERS
+          numTargets[index] >= 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_MULTI_TAG * (1 + (numTargets[index] - 2) * VisionConstants.TAG_MULTIPLIER) &&
+          heightError[index] < VisionConstants.MAX_HEIGHT_ERR_METERS
         ) {
           poseEstimator.addVisionMeasurement(
             visionPose,

diff --git a/src/main/kotlin/frc/team449/control/vision/VisionEstimator.kt b/src/main/kotlin/frc/team449/control/vision/VisionEstimator.kt
@@ -166,54 +166,62 @@ class VisionEstimator(
 
     val bestPose = targetPosition
       .get()
-      .transformBy(Transform3d(
-        lowestAmbiguityTarget.bestCameraToTarget.translation,
-        Rotation3d(
-          lowestAmbiguityTarget.bestCameraToTarget.rotation.x,
-          lowestAmbiguityTarget.bestCameraToTarget.rotation.y,
-          driveHeading!!.radians + robotToCam.rotation.z - targetPosition.get().rotation.z
-        )
-      ).inverse())
+      .transformBy(
+        Transform3d(
+          lowestAmbiguityTarget.bestCameraToTarget.translation,
+          Rotation3d(
+            lowestAmbiguityTarget.bestCameraToTarget.rotation.x,
+            lowestAmbiguityTarget.bestCameraToTarget.rotation.y,
+            driveHeading!!.radians + robotToCam.rotation.z - targetPosition.get().rotation.z
+          )
+        ).inverse()
+      )
       .transformBy(robotToCam.inverse())
 
     val altPose = targetPosition
       .get()
-      .transformBy(Transform3d(
-        lowestAmbiguityTarget.alternateCameraToTarget.translation,
-        Rotation3d(
-          lowestAmbiguityTarget.alternateCameraToTarget.rotation.x,
-          lowestAmbiguityTarget.alternateCameraToTarget.rotation.y,
-          driveHeading!!.radians + robotToCam.rotation.z - targetPosition.get().rotation.z
-        )
-      ).inverse())
+      .transformBy(
+        Transform3d(
+          lowestAmbiguityTarget.alternateCameraToTarget.translation,
+          Rotation3d(
+            lowestAmbiguityTarget.alternateCameraToTarget.rotation.x,
+            lowestAmbiguityTarget.alternateCameraToTarget.rotation.y,
+            driveHeading!!.radians + robotToCam.rotation.z - targetPosition.get().rotation.z
+          )
+        ).inverse()
+      )
       .transformBy(robotToCam.inverse())
 
     val usedPose = checkBest(lastPose, bestPose, altPose) ?: bestPose
 
     if (usedPose == bestPose) {
       val camBestPose = targetPosition
         .get()
-        .transformBy(Transform3d(
-          lowestAmbiguityTarget.bestCameraToTarget.translation,
-          lowestAmbiguityTarget.bestCameraToTarget.rotation
-        ).inverse())
+        .transformBy(
+          Transform3d(
+            lowestAmbiguityTarget.bestCameraToTarget.translation,
+            lowestAmbiguityTarget.bestCameraToTarget.rotation
+          ).inverse()
+        )
         .transformBy(robotToCam.inverse())
 
-      if (abs(camBestPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees) > VisionConstants.SINGLE_TAG_HEADING_MAX_DEV_DEG){
+      if (abs(camBestPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees) > VisionConstants.SINGLE_TAG_HEADING_MAX_DEV_DEG) {
         DriverStation.reportWarning("Best Single Tag Heading over Max Deviation, deviated by ${abs(camBestPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees)}", false)
         return Optional.empty()
       }
     } else {
 
       val camAltPose = targetPosition
         .get()
-        .transformBy(Transform3d(
-          lowestAmbiguityTarget.alternateCameraToTarget.translation,
-          lowestAmbiguityTarget.alternateCameraToTarget.rotation
-        ).inverse())
+        .transformBy(
+          Transform3d(
+            lowestAmbiguityTarget.alternateCameraToTarget.translation,
+            lowestAmbiguityTarget.alternateCameraToTarget.rotation
+          ).inverse()
+        )
         .transformBy(robotToCam.inverse())
 
-      if (abs(camAltPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees) > VisionConstants.SINGLE_TAG_HEADING_MAX_DEV_DEG){
+      if (abs(camAltPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees) > VisionConstants.SINGLE_TAG_HEADING_MAX_DEV_DEG) {
         DriverStation.reportWarning("Alt Single Tag Heading over Max Deviation, deviated by ${abs(camAltPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees)}", false)
         return Optional.empty()
       }

diff --git a/src/main/kotlin/frc/team449/control/vision/VisionSubsystem.kt b/src/main/kotlin/frc/team449/control/vision/VisionSubsystem.kt
@@ -15,7 +15,6 @@ import org.photonvision.PhotonCamera
 import org.photonvision.simulation.PhotonCameraSim
 import org.photonvision.simulation.SimCameraProperties
 import org.photonvision.simulation.VisionSystemSim
-import org.photonvision.targeting.PhotonPipelineResult
 import java.util.Optional
 import kotlin.math.abs
 import kotlin.math.pow
@@ -56,10 +55,6 @@ class VisionSubsystem(
     }
   }
 
-  fun getLatestResult(): PhotonPipelineResult {
-    return estimator.camera.latestResult
-  }
-
   private fun getSimDebugField(): Field2d? {
     return if (!RobotBase.isSimulation()) null else visionSystemSim!!.debugField
   }
@@ -76,11 +71,11 @@ class VisionSubsystem(
         }
       ) { if (newResult) getSimDebugField()!!.getObject("VisionEstimation").setPoses() }
     }
-    if (newResult) {
+    return if (newResult) {
       lastEstTimestamp = latestTimestamp
-      return visionEst
+      visionEst
     } else {
-      return Optional.empty()
+      Optional.empty()
     }
   }
 

diff --git a/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt
@@ -3,7 +3,7 @@ package frc.team449.robot2023.constants.drives
 import edu.wpi.first.math.util.Units
 
 object SwerveConstants {
-  const val EFFICIENCY = 0.90
+  const val EFFICIENCY = 0.95
 
   /** Drive motor ports */
   const val DRIVE_MOTOR_FL = 11

diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt
@@ -1,14 +1,11 @@
 package frc.team449.robot2023.subsystems
 
 import edu.wpi.first.math.geometry.Rotation2d
-import edu.wpi.first.math.geometry.Transform2d
-import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.wpilibj.DoubleSolenoid
 import edu.wpi.first.wpilibj.XboxController
 import edu.wpi.first.wpilibj2.command.*
 import edu.wpi.first.wpilibj2.command.button.JoystickButton
 import edu.wpi.first.wpilibj2.command.button.Trigger
-import frc.team449.control.holonomic.SwerveSim
 import frc.team449.robot2023.Robot
 import frc.team449.robot2023.commands.characterization.Characterization
 import frc.team449.robot2023.commands.light.BlairChasing