diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/configs/AutoConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/configs/AutoConfig.java index 8d92434..0c6231f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/configs/AutoConfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/configs/AutoConfig.java @@ -5,6 +5,8 @@ import com.acmerobotics.dashboard.config.Config; @Config public class AutoConfig { + //REMEMBER THAT LEFT IS POS 1, RIGHT IS POS 3 + public static int OBJECT_CENTER_Y = 135; public static int MIN_H = 100; public static int MIN_S = 100; @@ -16,13 +18,20 @@ public class AutoConfig { //carousel values public static double CAROUSEL_POWER = 0.2; public static int CAROUSEL_WAIT = 6000; + //carousel positions + //blue public static double BLUE_CAROUSEL_X = -60; public static double BLUE_CAROUSEL_Y = 58; public static double BLUE_CAROUSEL_ANGLE = 180; + //red + public static double RED_CAROUSEL_X = -60; + public static double RED_CAROUSEL_Y = -58; + public static double RED_CAROUSEL_ANGLE = 180; - // arm positions + + //arm positions public static int ARM_MAX_DIST = 415; public static int SLIGHTLY_OFF_GROUND = 40; public static int LAYER_ONE_BACK_ARM_POS = 340; @@ -38,9 +47,27 @@ public class AutoConfig { public static double GRABBER_OPEN = 0.6; //storage coords - public static double BLUE_STORAGE__X = -60; + //blue + public static double BLUE_STORAGE_X = -60; public static double BLUE_STORAGE_Y = 36; public static double BLUE_STORAGE_ANGLE = 360; + //red + public static double RED_STORAGE_X = -60; + public static double RED_STORAGE_Y = -36; + public static double RED_STORAGE_ANGLE = 0; + + + //warehouse coords + //blue + public static double BLUE_WAREHOUSE_X = -60; + public static double BLUE_WAREHOUSE_Y = 36; + public static double BLUE_WAREHOUSE_ANGLE = 360; + + //red + public static double RED_WAREHOUSE_X = -60; + public static double RED_WAREHOUSE_Y = 36; + public static double RED_WAREHOUSE_ANGLE = 360; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/ActionObject.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/ActionObject.java index 75b5ff5..4460acb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/ActionObject.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/ActionObject.java @@ -12,6 +12,7 @@ public class ActionObject { this.y = y; this.angle = angle; } + public ActionObject(double x, double y, double angle, int methodID){ this.x = x; this.y = y; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/EnhancedDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/EnhancedDriver.java index f0916db..b32c0b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/EnhancedDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/EnhancedDriver.java @@ -10,6 +10,7 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -81,7 +82,8 @@ public class EnhancedDriver extends SampleMecanumDrive{ case 3: turnCarouselMethods(subIndex); break; - + case 4: + executeAdvancedMovement(subIndex); } } @@ -171,6 +173,23 @@ public class EnhancedDriver extends SampleMecanumDrive{ carouselMotor2.setPower(0); } + private void executeAdvancedMovement(int id) throws IndexOutOfBoundsException{ + switch(id){ + //moves to the leftmost edge (blue side) and drives into warehouse + case 1: + Trajectory traj = trajectoryBuilder(getPoseEstimate()) + .lineToLinearHeading() + .lineToLinearHeading() + + + //moves to the rightmost edge (red side) and drives into warehouse + case 2: + + + } + } + + private void moveArms(int encoderVal){ armOne.setTargetPosition(armOne.getCurrentPosition() - encoderVal); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/MethodRefrences.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/MethodRefrences.txt index d657455..d02319b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/MethodRefrences.txt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/driveobjs/MethodRefrences.txt @@ -5,6 +5,8 @@ 13: bottom layer from front 14: max position 2X: grabber motions + 20: closes grabber and lifts up a little + START WITH THIS ONE!!!! 21: close grabber 22: open grabber 3X: carousel movements diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBase.java index 015faaa..31c83a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBase.java @@ -1,18 +1,79 @@ package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; +import static org.firstinspires.ftc.teamcode.createdcode.configs.AutoConfig.*; + +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@Disabled @Autonomous @Config -public class AutoBase extends enhancedAutoMode{ +public class AutoBase extends EnhancedAutoMode { + public static double START_POS_X = 0; + public static double START_POS_Y = 0; + public static double START_POS_ANGLE = 0; + public static int START_METHOD = 20; + + public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0; + public static int SHIPPING_ELEMENT_POS_1_X = 0; + public static int SHIPPING_ELEMENT_POS_2_X = 0; + public static int SHIPPING_ELEMENT_POS_3_X = 0; + + + FtcDashboard dashboard = FtcDashboard.getInstance(); + Telemetry dashboardTelemetry = dashboard.getTelemetry(); + + public static double[] xCoordArr = {}; private static double[] yCoordArr = {}; private static double[] angleArr = {}; private static int[] methodIdArr = {}; + public static double[] xCoordArrCase1 = {START_POS_X}; + private static double[] yCoordArrCase1 = {START_POS_Y}; + private static double[] angleArrCase1 = {START_POS_ANGLE}; + private static int[] methodIdArrCase1 = {START_METHOD}; + + public static double[] xCoordArrCase2 = {START_POS_X}; + private static double[] yCoordArrCase2 = {START_POS_Y}; + private static double[] angleArrCase2 = {START_POS_ANGLE}; + private static int[] methodIdArrCase2 = {START_METHOD}; + + public static double[] xCoordArrCase3 = {START_POS_X}; + private static double[] yCoordArrCase3 = {START_POS_Y}; + private static double[] angleArrCase3 = {START_POS_ANGLE}; + private static int[] methodIdArrCase3 = {START_METHOD}; + @Override public void runOpMode(){ + int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X); + dashboardTelemetry.addData("Position Detected is", pos); + dashboardTelemetry.update(); + + if (pos == 1) { + xCoordArr = xCoordArrCase1; + yCoordArr = yCoordArrCase1; + angleArr = angleArrCase1; + methodIdArr = methodIdArrCase1; + } + else if (pos == 2){ + xCoordArr = xCoordArrCase2; + yCoordArr = yCoordArrCase2; + angleArr = angleArrCase2; + methodIdArr = methodIdArrCase2; + } + else if (pos == 3) { + xCoordArr = xCoordArrCase3; + yCoordArr = yCoordArrCase3; + angleArr = angleArrCase3; + methodIdArr = methodIdArrCase3; + } + + initThings(); setXCoords(xCoordArr); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueNearParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueNearParkAuto.java index 3f21507..55bca46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueNearParkAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueNearParkAuto.java @@ -9,14 +9,16 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.robotcore.external.Telemetry; -@Autonomous +@Autonomous (group = "Blue") @Config -public class BlueNearParkAuto extends enhancedAutoMode{ +public class BlueNearParkAuto extends EnhancedAutoMode { public static double START_POS_X = -32; public static double START_POS_Y = 66; public static double START_POS_ANGLE = 270; public static int START_METHOD = 20; + public static int BLOCK_DROP_POINT = -30; + public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 135; public static int SHIPPING_ELEMENT_POS_1_X = 30; public static int SHIPPING_ELEMENT_POS_2_X = 165; @@ -32,17 +34,17 @@ public class BlueNearParkAuto extends enhancedAutoMode{ private static double[] angleArr = {}; private static int[] methodIdArr = {}; - public static double[] xCoordArrCase1 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, BLUE_CAROUSEL_X, BLUE_STORAGE__X}; + public static double[] xCoordArrCase1 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, BLUE_CAROUSEL_X, BLUE_STORAGE_X}; private static double[] yCoordArrCase1 = {START_POS_Y, 48, 24, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y}; private static double[] angleArrCase1 = {START_POS_ANGLE, 180, 90, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE}; private static int[] methodIdArrCase1 = {START_METHOD, 13, 0, 22, 10, 32, 0}; - public static double[] xCoordArrCase2 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, BLUE_CAROUSEL_X, BLUE_STORAGE__X}; + public static double[] xCoordArrCase2 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, BLUE_CAROUSEL_X, BLUE_STORAGE_X}; private static double[] yCoordArrCase2 = {START_POS_Y, 48, 24, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y}; private static double[] angleArrCase2 = {START_POS_ANGLE, 180, 90, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE}; private static int[] methodIdArrCase2 = {START_METHOD, 12 , 0, 22, 10, 32, 0}; - public static double[] xCoordArrCase3 = {START_POS_X, BLOCK_DROP_POINT, BLOCK_DROP_POINT, -56, BLUE_CAROUSEL_X, BLUE_STORAGE__X}; + public static double[] xCoordArrCase3 = {START_POS_X, BLOCK_DROP_POINT, BLOCK_DROP_POINT, -56, BLUE_CAROUSEL_X, BLUE_STORAGE_X}; private static double[] yCoordArrCase3 = {START_POS_Y, 48, 24, 24, BLUE_CAROUSEL_Y, BLUE_STORAGE_Y}; private static double[] angleArrCase3 = {START_POS_ANGLE, 135, 0, 0, BLUE_CAROUSEL_ANGLE, BLUE_STORAGE_ANGLE}; private static int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 32, 0}; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueWarehouseSideAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueWarehouseSideAuto.java new file mode 100644 index 0000000..bb0f17b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/BlueWarehouseSideAuto.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + + +@Autonomous (group = "Blue") +@Config +public class BlueWarehouseSideAuto extends EnhancedAutoMode { + public static double START_POS_X = 16; + public static double START_POS_Y = 66; + public static double START_POS_ANGLE = -90; + public static int START_METHOD = 20; + + //TODO: Get vals + public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0; + public static int SHIPPING_ELEMENT_POS_1_X = 0; + public static int SHIPPING_ELEMENT_POS_2_X = 0; + public static int SHIPPING_ELEMENT_POS_3_X = 0; + + + FtcDashboard dashboard = FtcDashboard.getInstance(); + Telemetry dashboardTelemetry = dashboard.getTelemetry(); + + + public static double[] xCoordArr = {}; + private static double[] yCoordArr = {}; + private static double[] angleArr = {}; + private static int[] methodIdArr = {}; + + public static double[] xCoordArrCase1 = {START_POS_X, -12. -12, 36}; + private static double[] yCoordArrCase1 = {START_POS_Y, 64, 36, 68}; + private static double[] angleArrCase1 = {START_POS_ANGLE, -90, -90, 0}; + private static int[] methodIdArrCase1 = {START_METHOD, 13, 22, 10, 0}; + + public static double[] xCoordArrCase2 = {START_POS_X, -12. -12, 36}; + private static double[] yCoordArrCase2 = {START_POS_Y, 64, 36, 68}; + private static double[] angleArrCase2 = {START_POS_ANGLE, -90, -90, 0}; + private static int[] methodIdArrCase2 = {START_METHOD, 12, 22, 10, 0}; + + public static double[] xCoordArrCase3 = {START_POS_X, -12. -12, 36}; + private static double[] yCoordArrCase3 = {START_POS_Y, 64, 36, 68}; + private static double[] angleArrCase3 = {START_POS_ANGLE, -90, -90, 0}; + private static int[] methodIdArrCase3 = {START_METHOD, 11, 22, 10, 0}; + + @Override + public void runOpMode(){ + int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X); + dashboardTelemetry.addData("Position Detected is", pos); + dashboardTelemetry.update(); + + if (pos == 1) { + xCoordArr = xCoordArrCase1; + yCoordArr = yCoordArrCase1; + angleArr = angleArrCase1; + methodIdArr = methodIdArrCase1; + } + else if (pos == 2){ + xCoordArr = xCoordArrCase2; + yCoordArr = yCoordArrCase2; + angleArr = angleArrCase2; + methodIdArr = methodIdArrCase2; + } + else if (pos == 3) { + xCoordArr = xCoordArrCase3; + yCoordArr = yCoordArrCase3; + angleArr = angleArrCase3; + methodIdArr = methodIdArrCase3; + } + + + initThings(); + + setXCoords(xCoordArr); + setYCoords(yCoordArr); + setAngles(angleArr); + setMethodIDS(methodIdArr); + + try { + makeActionObjects(); + } catch (Exception e) { + e.printStackTrace(); + } + + waitForStart(); + + run(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/enhancedAutoMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/EnhancedAutoMode.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/enhancedAutoMode.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/EnhancedAutoMode.java index d4e5539..14bceea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/enhancedAutoMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/EnhancedAutoMode.java @@ -15,8 +15,7 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; -@Config -public abstract class enhancedAutoMode extends LinearOpMode { +public abstract class EnhancedAutoMode extends LinearOpMode { private static List xCoords = new ArrayList<>(0); private static List yCoords = new ArrayList<>(0); private static List angles = new ArrayList<>(0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedNearParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedNearParkAuto.java new file mode 100644 index 0000000..61622dc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedNearParkAuto.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + +import static org.firstinspires.ftc.teamcode.createdcode.configs.AutoConfig.*; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + + +@Autonomous (group = "Red") +@Config +public class RedNearParkAuto extends EnhancedAutoMode { + public static double START_POS_X = -40; + public static double START_POS_Y = -66; + public static double START_POS_ANGLE = 90; + public static int START_METHOD = 20; + + public static int BLOCK_DROP_POINT = -30; + + //TODO: GET THESE VALUES + public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0; + public static int SHIPPING_ELEMENT_POS_1_X = 0; + public static int SHIPPING_ELEMENT_POS_2_X = 0; + public static int SHIPPING_ELEMENT_POS_3_X = 0; + + public static double[] xCoordArr = {}; + private static double[] yCoordArr = {}; + private static double[] angleArr = {}; + private static int[] methodIdArr = {}; + + public static double[] xCoordArrCase1 = {START_POS_X, BLOCK_DROP_POINT, BLOCK_DROP_POINT, -56, RED_CAROUSEL_Y, RED_STORAGE_Y}; + private static double[] yCoordArrCase1 = {START_POS_Y, -48, -24, -24, RED_CAROUSEL_Y, RED_STORAGE_Y}; + private static double[] angleArrCase1 = {START_POS_ANGLE, 0, 0, 0, RED_CAROUSEL_ANGLE, RED_STORAGE_ANGLE}; + private static int[] methodIdArrCase1 = {START_METHOD, 11, 22, 10, 31, 0}; + + public static double[] xCoordArrCase2 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, RED_CAROUSEL_X, RED_STORAGE_X}; + private static double[] yCoordArrCase2 = {START_POS_Y, -48, -24, -24, -24, RED_CAROUSEL_Y, RED_STORAGE_Y}; + private static double[] angleArrCase2 = {START_POS_ANGLE, 0, 0, 0, 0, RED_CAROUSEL_ANGLE, RED_STORAGE_ANGLE}; + private static int[] methodIdArrCase2 = {START_METHOD, 12 , 0, 22, 10, 31, 0}; + + public static double[] xCoordArrCase3 = {START_POS_X, -48, -48, BLOCK_DROP_POINT, -48, RED_CAROUSEL_X, RED_STORAGE_X}; + private static double[] yCoordArrCase3 = {START_POS_Y, -48, -24, -24, -24, RED_CAROUSEL_Y, RED_STORAGE_Y}; + private static double[] angleArrCase3 = {START_POS_ANGLE, 0, 0, 0, 0, RED_CAROUSEL_ANGLE, RED_STORAGE_ANGLE}; + private static int[] methodIdArrCase3 = {START_METHOD, 13, 0, 22, 10, 31, 0}; + + FtcDashboard dashboard = FtcDashboard.getInstance(); + Telemetry dashboardTelemetry = dashboard.getTelemetry(); + + @Override + public void runOpMode(){ + int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X); + dashboardTelemetry.addData("Position Detected is", pos); + dashboardTelemetry.update(); + + if (pos == 1) { + xCoordArr = xCoordArrCase1; + yCoordArr = yCoordArrCase1; + angleArr = angleArrCase1; + methodIdArr = methodIdArrCase1; + } + else if (pos == 2){ + xCoordArr = xCoordArrCase2; + yCoordArr = yCoordArrCase2; + angleArr = angleArrCase2; + methodIdArr = methodIdArrCase2; + } + else if (pos == 3) { + xCoordArr = xCoordArrCase3; + yCoordArr = yCoordArrCase3; + angleArr = angleArrCase3; + methodIdArr = methodIdArrCase3; + } + + + initThings(); + + setXCoords(xCoordArr); + setYCoords(yCoordArr); + setAngles(angleArr); + setMethodIDS(methodIdArr); + + try { + makeActionObjects(); + } catch (Exception e) { + e.printStackTrace(); + } + + waitForStart(); + + run(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedWarehouseSideAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedWarehouseSideAuto.java new file mode 100644 index 0000000..e298a27 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/RedWarehouseSideAuto.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + + +@Autonomous (group = "Red") +@Config +public class RedWarehouseSideAuto extends EnhancedAutoMode { + public static double START_POS_X = 8; + public static double START_POS_Y = -66; + public static double START_POS_ANGLE = -90; + public static int START_METHOD = 20; + + //TODO: Get vals + public static int SHIPPING_ELEMENT_CENTER_HEIGHT = 0; + public static int SHIPPING_ELEMENT_POS_1_X = 0; + public static int SHIPPING_ELEMENT_POS_2_X = 0; + public static int SHIPPING_ELEMENT_POS_3_X = 0; + + + FtcDashboard dashboard = FtcDashboard.getInstance(); + Telemetry dashboardTelemetry = dashboard.getTelemetry(); + + + public static double[] xCoordArr = {}; + private static double[] yCoordArr = {}; + private static double[] angleArr = {}; + private static int[] methodIdArr = {}; + + public static double[] xCoordArrCase1 = {START_POS_X, -12. -12, 36}; + private static double[] yCoordArrCase1 = {START_POS_Y, -64, -36, -68}; + private static double[] angleArrCase1 = {START_POS_ANGLE, -90, -90, 0}; + private static int[] methodIdArrCase1 = {START_METHOD, 11, 22, 10, 0}; + + public static double[] xCoordArrCase2 = {START_POS_X, -12. -12, 36}; + private static double[] yCoordArrCase2 = {START_POS_Y, -64, -36, -68}; + private static double[] angleArrCase2 = {START_POS_ANGLE, -90, -90, 0}; + private static int[] methodIdArrCase2 = {START_METHOD, 12, 22, 10, 0}; + + public static double[] xCoordArrCase3 = {START_POS_X, -12. -12, 36}; + private static double[] yCoordArrCase3 = {START_POS_Y, -64, -36, -68}; + private static double[] angleArrCase3 = {START_POS_ANGLE, -90, -90, 0}; + private static int[] methodIdArrCase3 = {START_METHOD, 13, 22, 10, 0}; + + @Override + public void runOpMode(){ + int pos = getShippingElementPos(SHIPPING_ELEMENT_CENTER_HEIGHT, SHIPPING_ELEMENT_POS_1_X, SHIPPING_ELEMENT_POS_2_X, SHIPPING_ELEMENT_POS_3_X); + dashboardTelemetry.addData("Position Detected is", pos); + dashboardTelemetry.update(); + + if (pos == 1) { + xCoordArr = xCoordArrCase1; + yCoordArr = yCoordArrCase1; + angleArr = angleArrCase1; + methodIdArr = methodIdArrCase1; + } + else if (pos == 2){ + xCoordArr = xCoordArrCase2; + yCoordArr = yCoordArrCase2; + angleArr = angleArrCase2; + methodIdArr = methodIdArrCase2; + } + else if (pos == 3) { + xCoordArr = xCoordArrCase3; + yCoordArr = yCoordArrCase3; + angleArr = angleArrCase3; + methodIdArr = methodIdArrCase3; + } + + + initThings(); + + setXCoords(xCoordArr); + setYCoords(yCoordArr); + setAngles(angleArr); + setMethodIDS(methodIdArr); + + try { + makeActionObjects(); + } catch (Exception e) { + e.printStackTrace(); + } + + waitForStart(); + + run(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/redAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/redAuto.java deleted file mode 100644 index 7a7f7c1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/redAuto.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; - -@Autonomous -@Config -public class redAuto extends enhancedAutoMode{ - public static double[] xCoordArr = {0, -30, -50}; - private static double[] yCoordArr = {0, 10, 50}; - private static double[] angleArr = {0, 90, 90}; - private static int[] methodIdArr = {0, 0, 0}; - - @Override - public void runOpMode(){ - initThings(); - - setXCoords(xCoordArr); - setYCoords(yCoordArr); - setAngles(angleArr); - setMethodIDS(methodIdArr); - try { - makeActionObjects(); - } catch (Exception e) { - e.printStackTrace(); - } - - waitForStart(); - - run(); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/AutoConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/AutoConstants.java index 2295ab2..6a19319 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/AutoConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/oldthings/automodes/AutoConstants.java @@ -1,8 +1,10 @@ package org.firstinspires.ftc.teamcode.createdcode.oldthings.automodes; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; -@Config +@Disabled +//@Config public class AutoConstants { public static double CAROUSEL_POWER = 0.7; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java index 536ff91..26c74a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java @@ -65,27 +65,7 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - /* - * Note from LearnRoadRunner.com: - * The velocity and acceleration constraints were calculated based on the following equation: - * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85 - * Resulting in 41.065033847087705 in/s. - * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above. - * This is capped at 85% because there are a number of variables that will prevent your bot from actually - * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc. - * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically - * max velocity. The theoretically maximum velocity is 48.311804525985536 in/s. - * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally - * affected if it is aiming for a velocity not actually possible. - * - * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on - * actual testing. Just set it at a reasonable value and keep increasing until your path following starts - * to degrade. As of now, it simply mirrors the velocity, resulting in 41.065033847087705 in/s/s - * - * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s. - * You are free to raise this on your own if you would like. It is best determined through experimentation. - */ public static double MAX_VEL = 41.065033847087705; public static double MAX_ACCEL = 41.065033847087705; public static double MAX_ANG_VEL = Math.toRadians(200.24281914893615);