From e2223813dce67f6d47cc46ac525cdb7063c080aa Mon Sep 17 00:00:00 2001 From: sihan Date: Thu, 7 Mar 2024 12:14:04 -0600 Subject: [PATCH] teleop testing --- .../ftc/teamcode/hardware/Robot.java | 27 +++--- .../roadrunner/drive/DriveConstants.java | 2 +- .../roadrunner/drive/MecanumDrive.java | 15 ++- .../drive/TwoWheelTrackingLocalizer.java | 2 +- .../opmodes/AutoRedFarTwoPlusTwo.java | 40 +++++++- .../ftc/teamcode/opmodes/MainTeleOp.java | 97 ++++++++++--------- .../ftc/teamcode/util/Configurables.java | 16 +-- 7 files changed, 118 insertions(+), 81 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 9b0a1d3..c557c2c 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,6 +1,5 @@ package org.firstinspires.ftc.teamcode.hardware; -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad1; 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; @@ -69,19 +68,17 @@ public class Robot { @Getter private Slides slides; -// GamepadEx controller2 = new GamepadEx(gamepad2); -// GamepadEx controller1 = new GamepadEx(gamepad1); public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); -// this.hang = new Hang().init(hardwareMap); -// this.arm = new Arm().init(hardwareMap); -// this.wrist = new Wrist().init(hardwareMap); -// this.claw = new Claw().init(hardwareMap); + this.hang = new Hang().init(hardwareMap); + this.arm = new Arm().init(hardwareMap); + this.wrist = new Wrist().init(hardwareMap); + this.claw = new Claw().init(hardwareMap); // this.camera = new Camera(hardwareMap); -// this.plane = new Plane().init(hardwareMap); -// this.slides= new Slides().init(hardwareMap); + this.plane = new Plane().init(hardwareMap); + this.slides= new Slides().init(hardwareMap); // this.led = new Led().init(hardwareMap); return this; } @@ -130,7 +127,16 @@ public class Robot { public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);} - public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);} + public void slideStop() { + this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.slidesLeft.setTargetPosition(this.slidesLeft.getCurrentPosition()); + this.slidesLeft.setPower(1); + + this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.slidesRight.setTargetPosition(this.slidesRight.getCurrentPosition()); + this.slidesRight.setPower(1);} } @@ -351,7 +357,6 @@ public class Robot { case NEUTRAL: if (runtime > delay) { this.getArm().armRest(); - gamepad1.rumble(300); pickupMacroState = pickupMacroStates.IDLE; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index c9b3922..629a458 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -46,7 +46,7 @@ public class DriveConstants { */ public static double WHEEL_RADIUS = 1.88976; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 12.244; // in + public static double TRACK_WIDTH = 12.438; // in // public static double WHEEL_RADIUS = 1.88976; // in // public static double GEAR_RATIO = 652.5/435; // output (wheel) speed / input (motor) speed 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 8b8e933..c2055ad 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 @@ -32,12 +32,11 @@ import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.PIDFCoefficients; @@ -324,15 +323,15 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr return new ProfileAccelerationConstraint(maxAccel); } - public void setInput(GamepadEx controller1, GamepadEx controller2) { - double speedScale = controller1.isDown(GamepadKeys.Button.Y) ? SLOWMODE_SPEED : SPEED; - double turnScale = controller1.isDown(GamepadKeys.Button.Y) ? SLOWMODE_TURN : TURN; + public void setInput(Gamepad gamepad1, Gamepad gamepad2) { + double speedScale = gamepad1.y ? SLOWMODE_SPEED : SPEED; + double turnScale = gamepad1.y ? SLOWMODE_TURN : TURN; this.setWeightedDrivePower( new Pose2d( - controller1.getLeftY()* -1 * speedScale, - controller1.getLeftX()*-1 * speedScale, - -controller1.getRightX() * 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/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java index db854a5..f62a1f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -38,7 +38,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { public static double WHEEL_RADIUS = 0.944882; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double PARALLEL_X = 5.786; // X is the up and down direction + public static double PARALLEL_X = 129.5; // X is the up and down direction public static double PARALLEL_Y = -4.118; // Y is the strafe direction public static double PERPENDICULAR_X = 3.209; 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 47830e3..051616b 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 @@ -42,7 +42,9 @@ public class AutoRedFarTwoPlusTwo extends AutoBase { YELLOW, FLIPYELLOW, SCOREYELLOW, - PARK, + FIRSTCYCLE, + RETRACT, + SCORECYCLEONE, } protected void scorePreloadOne() { @@ -270,12 +272,40 @@ public class AutoRedFarTwoPlusTwo extends AutoBase { case SCOREYELLOW: if (!robot.getDrive().isBusy()){ this.robot.getClaw().open(); - state = autoState.PARK; + runtime = getRuntime(); + state = autoState.FIRSTCYCLE; } break; - case PARK: - - + case FIRSTCYCLE: + builder = this.robot.getTrajectorySequenceBuilder(); + builder.splineToConstantHeading(TO_BOARD.vec(),Math.toRadians(0)); + builder.lineToLinearHeading(READY_TRUSS); + builder.splineToConstantHeading(GET_STACK.vec(),Math.toRadians(0)); + builder.lineToLinearHeading(PICKUP_STACK, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + runtime = getRuntime(); + state = autoState.RETRACT; + break; + case RETRACT: + if (getRuntime() > runtime + .3) { + this.robot.getClaw().close(); + this.robot.getArm().armRest(); + this.robot.getWrist().wristPickup(); + this.robot.getSlides().slideDown(); + } + if (getRuntime() > runtime + 2) { + this.robot.getClaw().openStack(); + this.robot.getArm().armPickupStack(); + } + if (!robot.getDrive().isBusy()) { + this.robot.getClaw().close(); + state = autoState.SCORECYCLEONE; + } + break; + case SCORECYCLEONE: + break; } } 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 de3a9d7..54c0958 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; @@ -10,74 +11,76 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; public class MainTeleOp extends OpMode { private Robot robot; private MecanumDrive drive; + GamepadEx controller1; + GamepadEx controller2; @Override public void init() { this.robot = new Robot().init(hardwareMap); + this.controller2 = new GamepadEx(this.gamepad2); + this.controller1 = new GamepadEx(this.gamepad1); } - GamepadEx controller2 = new GamepadEx(this.gamepad2); - GamepadEx controller1 = new GamepadEx(this.gamepad1); public void loop() { -// 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; + 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; controller1.readButtons(); controller2.readButtons(); //Drive - robot.getDrive().setInput(this.controller1, this.controller2); + robot.getDrive().setInput(gamepad1, gamepad2); //slides -// if (slideUp) { -// 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)) { -// this.robot.getSlides().slideStop(); -// } + if (slideUp) { + 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)) { + this.robot.getSlides().slideStop(); + } ////Macros -// this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN + this.robot.pickupMacro(controller2, getRuntime()); //DPADDOWN // -////Arm and Wrist -// if (controller2.wasJustPressed(GamepadKeys.Button.X)) { -// this.robot.getArm().armSecondaryScore(); -// this.robot.getWrist().wristScore(); -// } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) { -// this.robot.getArm().armRest(); -// this.robot.getWrist().wristPickup(); -// } +//Arm and Wrist + if (controller2.wasJustPressed(GamepadKeys.Button.X)) { + this.robot.getArm().armSecondaryScore(); + this.robot.getWrist().wristScore(); + } else if (controller2.wasJustPressed(GamepadKeys.Button.A)) { + this.robot.getArm().armRest(); + this.robot.getWrist().wristPickup(); + } ////Claw -// if (controller2.wasJustPressed(GamepadKeys.Button.B)) { -// gamepad1.rumble(300); -// } else if (controller2.isDown(GamepadKeys.Button.B)){ -// this.robot.getClaw().open(); -// } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){ -// this.robot.getClaw().close(); -// } + if (controller2.wasJustPressed(GamepadKeys.Button.B)) { + gamepad1.rumble(300); + } else if (controller2.isDown(GamepadKeys.Button.B)){ + this.robot.getClaw().open(); + } else if (controller2.wasJustReleased(GamepadKeys.Button.B)){ + this.robot.getClaw().close(); + } ////Hang -// if (hang) { -// this.robot.getHang().hang(); -// } else if (hangRelease){ -// this.robot.getHang().hangRelease(); -// } else if (hangPlane) { -// this.robot.getHang().hangPlane(); -// } -// + if (hang) { + this.robot.getHang().hang(); + } else if (hangRelease){ + this.robot.getHang().hangRelease(); + } else if (hangPlane) { + this.robot.getHang().hangPlane(); + } + // int Position = this.robot.getHang().hangRight.getCurrentPosition(); // telemetry.addData("position",(Position)); // telemetry.update(); // -////Plane -// if (plane) { -// this.robot.getPlane().planeLaunch(); -// }else { -// this.robot.getPlane().planeLock(); -// } +//Plane + if (plane) { + this.robot.getPlane().planeLaunch(); + }else { + this.robot.getPlane().planeLock(); + } // } } 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 6fb30a1..8eaacb1 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 @@ -14,16 +14,16 @@ public class Configurables { public static Color FTC_WHITE_UPPER = new Color(180, 30, 255); //Servo Positions - public static double ARMREST = 0.8; + public static double ARMREST = 0.91; public static double ARMSCORE = 0.4; - public static double ARMACCSCORE = .37; + public static double ARMACCSCORE = .57; public static double ARMPICKUPSTACK = 0.815; public static double PICKUP = 0.835; - public static double WRISTPICKUP = 0.28; - public static double WRISTSCORE = .96; - public static double OPEN = 0.82; + public static double WRISTPICKUP = 0.3; + public static double WRISTSCORE = .98; + public static double OPEN = 0.483; public static double BIGOPEN = 0.65; - public static double CLOSE = 0.91; + public static double CLOSE = .51; public static double PLANELOCK = 0.1; public static double PLANELAUNCH = 0.9; @@ -39,9 +39,9 @@ public class Configurables { public static int SLIDELAYERONE = 60; public static int SLIDEAUTOSTACKS = 250; public static int SLIDEUP = 1150; - public static int HANGRELEASE = 2500; + public static int HANGRELEASE = 1550; public static int HANG = 0; - public static int HANGPLANE = 1800; + public static int HANGPLANE = 1150; public static int SLIDELAYERTWO = 350; public static int SLIDESTACK = 80; public static int SLIDEPICKUPSTACKSTWO = 30;