From 86370534f5623ca295628f6c371c361148e347c6 Mon Sep 17 00:00:00 2001 From: sihan Date: Sat, 9 Mar 2024 09:06:11 -0600 Subject: [PATCH] Goog --- .../ftc/teamcode/hardware/Robot.java | 14 +++--- .../roadrunner/drive/MecanumDrive.java | 6 +-- .../ftc/teamcode/opmodes/AutoTest.java | 44 +++++++++++++++++++ .../ftc/teamcode/opmodes/MainTeleOp.java | 16 ++++--- .../ftc/teamcode/util/Configurables.java | 19 ++++++++ 5 files changed, 83 insertions(+), 16 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 6eadc69..d2d6269 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -31,7 +31,7 @@ import lombok.Getter; @Config public class Robot { public static boolean clawIsOpen; - public static double WRISTDELAY = .3; + public static double WRISTDELAY = .08; double delay; public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE; public armMacroStates armMacroState = armMacroStates.IDLE; @@ -148,10 +148,10 @@ public class Robot { public static class Slides { //Values public static double SLIDE_POWER_UP = .7; - public static double SLIDE_POWER_DOWN = .5; + public static double SLIDE_POWER_DOWN = .2; public static int SLIDELAYERONE = 60; public static int SLIDEAUTOSTACKS = 250; - public static int SLIDEUP = 1150; + public static int SLIDEUP = 630; public static int SLIDELAYERTWO = 350; public static int SLIDESTACK = 80; public static int SLIDEPICKUPSTACKSTWO = 30; @@ -426,11 +426,11 @@ public class Robot { @Config public static class Claw { //Values - public static double OPEN = 0.45; + public static double OPEN = 0.65; public static double BIGOPEN = 0f; - public static double CLOSE = 0.85; - public static double CLAW_MIN = 0.46; - public static double CLAW_MAX = 0.5; + public static double CLOSE = 0.73; + public static double CLAW_MIN = 0; + public static double CLAW_MAX = 1; //Servo private Servo claw; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index b88eee3..3d43da9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -329,9 +329,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr this.setWeightedDrivePower( new Pose2d( - gamepad2.left_stick_y * -1 * speedScale, - gamepad2.left_stick_x * -1 * speedScale, - -gamepad2.right_stick_x * turnScale + gamepad1.left_stick_y * -1 * speedScale, + gamepad1.left_stick_x * -1 * speedScale, + -gamepad1.right_stick_x * turnScale )); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java new file mode 100644 index 0000000..4e9dfdf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; + +@Autonomous(name = "Test Stuff") +public class AutoTest extends LinearOpMode { + protected Pose2d initialPosition; + private Robot robot; + + //Pose2ds + //Preloads + final static Pose2d FIRSTMOVE = new Pose2d(10, 10, Math.toRadians(0)); + + protected void sequenceOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.splineToConstantHeading(new Vector2d(10, 10), Math.toRadians(0)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + @Override + public void runOpMode() throws InterruptedException { + + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.initialPosition = new Pose2d(0, 0, Math.toRadians(0)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + // Do super fancy chinese shit + while (!this.isStarted()) { + } + + sequenceOne(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index 6f1d375..124af7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -25,6 +25,8 @@ public class MainTeleOp extends OpMode { //GamePad Controls boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP); boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT); + boolean slideStop = controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP) || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT); + boolean slideSlow = gamepad2.left_trigger > .1; boolean hang = gamepad2.left_bumper; boolean hangRelease = gamepad2.right_bumper; boolean hangPlane = gamepad2.y; @@ -39,19 +41,21 @@ public class MainTeleOp extends OpMode { this.robot.getSlides().slideUp(); } else if (slideDown) { this.robot.getSlides().slideDown(); - } else if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_UP) - || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) { + } else if (slideStop) { this.robot.getSlides().slideStop(); } - if (gamepad2.left_trigger > .1) { - Robot.Slides.SLIDE_POWER_UP = .3; + //Slide Power + if (slideSlow) { + Robot.Slides.SLIDE_POWER_UP = .25; + Robot.Slides.SLIDE_POWER_DOWN = .2; } else { Robot.Slides.SLIDE_POWER_UP = .7; + Robot.Slides.SLIDE_POWER_DOWN = .5; } -////Macros +//Macros this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN this.robot.armMacro(controller2, getRuntime()); //X -// + //Arm and Wrist // if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { // this.robot.getArm().pickup(true); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index 43d639d..984de2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.util; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; @Config public class Configurables { @@ -21,4 +22,22 @@ public class Configurables { public static double TURN = .75; public static double SLOWMODE_TURN = 0.3; } + + //Auto Temp + @Config + public static class AuToDeV { + //Things + public static double X1 = 0, Y1 = 0, R1 = 0; + public static double X2 = 0, Y2 = 0, R2 = 0; + public static double X3 = 0, Y3 = 0, R3 = 0; + public static double X4 = 0, Y4 = 0, R4 = 0; + public static double X5 = 0, Y5 = 0, R5 = 0; + + //Pose2d + public static Pose2d TEMP1 = new Pose2d(X1, Y1, Math.toRadians(R1)); + public static Pose2d TEMP2 = new Pose2d(X2, Y2, Math.toRadians(R2)); + public static Pose2d TEMP3 = new Pose2d(X3, Y3, Math.toRadians(R3)); + public static Pose2d TEMP4 = new Pose2d(X4, Y4, Math.toRadians(R4)); + public static Pose2d TEMP5 = new Pose2d(X5, Y5, Math.toRadians(R5)); + } } \ No newline at end of file