retune v2

Author: Craftzman7

src/main/kotlin/com/frcteam3636/frc2025/Dashboard.kt

@@ -36,4 +36,5 @@ enum class AutoModes(val autoName: String, val developerAuto: Boolean = false, v
     TestAutoOneCoral("Test Auto 1 Coral", true, false),
     TestAutoTwoCoral("Test Auto 2 Coral", true, false),
     TestAutoThreeCoral("Test Auto 3 Coral", true, false),
+    TestAutoFourCoral("Test Auto 4 Coral", true, false),
 }

src/main/kotlin/com/frcteam3636/frc2025/Robot.kt

@@ -317,6 +317,7 @@ object Robot : LoggedRobot() {
                 AutoModes.TestAutoOneCoral -> TestAuto().autoSequence()
                 AutoModes.TestAutoTwoCoral -> TestAutoTwoCoral().autoSequence()
                 AutoModes.TestAutoThreeCoral -> TestAutoThreeCoral().autoSequence()
+                AutoModes.TestAutoFourCoral -> TestAutoFourCoral().autoSequence()
                 AutoModes.None -> Commands.none()
             }
         }
@@ -336,6 +337,7 @@ object Robot : LoggedRobot() {
             AutoModes.TestAutoOneCoral -> TestAuto().autoSequence()
             AutoModes.TestAutoTwoCoral -> TestAutoTwoCoral().autoSequence()
             AutoModes.TestAutoThreeCoral -> TestAutoThreeCoral().autoSequence()
+            AutoModes.TestAutoFourCoral -> TestAutoFourCoral().autoSequence()
             AutoModes.None -> Commands.none()
         }
     }

src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt

@@ -668,7 +668,7 @@ object Drivetrain : Subsystem {
                 Pose2d(target.translation, heading)
             )
 
-            val constraints = PathConstraints(10.0, 3.0, 2 * Math.PI, 4 * Math.PI)
+            val constraints = AutoMode.DEFAULT_AUTO_CONSTRAINTS
 
             val constraintZone = ConstraintsZone(
                 0.85, 1.0, AutoMode.DEFAULT_AUTO_CONSTRAINTS_SLOW_ZONE
@@ -859,7 +859,7 @@ object Drivetrain : Subsystem {
         val BRAKE_POSITION = MODULE_POSITIONS.map { position -> SwerveModuleState(0.0, position.translation.angle) }
 
 
-        val ALIGN_TRANSLATION_PID_GAINS = PIDGains(16.0)
+        val ALIGN_TRANSLATION_PID_GAINS = PIDGains(21.0)
         val ALIGN_ROTATION_PID_GAINS = PIDGains(5.0)
     }
 }

src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/autos/TestAuto.kt

@@ -13,12 +13,11 @@ class TestAuto() : AutoMode() {
         val reefPose = AprilTagTarget(18, ReefBranchSide.Right).pose
 
         return Commands.sequence(
-            Drivetrain.driveToPointAllianceRelativeWithSlowZone(
+            Drivetrain.driveToPointAllianceRelativeWithSlowConstraintZone(
                 reefPose,
                 DEFAULT_AUTO_CONSTRAINTS,
                 DEFAULT_AUTO_CONSTRAINTS_SLOW_ZONE,
                 SLOW_ZONE_DISTANCE,
-                SLOW_ZONE_ENTER_VELOCITY
             ),
             Manipulator.outtake().withTimeout(OUTTAKE_TIMEOUT),
             Elevator.setTargetHeight(Elevator.Position.Stowed).onlyIf { shouldAutoStow },

src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/autos/TestAutoFourCoral.kt

@@ -0,0 +1,52 @@
+package com.frcteam3636.frc2025.subsystems.drivetrain.autos
+
+import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain
+import com.frcteam3636.frc2025.subsystems.drivetrain.poi.AprilTagTarget
+import com.frcteam3636.frc2025.subsystems.drivetrain.poi.ReefBranchSide
+import com.frcteam3636.frc2025.subsystems.elevator.Elevator
+import com.frcteam3636.frc2025.subsystems.funnel.Funnel
+import com.frcteam3636.frc2025.subsystems.manipulator.CoralState
+import com.frcteam3636.frc2025.subsystems.manipulator.Manipulator
+import com.frcteam3636.frc2025.utils.math.meters
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
+
+class TestAutoFourCoral : AutoMode() {
+    override fun autoSequence(shouldAutoStow: Boolean): Command {
+        val reefPose = AprilTagTarget(18, ReefBranchSide.Right).pose
+        val pickupPose = Pose2d(1.251.meters, 4.034.meters, Rotation2d.kZero)
+
+        return Commands.sequence(
+            TestAutoThreeCoral().autoSequence(false),
+            Commands.parallel(
+                Elevator.setTargetHeight(Elevator.Position.Stowed),
+                Drivetrain.driveToPointAllianceRelative(pickupPose, DEFAULT_AUTO_CONSTRAINTS)
+            ),
+            Commands.parallel(
+                Commands.sequence(
+                    Commands.race(
+                        Commands.waitUntil {
+                            Manipulator.coralState != CoralState.NONE
+                        },
+                        Commands.waitSeconds(5.0)
+                    ),
+                    Drivetrain.driveToPointAllianceRelativeWithSlowConstraintZone(
+                        reefPose,
+                        DEFAULT_AUTO_CONSTRAINTS,
+                        DEFAULT_AUTO_CONSTRAINTS_SLOW_ZONE,
+                        SLOW_ZONE_DISTANCE,
+                        raisePoint = Elevator.Position.MidBar
+                    ),
+                ),
+                Commands.race(
+                    Manipulator.intakeAuto(),
+                    Funnel.intake()
+                ).withTimeout(5.0),
+            ),
+            Manipulator.outtake().withTimeout(OUTTAKE_TIMEOUT),
+            Elevator.setTargetHeight(Elevator.Position.Stowed).onlyIf { shouldAutoStow },
+        )
+    }
+}

src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/autos/TestAutoThreeCoral.kt

@@ -32,12 +32,11 @@ class TestAutoThreeCoral() : AutoMode() {
                         },
                         Commands.waitSeconds(5.0)
                     ),
-                    Drivetrain.driveToPointAllianceRelativeWithSlowZone(
+                    Drivetrain.driveToPointAllianceRelativeWithSlowConstraintZone(
                         reefPose,
                         DEFAULT_AUTO_CONSTRAINTS,
                         DEFAULT_AUTO_CONSTRAINTS_SLOW_ZONE,
                         SLOW_ZONE_DISTANCE,
-                        SLOW_ZONE_ENTER_VELOCITY,
                         raisePoint = Elevator.Position.MidBar
                     ),
                 ),

src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/autos/TestAutoTwoCoral.kt

@@ -32,12 +32,11 @@ class TestAutoTwoCoral() : AutoMode() {
                         },
                         Commands.waitSeconds(INTAKE_TIMEOUT)
                     ),
-                    Drivetrain.driveToPointAllianceRelativeWithSlowZone(
+                    Drivetrain.driveToPointAllianceRelativeWithSlowConstraintZone(
                         reefPose,
                         DEFAULT_AUTO_CONSTRAINTS,
                         DEFAULT_AUTO_CONSTRAINTS_SLOW_ZONE,
                         SLOW_ZONE_DISTANCE,
-                        SLOW_ZONE_ENTER_VELOCITY
                     ),
                 ),
                 Commands.race(

src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/poi/AlignTargets.kt

@@ -283,4 +283,4 @@ private enum class FieldOffset(val distance: Distance) {
     }
 }
 
-private val REEF_DISTANCE_OFFSET = (0.75).inches
+private val REEF_DISTANCE_OFFSET = 0.5.inches