From de57248cb703504d886aae67df52a5950283329a Mon Sep 17 00:00:00 2001
From: stheyounger <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>
Date: Wed, 6 Mar 2024 20:30:38 -0800
Subject: [PATCH] seperate rotation and translation

---
 .../us/brainstormz/robotTwo/RobotTwoAuto.kt   | 30 +++++++++++--------
 1 file changed, 17 insertions(+), 13 deletions(-)

diff --git a/RoboCode/src/main/java/us/brainstormz/robotTwo/RobotTwoAuto.kt b/RoboCode/src/main/java/us/brainstormz/robotTwo/RobotTwoAuto.kt
index c7bd94cb..1e76de5d 100644
--- a/RoboCode/src/main/java/us/brainstormz/robotTwo/RobotTwoAuto.kt
+++ b/RoboCode/src/main/java/us/brainstormz/robotTwo/RobotTwoAuto.kt
@@ -556,7 +556,7 @@ class RobotTwoAuto(private val telemetry: Telemetry, private val aprilTagPipelin
                     ),
                     navigatingTargetSetup(
                             targetPosition =
-                                startPosition.copy(x= redDistanceFromCenterlineInches - 5, y = 52.0, r = 0.0),
+                                startPosition.copy(x= redDistanceFromCenterlineInches - 5, y = 52.0),
                             isTargetReached = ::isRobotAtAngle
                     ),
             )
@@ -580,18 +580,22 @@ class RobotTwoAuto(private val telemetry: Telemetry, private val aprilTagPipelin
                         ),
                 )
             }
-        } + listOf(navigatingTargetSetup(
-                targetPosition = readyToGoAroundTrussWaypoint,
-                isTargetReached = { targetState: TargetWorld, actualState: ActualWorld, previousActualState: ActualWorld ->
-                    val isRobotAtPosition = isRobotAtPosition(targetState, actualState, previousActualState)
-
-                    drivetrain.rotationPID = if (isRobotAtPosition)
-                        drivetrain.rotationOnlyPID
-                    else
-                        drivetrain.rotationWithOtherAxisPID
-
-                    isRobotAtPosition
-                }))
+        } + listOf(
+                navigatingTargetSetup(
+                        targetPosition = readyToGoAroundTrussWaypoint.copy(r = startPosition.r),
+                        isTargetReached = { targetState: TargetWorld, actualState: ActualWorld, previousActualState: ActualWorld ->
+                            val isRobotAtPosition = isRobotAtPosition(targetState, actualState, previousActualState)
+                            isRobotAtPosition
+                        }),
+                navigatingTargetSetup(
+                        targetPosition = readyToGoAroundTrussWaypoint,
+                        isTargetReached = { targetState: TargetWorld, actualState: ActualWorld, previousActualState: ActualWorld ->
+                            val isRobotAtPosition = isRobotAtPosition(targetState, actualState, previousActualState)
+                            isRobotAtPosition
+                        }),
+
+
+                )
 
         val driveToBackboard = listOf(
                 navigatingTargetSetup(targetPosition =