diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java index fffbcec4..0c4caf3b 100644 --- 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 index d25c274a..76d47785 100644 --- 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 new file mode 100644 index 00000000..40a50300 --- /dev/null +++ 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 index 90c74f27..4322162f 100644 --- 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