we're doing this again (#14)

* Made slides go to the perfect position to hang and then they fall down trying to get to 0 ticks but then when it's hanging the motors grind a bit.

* Used TFOD sample to detect the white pixel

* Fixes, template auto

* more changes

* Made gamepad1 use button y to make the slides go up but not back down.

* Dumbest autonomous strafing to the right.

* More Auto Stuff in the making

* More Auto Stuff in the making

* Added the claw to drop two pixels in the backstage in auto

* Added the claw to drop two pixels in the backstage in auto

* Added the claw to drop two pixels in the backstage in auto

* Made all simple autonomous code

* Made all simple autonomous code with the beginning meaning after init is pressed the claw locks and goes into stow mode.

* Finished autos.

* I Forgor to commit D:

* Changes! Fri Nov 10 3:30

* Merge conflicts

* [skip ci] move magic numbers to Var.kt

* Finished autos.

* Finished autos.

* Motor balancing

* Retune, bug fix

* asyncify gamepad1 Y button

* oopsie

* more timing

* Lift speeds as tick/second

+ separate lift speeds for p1 and p2
+ X button can be struck by truck >:(
+ flicc the sticc to go to lift presets

* MORE Var.kt WHEEE

* Tweaks

* Hardware Test + value tweaks

* Detects red solo cup prototype.

* commit forgor D:

* BROKEN IMPL!! uhh yeah

* Simple auto with PID.

* Rwekrts

* The jank real

* Bill-mandated utility script

* Added IMU thing :)

* liam changes

* compiler fix

* Intake controls

* here's your heckin vision processor

* added timer into the loop for the motion profile and started the code for the pipeline for the blue team prop in java.

* I <3 JAVA.LANG.REFLECT :DDD

* Claw obliterated, other changes

* IT WORKS, WOAH

* Made the turning subroutine.

* panic

* Reconcilibility shenanigans

* Bill helps me fix the code

* experimental strafe code

* TurnPID is not an opmode

* Made the turning subroutine possibly work for negative angles. I also corrected for error.

* Twenty Point Auto + Bug Fixes

* Retuned corrective factors

* More configuration vars

* FUNKAY

* panic commit lul

* Quick hanging fix

* WORKING AUTO YEAHHHHHHh

* WOAH PARKING

* Parking 2.0

* WHOOPS

* The circles works!!!

* Fix tests

* Kinda bad button

* retune, remove weird back button thing Liam made me do in 3 minutes

* Clean

* Canonicalize using Pair<Task,Task> for then()

* WIP: Turn odo drive into a Task
TODO: Check strafeLeft, and make it actually finish (tweak power)

* tweak intake speed

* AO2.0

* FIELD CENTRIC 🎉🎉🎉

* okay

* don't call to() and inches() every single iteration

(yes it looks like a field access, but in Kotlin it's actually a property = a method call)

* Code review suggestions

* Reformat code

* use the right motion function dummy

* woah it works ?!?!?

* WHEEE

* Added the p constant and made a ramp down in the turning subroutine.

* Correct TurnPID for other direction

* damn it broke

* wipes sweat

* Update HEY NISHK.md

* Update HEY NISHK.md

* Got the the servo position for releasing only the top pixel. Worked on the java version of the strafing code.

* Implement suggested changes

* Adjust TODOs and FIXMEs

* Change odometry configurations

* fix wrong names

* This strafe program works well and slightly over shoots by an inch.

* Made the ramp down slightly higher for strafing so that it's more accurate.

* Strafing left works but retest after we put the intake on.

* Duplicate changes to Kotlin

* Rework dumper

* generify MotorSet

* use OtherParking

* Made a seperate auto called TwentyBlueRightTest which includes all the robot movements to move the robot from the beginning to the front of their respective backdrop april tags. Miles needs to add the pixel dropper code and vision code in here.

* fix issues with generic MotorSet (this does literally nothing after compilation)

* rm left stick preset & add A button

* logs fun fun fun

* Dep. DriveForwardPID, forward/reverse curve sync

* why ki broke?

* well I fucked up the Kotlin

* yeah ok what the hell is going on here

* capture originals

* it kinda works

* Bring strafing parity up to date (?)

* try to implement turning

* BUG LOCATED

* (untested) don't conflate speeds and powers

maybe rename MotorPowers because it's just being used as a generic Motor-anything container now?

* make names less confusing

* Clean up Imports

* Clean up Lines

* i <3 reversions

* wheeeeeeeee

* the code is EVEN WORSE NOW WHEEEEEE

use TwentyAutoRedLeft/Right
use TeleOp

* I HATE THIS CODEBASE AAAAAAAAAaaa

* Strafe working. Canonicals: drive, strafe: kotlin; rotate: java

* HELL YEAH BLUE LEFT WORKS

* Red Right works also

* Blue Right ✅

* No jtr

* Timer conf on init scrn. Blue side not tested with timer.

* yeah so tests broke lol

---------

Co-authored-by: Nishk04 <nishkjimikshah@gmail.com>
Co-authored-by: Liam Doran <65523663+Minerbot360@users.noreply.github.com>

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AprilTagToPose.kt

@@ -1,28 +1,38 @@
 package org.firstinspires.ftc.teamcode
 
-import org.firstinspires.ftc.teamcode.odo.Pose
-import org.firstinspires.ftc.teamcode.odo.degrees
-import org.firstinspires.ftc.teamcode.odo.inches
-import org.firstinspires.ftc.teamcode.utility.Vector2
+import org.firstinspires.ftc.teamcode.utilities.Pose
+import org.firstinspires.ftc.teamcode.utilities.Vector2
+import org.firstinspires.ftc.teamcode.utilities.degrees
+import org.firstinspires.ftc.teamcode.utilities.inches
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection
 import kotlin.math.abs
 import kotlin.math.cos
 import kotlin.math.pow
 import kotlin.math.sin
 import kotlin.math.sqrt
 
-private const val DISTANCE_FROM_FRONT = (142.0-10.75)
+private const val DISTANCE_FROM_FRONT = (142.0 - 10.75)
 val tagPositions = mapOf(
     1 to Vector2(29.5, DISTANCE_FROM_FRONT),
-    3 to Vector2(41.5, DISTANCE_FROM_FRONT),
     2 to Vector2(35.5, DISTANCE_FROM_FRONT),
+    3 to Vector2(41.5, DISTANCE_FROM_FRONT),
+
+    4 to Vector2(101.5, DISTANCE_FROM_FRONT),
+    5 to Vector2(107.5, DISTANCE_FROM_FRONT),
+    6 to Vector2(113.5, DISTANCE_FROM_FRONT),
 )
 
+/**
+ * this doesn't work
+ */
+@Deprecated("Literally doesn't work LMAO")
 fun detectPairToPose(first: AprilTagDetection, second: AprilTagDetection): Pose {
     val kA = first.ftcPose.range
     val kB = second.ftcPose.range
-    val firstPosition = tagPositions[first.id] ?: throw IllegalArgumentException("where is tag#${first.id}")
-    val secondPosition = tagPositions[second.id] ?: throw IllegalArgumentException("where is tag#${second.id}")
+    val firstPosition =
+        tagPositions[first.id] ?: throw IllegalArgumentException("where is tag#${first.id}")
+    val secondPosition =
+        tagPositions[second.id] ?: throw IllegalArgumentException("where is tag#${second.id}")
     val avgPose = firstPosition.add(secondPosition).scale(0.5)
 
     // d = half distance between the two tags
@@ -38,19 +48,24 @@ fun detectPairToPose(first: AprilTagDetection, second: AprilTagDetection): Pose
     // val y4 = sqrt(kB.pow(2.0) - (x - d).pow(2.0))
     // add to the midpoint between the two tags (tag 1 + D or tag 2 - D)
 
-    return Pose((x+avgPose.x).inches, (avgPose.y-y1).inches, ((first.ftcPose.yaw + second.ftcPose.yaw) / -2.0 + 90.0).degrees)
+    return Pose(
+        (x + avgPose.x).inches,
+        (avgPose.y - y1).inches,
+        ((first.ftcPose.yaw + second.ftcPose.yaw) / -2.0 + 90.0).degrees
+    )
 }
 
 fun detectSingleToPose(detection: AprilTagDetection): Pose {
     val k = detection.ftcPose.range
-    val phi: Double = -detection.ftcPose.yaw.degrees.to.radians.value
+    val criticalAngle: Double =
+        (-detection.ftcPose.yaw.degrees.to.radians.value) + (detection.ftcPose.bearing.degrees.to.radians.value)
     // the yaw happens to be the angle at the camera
     // such that the two legs of a right triangle w/ phi are aligned with the tag
-    val globalX = k * sin(phi) // would be Y rel. to camera
-    val globalY = k * cos(phi) // would be X rel. to camera
+    val globalX = k * sin(criticalAngle) // would be Y rel. to camera
+    val globalY = k * cos(criticalAngle) // would be X rel. to camera
     val tagPos = tagPositions[detection.id]
         ?: throw java.lang.IllegalArgumentException("where is tag#${detection.id}")
     // offset the relative position with the known pos of the tag
     val globalAll = tagPos.add(Vector2(globalX, -globalY))
-    return Pose(globalAll.x.inches, globalAll.y.inches, (-detection.ftcPose.yaw + 90.0).degrees)
+    return Pose(globalAll.x.inches, globalAll.y.inches, (-detection.ftcPose.yaw - 90.0).degrees)
 }