From f6cdb245d69704569ceabaf7470e6384e5c6dc2f Mon Sep 17 00:00:00 2001 From: sihan Date: Fri, 8 Mar 2024 06:49:00 -0600 Subject: [PATCH] Goog --- .../ftc/teamcode/hardware/Robot.java | 370 ++++++++++++------ .../roadrunner/drive/MecanumDrive.java | 14 +- .../ftc/teamcode/opmodes/AutoBase.java | 6 +- .../ftc/teamcode/opmodes/AutoBlue.java | 2 - .../ftc/teamcode/opmodes/AutoBlueFarStem.java | 2 - .../teamcode/opmodes/AutoBlueTwoPlusTwo.java | 2 - .../ftc/teamcode/opmodes/AutoRed.java | 2 - .../ftc/teamcode/opmodes/AutoRedFar.java | 2 - .../opmodes/AutoRedFarTwoPlusTwo.java | 2 - .../opmodes/AutoRedFaronepluszero.java | 2 - .../teamcode/opmodes/AutoRedTwoPlusTwo.java | 17 +- .../ftc/teamcode/opmodes/MainTeleOp.java | 55 +-- .../ftc/teamcode/util/Configurables.java | 39 +- 13 files changed, 303 insertions(+), 212 deletions(-) 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 7bd108a..6eadc69 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 @@ -1,27 +1,5 @@ package org.firstinspires.ftc.teamcode.hardware; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMPICKUPSTACK; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE; -import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN; -import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE; -import static org.firstinspires.ftc.teamcode.util.Configurables.HANG; -import static org.firstinspires.ftc.teamcode.util.Configurables.HANGPLANE; -import static org.firstinspires.ftc.teamcode.util.Configurables.HANGRELEASE; -import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN; -import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP; -import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH; -import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEAUTOSTACKS; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERONE; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERTWO; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEPICKUPSTACKSTWO; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP; -import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP; -import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE; import static org.firstinspires.ftc.teamcode.util.Constants.CLAW; import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT; import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT; @@ -35,6 +13,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.WRIST; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PController; +import com.arcrobotics.ftclib.controller.PIDFController; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; @@ -48,15 +27,21 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra import org.firstinspires.ftc.teamcode.vision.Camera; import lombok.Getter; + @Config public class Robot { + public static boolean clawIsOpen; + public static double WRISTDELAY = .3; + double delay; + public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE; + public armMacroStates armMacroState = armMacroStates.IDLE; + @Getter + public Arm arm; @Getter private MecanumDrive drive; @Getter private Led led; @Getter - private Arm arm; - @Getter private Wrist wrist; @Getter private Claw claw; @@ -69,8 +54,6 @@ public class Robot { @Getter private Slides slides; - - public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); this.hang = new Hang().init(hardwareMap); @@ -79,12 +62,100 @@ public class Robot { this.claw = new Claw().init(hardwareMap); // this.camera = new Camera(hardwareMap); this.plane = new Plane().init(hardwareMap); - this.slides= new Slides().init(hardwareMap); + this.slides = new Slides().init(hardwareMap); // this.led = new Led().init(hardwareMap); return this; } + public void pickupMacro(GamepadEx gamepadEx, double runtime) { + switch (pickupMacroState) { + case IDLE: + if (gamepadEx.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) { + pickupMacroState = pickupMacroStates.DROP; + } + break; + case OPEN: + delay = runtime + .3; + this.getClaw().open(); + pickupMacroState = pickupMacroStates.DROP; + break; + case DROP: + if (runtime > delay) { + this.getArm().pickup(true); + delay = runtime + .2; + pickupMacroState = pickupMacroStates.CLOSE; + } + break; + case CLOSE: + if (runtime > delay) { + this.getClaw().close(); + delay = runtime + .3; + pickupMacroState = pickupMacroStates.NEUTRAL; + } + break; + case NEUTRAL: + if (runtime > delay) { + this.getArm().armRest(true); + pickupMacroState = pickupMacroStates.IDLE; + } + break; + + } + } + + public void armMacro(GamepadEx gamepadEx, double runtime) { + switch (armMacroState) { + case IDLE: + if (gamepadEx.wasJustPressed(GamepadKeys.Button.X)) { + armMacroState = armMacroStates.ARMSWING; + } + break; + case ARMSWING: + arm.armSecondaryScore(); + delay = runtime + WRISTDELAY; + armMacroState = armMacroStates.WRIST; + break; + case WRIST: + if (runtime > delay) { + wrist.wristScore(); + armMacroState = armMacroStates.IDLE; + } + break; + } + } + + public TrajectorySequenceBuilder getTrajectorySequenceBuilder() { + this.drive.update(); + return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate()); + } + + public void update() { + this.arm.update(); + this.wrist.update(); + this.drive.update(); + } + + public enum pickupMacroStates { + IDLE, OPEN, DROP, CLOSE, NEUTRAL + + } + + public enum armMacroStates { + IDLE, ARMSWING, WRIST + } + + @Config public static class Slides { + //Values + public static double SLIDE_POWER_UP = .7; + public static double SLIDE_POWER_DOWN = .5; + public static int SLIDELAYERONE = 60; + public static int SLIDEAUTOSTACKS = 250; + public static int SLIDEUP = 1150; + public static int SLIDELAYERTWO = 350; + public static int SLIDESTACK = 80; + public static int SLIDEPICKUPSTACKSTWO = 30; + //Motors public DcMotor slidesRight = null; public DcMotor slidesLeft = null; @@ -116,17 +187,29 @@ public class Robot { this.slidesRight.setPower(power); } - public void slideUp(){this.slideTo(SLIDEUP, SLIDE_POWER_UP);} + public void slideUp() { + this.slideTo(SLIDEUP, SLIDE_POWER_UP); + } - public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);} + public void slideDown() { + this.slideTo(0, SLIDE_POWER_DOWN); + } - public void slideFirstLayer(){this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);} + public void slideFirstLayer() { + this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP); + } - public void slideScoreStack(){this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);} + public void slideScoreStack() { + this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP); + } - public void slideAutoStacks(){this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);} + public void slideAutoStacks() { + this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP); + } - public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);} + public void slidePickupStackTwo() { + this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP); + } public void slideStop() { this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -137,11 +220,18 @@ public class Robot { this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); this.slidesRight.setTargetPosition(this.slidesRight.getCurrentPosition()); - this.slidesRight.setPower(1);} + this.slidesRight.setPower(1); + } } + @Config public static class Hang { + //Values + public static int HANGRELEASE = 1550; + public static int HANG = 0; + public static int HANGPLANE = 1150; + //Motors public DcMotor hangLeft = null; public DcMotor hangRight = null; @@ -161,43 +251,48 @@ public class Robot { return this; } - public void hangTo(int hangPos, double daPower) { + public void hangTo(int hangPos, double Power) { this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); this.hangLeft.setTargetPosition(hangPos); - this.hangLeft.setPower(daPower); + this.hangLeft.setPower(Power); this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); this.hangRight.setTargetPosition(hangPos); - this.hangRight.setPower(daPower); + this.hangRight.setPower(Power); } - public void hangRelease(){ - this.hangTo(HANGRELEASE,1); + public void hangRelease() { + this.hangTo(HANGRELEASE, 1); } - public void hang(){ - this.hangTo(HANG,1); - } - public void hangPlane(){ - this.hangTo(HANGPLANE,1); + public void hang() { + this.hangTo(HANG, 1); } - public void hangIdle() { - this.hangLeft.setPower(0); - this.hangRight.setPower(0); + public void hangPlane() { + this.hangTo(HANGPLANE, 1); } - - - } + @Config public static class Arm { + //Values + public static double DIFFY_KP = 0.1; + public static double DIFFY_TOL = 0.01; + public static double ARM_MAX_DELTA = 0.02; + public static double ARMREST = 0.89; + public static double ARMSCORE = 0.4; + public static double ARMACCSCORE = .57; + public static double ARMPICKUPSTACK = 0.815; + public static double PICKUP = 0.92; + //PControler + public PController armPController; + private double armTarget; + //Servos private Servo leftArm; private Servo rightArm; - private PController armController; - public Arm init(HardwareMap hardwareMap) { this.leftArm = hardwareMap.get(Servo.class, LEFTARM); @@ -205,62 +300,143 @@ public class Robot { this.rightArm.setDirection(Servo.Direction.REVERSE); this.rightArm.setPosition(ARMREST); this.leftArm.setPosition(ARMREST); + this.armPController = new PController(DIFFY_KP); + this.armRest(true); return this; } public void pickup() { - this.leftArm.setPosition(PICKUP); - this.rightArm.setPosition(PICKUP); + pickup(false); + } + + public void pickup(boolean now) { + moveArm(PICKUP, now); } public void armScore() { - this.leftArm.setPosition(ARMSCORE); - this.rightArm.setPosition(ARMSCORE); + this.armSecondaryScore(); } public void armSecondaryScore() { - this.leftArm.setPosition(ARMACCSCORE); - this.rightArm.setPosition(ARMACCSCORE); + moveArm(ARMACCSCORE, false); } - public void armPickupStack() { - this.leftArm.setPosition(ARMPICKUPSTACK); - this.rightArm.setPosition(ARMPICKUPSTACK); + moveArm(ARMPICKUPSTACK, false); } public void armRest() { - this.leftArm.setPosition(ARMREST); - this.rightArm.setPosition(ARMREST); + armRest(false); } + + public void armRest(boolean now) { + moveArm(ARMREST, now); + } + + private void moveArm(double position, boolean now) { + this.armTarget = position; + this.armPController.setSetPoint(this.armTarget); + if (now) { + this.leftArm.setPosition(position); + this.rightArm.setPosition(position); + } + } + + public boolean isAtTarget() { + return this.armPController.atSetPoint(); + } + + public void update() { + this.armPController.setSetPoint(this.armTarget); + this.armPController.setTolerance(DIFFY_TOL); + this.armPController.setP(DIFFY_KP); + + if (!isAtTarget()) { + double armDelta = this.armPController.calculate(leftArm.getPosition()); + if (Math.abs(this.armPController.getPositionError()) > 0.1) { + armDelta = Math.copySign(ARM_MAX_DELTA, armDelta); + } + this.leftArm.setPosition(leftArm.getPosition() + armDelta); + this.rightArm.setPosition(rightArm.getPosition() + armDelta); + } + +// +// this.rightPController.setSetPoint(this.rightTarget); +// this.rightPController.setTolerance(DIFFY_TOL); +// this.rightPController.setP(DIFFY_KP); +// double rightDelta = this.rightPController.calculate(rightArm.getPosition()); +// rightDelta = Math.max(-1 * DIFFY_MAX_DELTA, Math.min(DIFFY_MAX_DELTA, rightDelta)); +// this.rightArm.setPosition(rightArm.getPosition() + rightDelta); + + // A is where I am + // B is where I want to get to + // E is the difference between them + // X is the midpoint between A and B + // while I am at on the left side of X, delta should be constant + // once I am on the right side of X, delta should be whatever ther P controller says + + + } + } + @Config public static class Wrist { + //Pcontroller + public static double KP = 0.2; + public static double TOL = 0.005; + public static double MAX_DELTA = 0.04; + private PIDFController wristPController; + //Values + public static double WRISTPICKUP = 0.3; + public static double WRISTSCORE = .98; + //Servo private Servo wrist; public Wrist init(HardwareMap hardwareMap) { this.wrist = hardwareMap.get(Servo.class, WRIST); this.wrist.setPosition(WRISTPICKUP); + this.wristPController = new PController(KP); + this.wristPController.setSetPoint(WRISTPICKUP); return this; } public void wristPickup() { - this.wrist.setPosition(WRISTPICKUP); + this.wristPController.setSetPoint(WRISTPICKUP); } public void wristScore() { - this.wrist.setPosition(WRISTSCORE); + this.wristPController.setSetPoint(WRISTSCORE); } + public void update() { + this.wristPController.setTolerance(TOL); + this.wristPController.setP(KP); + + if (!wristPController.atSetPoint()) { + double wristDelta = this.wristPController.calculate(wrist.getPosition()); + if (Math.abs(this.wristPController.getPositionError()) > 0.1) { + wristDelta = Math.copySign(MAX_DELTA, wristDelta); + } + this.wrist.setPosition(wrist.getPosition() + wristDelta); + } + } } - public static boolean clawIsOpen; - + @Config public static class Claw { + //Values + public static double OPEN = 0.45; + 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; + //Servo private Servo claw; public Claw init(HardwareMap hardwareMap) { this.claw = hardwareMap.get(Servo.class, CLAW); + this.claw.scaleRange(CLAW_MIN, CLAW_MAX); close(); return this; } @@ -286,6 +462,7 @@ public class Robot { } + @Config public static class Led { private RevBlinkinLedDriver led; @@ -311,7 +488,12 @@ public class Robot { } } + @Config public static class Plane { + //Values + public static double PLANELOCK = 0.1; + public static double PLANELAUNCH = 0.9; + //Servo private Servo plane; public Plane init(HardwareMap hardwareMap) { @@ -329,59 +511,5 @@ public class Robot { } } - - public void pickupMacro(GamepadEx gamepadEx, double runtime) { - switch (pickupMacroState) { - case IDLE: - if (gamepadEx.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)){ - pickupMacroState = pickupMacroStates.OPEN; - } - break; - case OPEN: - delay = runtime + .3; - this.getClaw().open(); - pickupMacroState = pickupMacroStates.DROP; - break; - case DROP: - if (runtime > delay) { - this.getArm().pickup(); - delay= runtime + .2; - pickupMacroState = pickupMacroStates.CLOSE; - } - break; - case CLOSE: - if (runtime > delay) { - this.getClaw().close(); - delay= runtime + .3; - pickupMacroState = pickupMacroStates.NEUTRAL; - } - break; - case NEUTRAL: - if (runtime > delay) { - this.getArm().armRest(); - pickupMacroState = pickupMacroStates.IDLE; - } - break; - - } - } - - public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE; - - public enum pickupMacroStates{ - IDLE, - OPEN, - DROP, - CLOSE, - NEUTRAL - - } - - double delay; - - public TrajectorySequenceBuilder getTrajectorySequenceBuilder() { - this.drive.update(); - return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate()); - } } 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 c2055ad..b88eee3 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 @@ -11,10 +11,10 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_SPEED; -import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_TURN; -import static org.firstinspires.ftc.teamcode.util.Configurables.SPEED; -import static org.firstinspires.ftc.teamcode.util.Configurables.TURN; +import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_SPEED; +import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SLOWMODE_TURN; +import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.SPEED; +import static org.firstinspires.ftc.teamcode.util.Configurables.driveSpeed.TURN; import androidx.annotation.NonNull; @@ -329,9 +329,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr this.setWeightedDrivePower( new Pose2d( - gamepad1.left_stick_y* -1 * speedScale, - gamepad1.left_stick_x*-1 * speedScale, - -gamepad1.right_stick_x * turnScale + gamepad2.left_stick_y * -1 * speedScale, + gamepad2.left_stick_x * -1 * speedScale, + -gamepad2.right_stick_x * turnScale )); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java index e912c3a..9487a81 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -9,7 +8,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState; -@Config public abstract class AutoBase extends LinearOpMode { protected Pose2d initialPosition; Robot robot; @@ -37,11 +35,11 @@ public abstract class AutoBase extends LinearOpMode { this.telemetry.addData("Park Position", parkLocation); this.telemetry.addData("Delay", delay); this.telemetry.update(); - } + while (state != autoState.STOP) { followTrajectories(); - robot.getDrive().update(); + robot.update(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java index 702b526..827072d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -10,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "autoBlue") public class AutoBlue extends LinearOpMode { protected Pose2d initialPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java index c282bb5..2e7e1a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -10,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "autoBlueFar") public class AutoBlueFarStem extends LinearOpMode { protected Pose2d initialPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java index fea0bde..47815ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "autoBlue2+2") public class AutoBlueTwoPlusTwo extends LinearOpMode { protected Pose2d initialPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index faad40f..e35e384 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "autoRed") public class AutoRed extends LinearOpMode { protected Pose2d initialPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java index e4a9276..1ef56fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "AutoRedFar") public class AutoRedFar extends LinearOpMode { protected Pose2d initialPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java index 051616b..c0826ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.opmodes; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -8,7 +7,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp") public class AutoRedFarTwoPlusTwo extends AutoBase { //Pose2ds diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java index 817a494..b2bbc94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -12,7 +11,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config @Autonomous(name = "AutoRedFar1+0") public class AutoRedFaronepluszero extends LinearOpMode { protected Pose2d initialPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java index 8983e34..53bb7d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -11,8 +10,9 @@ import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config + @Autonomous(name = "autoRed2+2") + public class AutoRedTwoPlusTwo extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; @@ -37,7 +37,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360)); final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360)); //Park - final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360)); + final static Pose2d BACK_OFF = new Pose2d(60, -58, Math.toRadians(360)); final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360)); protected void scorePreloadOne() { @@ -64,7 +64,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { case "LEFT": builder.lineToLinearHeading(LEFT_BOARD, MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20));; + MecanumDrive.getAccelerationConstraint(20)); + ; break; case "CENTER": builder.lineToLinearHeading(CENTER_BOARD, @@ -89,7 +90,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.addTemporalMarker(.3, robot.getArm()::armRest); builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.addTemporalMarker(1.5, robot.getClaw()::openStack); builder.addTemporalMarker(1.5, robot.getArm()::pickup); builder.lineToLinearHeading(TO_STACK); builder.lineToLinearHeading(TO_STACK_SLOW, @@ -104,7 +105,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.addTemporalMarker(.3, robot.getArm()::armRest); builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.addTemporalMarker(1.5, robot.getClaw()::openStack); builder.addTemporalMarker(1.5, robot.getArm()::pickup); builder.lineToLinearHeading(TO_STACK); builder.lineToLinearHeading(TO_STACK_SLOW2, @@ -113,7 +114,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { this.robot.getDrive().followTrajectorySequence(builder.build()); } - protected void scoreStack() { + protected void scoreStack() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(BACK_THROUGH_GATE); builder.lineToLinearHeading(APPROACHING_BOARD); @@ -129,7 +130,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(new Pose2d(75, -31, Math.toRadians(360)), MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(20)); - builder.addTemporalMarker(.2,this::clawSlowOpen); + builder.addTemporalMarker(.2, this::clawSlowOpen); this.robot.getDrive().followTrajectorySequence(builder.build()); } 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 97fb534..6f1d375 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 @@ -6,14 +6,13 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; -import org.firstinspires.ftc.teamcode.util.Configurables; @com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development") public class MainTeleOp extends OpMode { - private Robot robot; - private MecanumDrive drive; GamepadEx controller1; GamepadEx controller2; + private Robot robot; + private MecanumDrive drive; @Override public void init() { @@ -22,15 +21,15 @@ public class MainTeleOp extends OpMode { this.controller1 = new GamepadEx(this.gamepad1); } - - public void loop() { +//GamePad Controls boolean slideUp = controller2.isDown(GamepadKeys.Button.DPAD_UP); boolean slideDown = controller2.isDown(GamepadKeys.Button.DPAD_LEFT); boolean hang = gamepad2.left_bumper; boolean hangRelease = gamepad2.right_bumper; boolean hangPlane = gamepad2.y; boolean plane = gamepad2.dpad_right; +//Read Controller controller1.readButtons(); controller2.readButtons(); //Drive @@ -44,51 +43,55 @@ public class MainTeleOp extends OpMode { || controller2.wasJustReleased(GamepadKeys.Button.DPAD_LEFT)) { this.robot.getSlides().slideStop(); } - if (gamepad2.left_trigger > .5){ - Configurables.SLIDE_POWER_UP = .3; + if (gamepad2.left_trigger > .1) { + Robot.Slides.SLIDE_POWER_UP = .3; } else { - Configurables.SLIDE_POWER_UP = .7; + Robot.Slides.SLIDE_POWER_UP = .7; } ////Macros this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN + this.robot.armMacro(controller2, getRuntime()); //X // //Arm and Wrist - if (controller2.wasJustPressed(GamepadKeys.Button.X)) { - this.robot.getArm().armSecondaryScore(); - this.robot.getWrist().wristScore(); - } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) { +// if (controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { +// this.robot.getArm().pickup(true); +// } else +// if (controller2.wasJustReleased(GamepadKeys.Button.DPAD_DOWN)) { +// this.robot.getArm().armRest(true); +// } else + if (controller2.wasJustPressed(GamepadKeys.Button.A)) { this.robot.getArm().armRest(); this.robot.getWrist().wristPickup(); } -////Claw +//Claw if (controller2.wasJustPressed(GamepadKeys.Button.B)) { gamepad1.rumble(300); - } else if (controller2.isDown(GamepadKeys.Button.B)){ + } else if (controller2.isDown(GamepadKeys.Button.B) || controller2.isDown(GamepadKeys.Button.DPAD_DOWN)) { this.robot.getClaw().open(); - } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){ + } else if (controller2.wasJustReleased(GamepadKeys.Button.B)) { this.robot.getClaw().close(); } -////Hang +//Hang if (hang) { this.robot.getHang().hang(); - } else if (hangRelease){ + } else if (hangRelease) { this.robot.getHang().hangRelease(); } else if (hangPlane) { this.robot.getHang().hangPlane(); } - - int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition(); - telemetry.addData("positionLeft",(PositionLeft)); - int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition(); - telemetry.addData("positionRight",(PositionRight)); - telemetry.update(); -// //Plane if (plane) { this.robot.getPlane().planeLaunch(); - }else { + } else { this.robot.getPlane().planeLock(); } -// +//Telemetry + int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition(); + telemetry.addData("positionLeft", (PositionLeft)); + int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition(); + telemetry.addData("positionRight", (PositionRight)); + telemetry.update(); +//Update Robot + this.robot.update(); } } 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 c5f5dc7..43d639d 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 @@ -13,37 +13,12 @@ public class Configurables { public static Color FTC_WHITE_LOWER = new Color(0, 0, 40); public static Color FTC_WHITE_UPPER = new Color(180, 30, 255); - //Servo Positions - public static double ARMREST = 0.91; - public static double ARMSCORE = 0.4; - public static double ARMACCSCORE = .57; - public static double ARMPICKUPSTACK = 0.815; - public static double PICKUP = 0.94; - public static double WRISTPICKUP = 0.3; - public static double WRISTSCORE = .98; - public static double OPEN = 0.481; - public static double BIGOPEN = 0.65; - public static double CLOSE = .51; - public static double PLANELOCK = 0.1; - public static double PLANELAUNCH = 0.9; - //Drive Speed - public static double SPEED = 1; - public static double SLOWMODE_SPEED = 0.3; - public static double TURN = .75; - public static double SLOWMODE_TURN = 0.3; - - //Motor Positions - public static double SLIDE_POWER_UP = .7; - public static double SLIDE_POWER_DOWN = .5; - public static int SLIDELAYERONE = 60; - public static int SLIDEAUTOSTACKS = 250; - public static int SLIDEUP = 1150; - public static int HANGRELEASE = 1550; - public static int HANG = 0; - public static int HANGPLANE = 1150; - public static int SLIDELAYERTWO = 350; - public static int SLIDESTACK = 80; - public static int SLIDEPICKUPSTACKSTWO = 30; - + @Config + public static class driveSpeed { + public static double SPEED = 1; + public static double SLOWMODE_SPEED = 0.3; + public static double TURN = .75; + public static double SLOWMODE_TURN = 0.3; + } } \ No newline at end of file