From 24540bf5ca1b20d72904bdb5456b559001446276 Mon Sep 17 00:00:00 2001 From: sihan Date: Tue, 9 Jan 2024 19:17:19 -0600 Subject: [PATCH] before LM3 all subsystems working, auto nothing --- .../ftc/teamcode/hardware/Robot.java | 130 +++++++++++++----- .../ftc/teamcode/opmodes/AutoBlue.java | 6 +- .../ftc/teamcode/opmodes/AutoBlueFar.java | 2 +- .../ftc/teamcode/opmodes/AutoRed.java | 6 +- .../ftc/teamcode/opmodes/AutoRedFar.java | 8 +- .../ftc/teamcode/opmodes/MainTeleOp.java | 46 +++++-- .../ftc/teamcode/util/Configurables.java | 26 ++-- .../ftc/teamcode/util/Constants.java | 4 + 8 files changed, 157 insertions(+), 71 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 b86e86e..1bfba94 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,26 +1,31 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE; +import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCOREAUTO; 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.LOCK; -import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED; +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.UNLOCK; -import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED; +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; import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM; -import static org.firstinspires.ftc.teamcode.util.Constants.LEFTHANG; import static org.firstinspires.ftc.teamcode.util.Constants.PLANE; import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM; -import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTHANG; +import static org.firstinspires.ftc.teamcode.util.Constants.SLIDELEFT; +import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT; import static org.firstinspires.ftc.teamcode.util.Constants.WRIST; import com.qualcomm.robotcore.hardware.DcMotor; @@ -36,8 +41,8 @@ import lombok.Getter; public class Robot { @Getter private MecanumDrive drive; - @Getter - private Intake intake; +// @Getter +// private Intake intake; @Getter private Arm arm; @Getter @@ -50,64 +55,111 @@ public class Robot { private Camera camera; @Getter private Plane plane; + @Getter + private Slides slides; public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); this.hang = new Hang().init(hardwareMap); - this.intake = new Intake().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); return this; } - public static class Intake { - private DcMotor intake = null; + public static class Slides { + private DcMotor slidesRight = null; + private DcMotor slidesLeft = null; - public Intake init(HardwareMap hardwareMap) { - this.intake = hardwareMap.dcMotor.get("intake"); - this.intake.setDirection(DcMotorSimple.Direction.REVERSE); - this.intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + public Slides init(HardwareMap hardwareMap) { + this.slidesLeft = hardwareMap.get(DcMotor.class, SLIDELEFT); + this.slidesRight = hardwareMap.get(DcMotor.class, SLIDERIGHT); + this.slidesLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.slidesRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.slidesRight.setTargetPosition(0); + this.slidesLeft.setTargetPosition(0); + + this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.slidesLeft.setDirection(DcMotorSimple.Direction.REVERSE); + this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); return this; } - public void spinIn() { - this.intake.setPower(0.7); + public void slideTo(int position, double power) { + this.slidesLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.slidesLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.slidesLeft.setTargetPosition(position); + this.slidesLeft.setPower(power); + + this.slidesRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.slidesRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.slidesRight.setTargetPosition(position); + this.slidesRight.setPower(power); } - public void spinOut() { - this.intake.setPower(-0.7); - } + public void slideUp(){this.slideTo(SLIDEUP, SLIDE_POWER_UP);} - public void stop() { - this.intake.setPower(0); - } + public void slideDown(){this.slideTo(0, SLIDE_POWER_DOWN);} + + public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);} } public static class Hang { - private Servo hangLeft; - private Servo hangRight; + public DcMotor hangLeft = null; + public DcMotor hangRight = null; public Hang init(HardwareMap hardwareMap) { - this.hangLeft = hardwareMap.get(Servo.class, LEFTHANG); - this.hangRight = hardwareMap.get(Servo.class, RIGHTHANG); - this.hangLeft.setPosition(LOCKSPEED); - this.hangRight.setPosition(LOCK); + this.hangLeft = hardwareMap.get(DcMotor.class, HANGLEFT); + this.hangRight = hardwareMap.get(DcMotor.class, HANGRIGHT); + this.hangLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.hangRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.hangRight.setTargetPosition(0); + this.hangLeft.setTargetPosition(0); + + this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.hangLeft.setDirection(DcMotorSimple.Direction.REVERSE); + this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); return this; } - public void lock() { - this.hangLeft.setPosition(LOCKSPEED); - this.hangRight.setPosition(LOCK); + public void hangTo(int hangPos, double daPower) { + this.hangLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.hangLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.hangLeft.setTargetPosition(hangPos); + this.hangLeft.setPower(daPower); + + this.hangRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.hangRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.hangRight.setTargetPosition(hangPos); + this.hangRight.setPower(daPower); } - public void release() { - this.hangLeft.setPosition(UNLOCKSPEED); - this.hangRight.setPosition(UNLOCK); + public void hangRelease(){ + this.hangTo(HANGRELEASE,1); } + + public void hang(){ + this.hangTo(HANG,1); + } + public void hangPlane(){ + this.hangTo(HANGPLANE,1); + } + + public void hangIdle() { + this.hangLeft.setPower(0); + this.hangRight.setPower(0); + } + + + } public static class Arm { @@ -139,6 +191,12 @@ public class Robot { this.rightArm.setPosition(ARMACCSCORE); } + + public void armAccurateScoreAuto() { + this.leftArm.setPosition(ARMACCSCOREAUTO); + this.rightArm.setPosition(ARMACCSCOREAUTO); + } + public void armRest() { this.leftArm.setPosition(ARMREST); this.rightArm.setPosition(ARMREST); 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 d26141c..2aeb87b 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 @@ -57,7 +57,7 @@ public class AutoBlue extends LinearOpMode { .build(); this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) - .lineToLinearHeading(new Pose2d(-72, -26, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(-72, -26.3, Math.toRadians(180))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); @@ -99,12 +99,12 @@ public class AutoBlue extends LinearOpMode { //Park this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) - .lineToLinearHeading(new Pose2d(-65, -10, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(-65, -55, Math.toRadians(180))) .addTemporalMarker(.3, robot.getArm()::armRest) .addTemporalMarker(.3, robot.getWrist()::wristPickup) .build(); this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) - .lineToLinearHeading(new Pose2d(-80, -10, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(-80, -60, Math.toRadians(180))) .build(); // Do super fancy chinese shit diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java index 113e487..30f2298 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java @@ -71,7 +71,7 @@ public class AutoBlueFar extends LinearOpMode { .build(); this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end()) - .lineToLinearHeading(new Pose2d(-49.25, -28, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(-50, -28, Math.toRadians(180))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); 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 257248c..4077ee4 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 @@ -76,12 +76,12 @@ public class AutoRed extends LinearOpMode { //Randomization Two this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(35, -28, Math.toRadians(270))) + .lineToLinearHeading(new Pose2d(31, -28, Math.toRadians(270))) .build(); this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) - .lineToLinearHeading(new Pose2d(72, -33.3, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .lineToLinearHeading(new Pose2d(70.75, -36.3, Math.toRadians(360))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScoreAuto) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); 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 5df1f2b..4301e14 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 @@ -127,24 +127,24 @@ public class AutoRedFar extends LinearOpMode { .build(); this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end()) - .lineToLinearHeading(new Pose2d(50, -37, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(50, -39, Math.toRadians(360))) .addTemporalMarker(.02, robot.getArm()::armAccurateScore) .addTemporalMarker(.02, robot.getWrist()::wristScore) .build(); this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end()) - .lineToLinearHeading(new Pose2d(40, -47, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(40, -42, Math.toRadians(360))) .build(); //Park this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end()) - .lineToLinearHeading(new Pose2d(40, -10, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(40, -12, Math.toRadians(360))) .addTemporalMarker(.3, robot.getArm()::armRest) .addTemporalMarker(.3, robot.getWrist()::wristPickup) .build(); this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) - .lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(10))) .build(); // Do super fancy chinese shit 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 54cf8b3..aa96aa7 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 @@ -17,20 +17,27 @@ public class MainTeleOp extends OpMode { @Override public void loop() { - boolean hang = gamepad2.dpad_up; - boolean restArm = gamepad2.dpad_right || gamepad2.x; + boolean slideUp = gamepad2.dpad_up; + boolean restArm = gamepad2.x; boolean pickupArm = gamepad2.dpad_down; - boolean scoreArm = gamepad2.dpad_left || gamepad2.a; - boolean accurateScoreArm = gamepad2.right_bumper; + boolean scoreArm = gamepad2.a; + boolean plane = gamepad2.right_bumper; boolean claw = gamepad2.b; boolean pickupWrist = gamepad2.x; - boolean scoreWrist = gamepad2.a || gamepad2.left_bumper; - boolean launch = gamepad2.y; + boolean scoreWrist = gamepad2.a; + boolean slideDown = gamepad2.dpad_left; + boolean hang = gamepad2.left_bumper; + boolean hangRelease = gamepad2.y; + boolean hangPlane = gamepad2.dpad_right; //Drive robot.getDrive().setInput(gamepad1, gamepad2); -//Hang - if (hang) { - this.robot.getHang().release(); +//slides + if (slideUp) { + this.robot.getSlides().slideUp(); + } else if (slideDown) { + this.robot.getSlides().slideDown(); + } else { + this.robot.getSlides().slideStop(); } //Arm if (pickupArm) { @@ -39,8 +46,6 @@ public class MainTeleOp extends OpMode { this.robot.getArm().armRest(); } else if (scoreArm) { this.robot.getArm().armScore(); - } else if (accurateScoreArm) { - this.robot.getArm().armAccurateScore(); } //Claw if (claw) { @@ -54,10 +59,23 @@ public class MainTeleOp extends OpMode { } else if (scoreWrist) { this.robot.getWrist().wristScore(); } -//Plane - if (launch) { - this.robot.getPlane().planeLaunch(); +//Hang + if (hang) { + this.robot.getHang().hang(); + } else if (hangRelease){ + this.robot.getHang().hangRelease(); + } else if (hangPlane) { + this.robot.getHang().hangPlane(); } else { + this.robot.getHang().hangIdle(); + } + 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(); } } 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 8bde221..048f5e6 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,19 +14,16 @@ public class Configurables { public static Color FTC_WHITE_UPPER = new Color(180, 30, 255); //Servo Positions - public static double UNLOCKSPEED = .1; - public static double LOCKSPEED = .25; - public static double UNLOCK = .6; - public static double LOCK = 0.4; - public static double ARMREST = .91; - public static double ARMSCORE = 0.14 ; - public static double ARMACCSCORE = 0.04; - public static double PICKUP = 0.97; + public static double ARMREST = 0.8; + public static double ARMSCORE = 0.38; + public static double ARMACCSCORE = 0.6; + public static double ARMACCSCOREAUTO = 0.6; + public static double PICKUP = 0.84; public static double WRISTPICKUP = 0.28; public static double WRISTSCORE = .96; - public static double OPEN = 0.53; + public static double OPEN = 0.85; public static double BIGOPEN = 0.45; - public static double CLOSE = 0.6; + public static double CLOSE = 0.95; public static double PLANELOCK = 0.1; public static double PLANELAUNCH = 0.9; @@ -36,4 +33,13 @@ public class Configurables { public static double TURN = 1; public static double SLOWMODE_TURN = 0.3; + //Motor Positions + public static double SLIDE_POWER_UP = 1; + public static double SLIDE_POWER_DOWN = .7; + public static int SLIDEUP = 1150; + public static int HANGRELEASE = 2500; + public static int HANG = 1000; + public static int HANGPLANE = 1900; + + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java index 811a874..7e0d326 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java @@ -57,4 +57,8 @@ public class Constants { public static final String RIGHTARM = "rightArm"; public static final String WRIST = "wrist"; public static final String PLANE = "plane"; + public static final String SLIDERIGHT = "slideright"; + public static final String SLIDELEFT = "slideleft"; + public static final String HANGRIGHT = "hangright"; + public static final String HANGLEFT = "hangleft"; } \ No newline at end of file