a bunch of code to try and correct sim visual (messy pose setting)

blair-robot-project · Feb 7, 2025 · 9a12cb1 · 9a12cb1
1 parent 733e6e9
commit 9a12cb1
Show file tree

Hide file tree

Showing 5 changed files with 66 additions and 5 deletions.
diff --git a/src/main/kotlin/frc/team449/ControllerBindings.kt b/src/main/kotlin/frc/team449/ControllerBindings.kt
@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
+import edu.wpi.first.wpilibj2.command.PrintCommand
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import frc.team449.commands.driveAlign.SimpleReefAlign
@@ -42,6 +43,13 @@ class ControllerBindings(
     premove_l4()
 
     stow()
+
+    mechanismController.pov(0).onTrue(
+      robot.wrist.setPosition(PI/2)
+    )
+    mechanismController.pov(180).onTrue(
+      robot.wrist.setPosition(0.0)
+    )
   }
 
   private fun nonRobotBindings() {

diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt
@@ -5,6 +5,10 @@ import dev.doglog.DogLog
 import dev.doglog.DogLogOptions
 import edu.wpi.first.hal.FRCNetComm
 import edu.wpi.first.hal.HAL
+import edu.wpi.first.math.geometry.Pose3d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Rotation3d
+import edu.wpi.first.math.geometry.Transform3d
 import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.*
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
@@ -25,6 +29,7 @@ import frc.team449.system.encoder.QuadCalibration
 import org.littletonrobotics.urcl.URCL
 import kotlin.jvm.optionals.getOrDefault
 import kotlin.jvm.optionals.getOrNull
+import kotlin.math.*
 
 /** The main class of the robot, constructs all the subsystems
  * and initializes default commands . */
@@ -38,6 +43,10 @@ class RobotLoop : TimedRobot() {
   private var routineMap = hashMapOf<String, Command>()
   private val controllerBinder = ControllerBindings(robot.driveController, robot.mechController, robot)
 
+  private var componentStorage: Array<Pose3d>? = null
+  private var wristMove: Pose3d? = null
+
+
   override fun robotInit() {
     // Yes this should be a print statement, it's useful to know that robotInit started.
     println("Started robotInit.")
@@ -161,9 +170,53 @@ class RobotLoop : TimedRobot() {
 
   override fun testPeriodic() {}
 
-  override fun simulationInit() {}
+  override fun simulationInit() {
+    // set position
+//    componentStorage = arrayOf(
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(-0.136, 0.0, 0.245,
+//        Rotation3d(0.0, 0.0, 0.0)),
+//      Pose3d(0.61, 0.0, 0.327,
+//        Rotation3d(0.0, 0.0, 0.0)))
+//    componentStorage = arrayOf(Pose3d(),
+//      Pose3d(),
+//      Pose3d(),
+//      Pose3d(),
+//      Pose3d(0.0, 0.0, 0.0,
+//        Rotation3d(0.0, 0.0, 0.0)))
+    robot.poseSubsystem.enableVisionFusion = false
+  }
 
   override fun simulationPeriodic() {
+    // sim pose calculator
+
+//    componentStorage[4]
+    val pivotPos = -robot.pivot.positionSupplier.get()
+
+    componentStorage = arrayOf(
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+      Pose3d(-0.136, 0.0, 0.245,
+        Rotation3d(0.0, pivotPos, 0.0)),
+//      Pose3d(0.569, 0.0, 0.366,
+//        Rotation3d(0.0, -robot.wrist.positionSupplier.get(), 0.0))
+      Pose3d(sqrt(0.569.pow(2) + 0.366.pow(2))*cos(-pivotPos + atan(0.569)),
+        0.0,
+        sqrt(0.569.pow(2) + 0.366.pow(2))*sin(-pivotPos + atan(0.366)),
+        Rotation3d(0.0, -robot.wrist.positionSupplier.get(), 0.0))
+    )
+    //sqrt(x0^2+y0^2)*<cos, sin>(theta + atan(y0, x0))
+    DogLog.log("SimComponent", componentStorage)
+
     robot.drive as SwerveSim
 
     VisionConstants.ESTIMATORS.forEach {

diff --git a/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt b/src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveOrthogonalCommand.kt
@@ -157,12 +157,12 @@ class SwerveOrthogonalCommand(
       rotRamp.calculate(
         min(
           MathUtil.applyDeadband(
-            abs(controller.rightX).pow(SwerveConstants.ROT_FILTER_ORDER),
+            abs(controller.getRawAxis(2)).pow(SwerveConstants.ROT_FILTER_ORDER),
             RobotConstants.ROTATION_DEADBAND,
             1.0
           ),
           1.0
-        ) * -sign(controller.rightX) * drive.maxRotSpeed
+        ) * -sign(controller.getRawAxis(2)) * drive.maxRotSpeed
       )
     } else {
       if (checkSnapToAngleTolerance()) headingLock = false

diff --git a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt
@@ -38,7 +38,8 @@ class SuperstructureManager(
       )
   }
 
-  companion object {
+
+    companion object {
     fun createSuperstructureManager(robot: Robot): SuperstructureManager {
       return SuperstructureManager(
         robot.elevator,

diff --git a/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt b/src/main/kotlin/frc/team449/subsystems/vision/ReefOnlyEstimator.kt
@@ -119,7 +119,6 @@ class ReefOnlyEstimator(
       ) {
         lowestAmbiguityScore = targetPoseAmbiguity
         lowestAmbiguityTarget = target
-        print("getting here")
       }
     }
-Original file line number
+Diff line change
@@ Expand Up / @@ -119,7 +119,6 @@ class ReefOnlyEstimator( @@
           ) {
             lowestAmbiguityScore = targetPoseAmbiguity
             lowestAmbiguityTarget = target
-            print("getting here")
           }
         }
@@ Expand Down @@