feat: auto main draft 1

BotsBurgh · Feb 24, 2024 · e63b528 · e63b528
1 parent 1451a67
commit e63b528
Showing 1 changed file with 27 additions and 3 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoMain.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoMain.kt
@@ -2,9 +2,14 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.api.PixelPlacer
 import org.firstinspires.ftc.teamcode.api.Telemetry
 import org.firstinspires.ftc.teamcode.api.TriWheels
+import org.firstinspires.ftc.teamcode.api.linear.AprilMovement
 import org.firstinspires.ftc.teamcode.api.linear.Encoders
+import org.firstinspires.ftc.teamcode.api.vision.AprilVision
+import org.firstinspires.ftc.teamcode.api.vision.AprilVision.optimizeForAprilTags
+import org.firstinspires.ftc.teamcode.api.vision.Vision
 import org.firstinspires.ftc.teamcode.utils.RobotConfig
 import org.firstinspires.ftc.teamcode.utils.Team
 
@@ -27,6 +32,14 @@ abstract class AutoMain : LinearOpMode() {
         TriWheels.init(this)
         Encoders.init(this)
 
+        PixelPlacer.init(this)
+
+        AprilVision.init(this)
+        Vision.init(this, AprilVision)
+        AprilMovement.init(this)
+
+        Vision.optimizeForAprilTags()
+
         Telemetry.sayInitialized()
 
         waitForStart()
@@ -35,7 +48,20 @@ abstract class AutoMain : LinearOpMode() {
 
         sleep(RobotConfig.AutoMain.WAIT_TIME)
 
-        // TODO: Rest of the autonomous.
+        Encoders.driveTo2(forward, tiles(1) + 6.0)
+        Encoders.spinTo2(pickTeam(90.0, -90.0))
+        Encoders.driveTo2(forward, tiles(0.5))
+
+        AprilMovement.driveTo(pickTeam(5, 2), 3.0)
+
+        PixelPlacer.place()
+
+        sleep(1000)
+
+        PixelPlacer.reset()
+
+        Encoders.spinTo2(pickTeam(90.0, -90.0))
+        Encoders.driveTo2(forward, tiles(1.2))
     }
 
     /** Converts an amount of tiles to inches. */
@@ -57,8 +83,6 @@ abstract class AutoMain : LinearOpMode() {
             Team.Red -> red
             Team.Blue -> blue
         }
-
-    data class Config(val team: Team)
 }
 
 @Autonomous(name = "AutoMain - Red")