seperate rotation and translation

FTCTeam10298 · Mar 7, 2024 · de57248 · de57248
1 parent 4411425
commit de57248
Showing 1 changed file with 17 additions and 13 deletions.
diff --git 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 =