16750 tuesday auto things, added a net scoring opmode (#68)

* 16750 auto things * Auto Changes for Observation Parking, not working, it's going berserk for some reason - Maggie --------- Co-authored-by: FTC16750 <[email protected]>
technototes · Nov 14, 2024 · 3887756 · 3887756
1 parent 72d0f47
commit 3887756
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 6 deletions.
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java
@@ -13,17 +13,16 @@ public class AutoConstants {
 
 
     //New testing constants for this year's game
-    public static ConfigurablePoseD NET_START = new ConfigurablePoseD(35, 63, 0);
     public static ConfigurablePoseD START = new ConfigurablePoseD(35, 63, 0);
     public static ConfigurablePoseD OBS_START = new ConfigurablePoseD(22, -65, 90);
-    public static ConfigurablePoseD NETSCORING = new ConfigurablePoseD(55, 55, 45);
     public static ConfigurablePoseD NETSCORING_TEST = new ConfigurablePoseD(55, 55, 45);
     public static ConfigurablePoseD NETSCORING_CLEAR = new ConfigurablePoseD(45, 35, -45);
     public static ConfigurablePoseD SPLINETEST1 = new ConfigurablePoseD(0, -55, 0);
     public static ConfigurablePoseD SPLINETEST2 = new ConfigurablePoseD(55, 0, 0);
     public static ConfigurablePoseD OBS_PARK = new ConfigurablePoseD(62, -6, 90);
 
-
+    public static ConfigurablePoseD NETSCORING = new ConfigurablePoseD(55, 55, 45);
+    public static ConfigurablePoseD NET_START = new ConfigurablePoseD(35, 63, 0);
     public static ConfigurablePoseD NETCLEAR = new ConfigurablePoseD(52, 52, 45);
     public static ConfigurablePoseD INTAKE1 = new ConfigurablePoseD(47, 40, 90);
     public static ConfigurablePoseD INTAKE2 = new ConfigurablePoseD(60, 35, 90);

diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java
@@ -11,7 +11,7 @@ public static class Connected {
         public static boolean VERTICALSLIDESUBSYSTEM = true;
         public static boolean HORIZONTALSLIDESUBSYSTEM = true;
         public static boolean BUCKET = false;
-        public static boolean ODOSUBSYSTEM = false;
+        public static boolean ODOSUBSYSTEM = true;
         public static boolean SAFETYSUBSYSTEM = false;
     }
 

diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/NetScoring.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/NetScoring.java
@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.sixteen750.opmodes.auto;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.command.SequentialCommandGroup;
+import com.technototes.library.structure.CommandOpMode;
+import com.technototes.library.util.Alliance;
+import org.firstinspires.ftc.sixteen750.AutoConstants;
+import org.firstinspires.ftc.sixteen750.Hardware;
+import org.firstinspires.ftc.sixteen750.Robot;
+import org.firstinspires.ftc.sixteen750.commands.auto.Paths;
+import org.firstinspires.ftc.sixteen750.controls.DriverController;
+import org.firstinspires.ftc.sixteen750.helpers.StartingPosition;
+
+@Autonomous(name = "NetScoring")
+@SuppressWarnings("unused")
+public class NetScoring extends CommandOpMode {
+
+    public Robot robot;
+    public DriverController controls;
+    public Hardware hardware;
+
+    @Override
+    public void uponInit() {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        hardware = new Hardware(hardwareMap);
+        robot = new Robot(hardware, Alliance.RED, StartingPosition.Observation);
+        robot.drivebase.setPoseEstimate(AutoConstants.FORWARD.toPose());
+        CommandScheduler.scheduleForState(
+            new SequentialCommandGroup(
+                Paths.SampleScoring(robot),
+                CommandScheduler::terminateOpMode
+            ),
+            OpModeState.RUN
+        );
+    }
+}
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Obs_Park.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/Obs_Park.java
@@ -27,8 +27,8 @@ public class Obs_Park extends CommandOpMode {
     public void uponInit() {
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         hardware = new Hardware(hardwareMap);
-        robot = new Robot(hardware, Alliance.RED, StartingPosition.Net);
-        robot.drivebase.setPoseEstimate(AutoConstants.OBS_START.toPose());
+        robot = new Robot(hardware, Alliance.RED, StartingPosition.Observation);
+        robot.drivebase.setPoseEstimate(AutoConstants.BACKWARD.toPose());
         CommandScheduler.scheduleForState(
             new SequentialCommandGroup(Paths.Obs_Parking(robot), CommandScheduler::terminateOpMode),
             OpModeState.RUN