From edff580e84a094915c094fb953e1a9e50e2b2c5e Mon Sep 17 00:00:00 2001 From: robotics1 Date: Wed, 29 Jan 2025 15:06:38 -0800 Subject: [PATCH] Specimen States written --- .../ftc/teamcode/PreLoadedBlueBasketAuto.java | 4 +- .../ftc/teamcode/SpecimenAuto.java | 63 ++++++++++++++++--- .../ftc/teamcode/SpecimenAutoLineTest.java | 10 +-- .../ftc/teamcode/configs/RobotConstants.java | 6 ++ .../ftc/teamcode/subsystem/ArmSubsystem.java | 7 ++- .../subsystem/DualMotorSliderSubsystem.java | 12 ++++ .../ftc/teamcode/subsystem/LiftArmWrist.java | 8 +-- .../teamcode/subsystem/WristSubsystem.java | 12 ++++ 8 files changed, 100 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java index 8934107..b41df39 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java @@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -@Autonomous(name = "Specimen Test", group = "Competition") +@Autonomous(name = "PreLoadedBlue", group = "Competition") public class PreLoadedBlueBasketAuto extends OpMode { private Telemetry telemetryA; @@ -27,7 +27,7 @@ public class PreLoadedBlueBasketAuto extends OpMode { public void init() { follower = new Follower(hardwareMap); - follower.setMaxPower(.45); + follower.setMaxPower(.20); follower.setStartingPose(startPose); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java index 1cd08eb..546a8c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java @@ -1,44 +1,85 @@ package org.firstinspires.ftc.teamcode; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.State; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; -@Autonomous(name = "Specimen Auto", group = "Dev") +@Autonomous(name = "Specimen Auto", group = "Development") public class SpecimenAuto extends OpMode { private Follower follower; private int state; private ElapsedTime runtime; + private DualMotorSliderSubsystem lift; + private ClawSubsystem claw; + private WristSubsystem wrist; + private ArmSubsystem arm; + private HangMotorSubsystem hang; + + + @Override public void init(){ - state = 0; - follower = new Follower(hardwareMap); - follower.setMaxPower(.90); - runtime = new ElapsedTime(); + lift = new DualMotorSliderSubsystem(hardwareMap); + claw = new ClawSubsystem(hardwareMap); + arm = new ArmSubsystem(hardwareMap); + wrist = new WristSubsystem(hardwareMap); + hang = new HangMotorSubsystem(hardwareMap); + follower = new Follower(hardwareMap); + runtime = new ElapsedTime(); + + state = 0; + lift.init(); + claw.init(); + wrist.InitAuto(); + arm.initAuto(); + + follower.setMaxPower(.45); + } @Override public void loop() { + telemetry.addData("state", state); switch(state) { case 0: - //path 1 + wrist.toSpecimenPrep(); + arm.toSpecimenPrep(); + state = 1; + new SleepAction(5); break; case 1: - //specimen drop + state = 2; break; case 2: - //path 2 + lift.toSpecimanHangHeight(); + new SleepAction(5); + state = 3; break; case 3: - //specimen pick up + wrist.toSpecimenHang(); + new SleepAction(5); + state = 4; break; case 4: - //path 3 + lift.toSpecimanReleaseHeight(); + new SleepAction(5); + state = 5; break; case 5: //specimen drop @@ -68,5 +109,7 @@ public class SpecimenAuto extends OpMode { //specimen drop break; } + new SleepAction(5); } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java index 6fc2181..fbed29f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java @@ -21,7 +21,7 @@ public class SpecimenAutoLineTest extends OpMode { private PathChain path; - private final Pose startPose = new Pose(8, 60); + private final Pose startPose = new Pose(8, 55); @Override public void init() { @@ -36,12 +36,12 @@ public class SpecimenAutoLineTest extends OpMode { .addPath( // Line 1 new BezierLine( - new Point(8.000, 60.000, Point.CARTESIAN), + new Point(8.000, 55.000, Point.CARTESIAN), new Point(39.500, 60.000, Point.CARTESIAN) ) ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) - .addPath( + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) +/* .addPath( // Line 2 new BezierLine( new Point(39.500, 60.000, Point.CARTESIAN), @@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode { new Point(37.000, 68.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(180)).build(); + .setConstantHeadingInterpolation(Math.toRadians(180))*/.build(); follower.followPath(path); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 3f2cdfa..4dbb6a7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -19,12 +19,18 @@ public class RobotConstants { public final static double wristHangBlueberrySkyhook = 0.7; public final static int slideHangBlueberrySkyhook = 500; public final static int slideBelowFloor = -150; + public final static int slideSpecimanHang = 900; + public final static int slideSpecimanRelease = 200; public final static int backwardBucketDrop = 4670; public final static double armBucket = 0.45; + public final static double armSpecimen = 0.155; public final static double armInit = 0.13; public final static double wristInit = 0.125; public final static double wristPickup = 0.475; public final static double wristBucket = 0.56; + public final static double wristSpecimenPrep = 0.63; + public final static double wristSpecimenHang = 0.53; + public final static double wristFloor = 0.75; public final static double wristBackwardBucket = 0.725; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java index 1a80f1f..0290f06 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -8,6 +8,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueb import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armReverseBucket; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSpecimen; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.ServoImplEx; @@ -15,7 +16,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class ArmSubsystem { public enum ArmState { - PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET + PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET, SPECIMEN } private ServoImplEx arm; @@ -54,6 +55,10 @@ public class ArmSubsystem { arm.setPosition(armBucket); setState(ArmState.BUCKET); } + public void toSpecimenPrep() { + arm.setPosition(armSpecimen); + setState(ArmState.SPECIMEN); + } public void toInitPosition(){ arm.setPosition(armInit); setState(ArmState.INIT); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java index 0c735e1..343c39b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java @@ -7,6 +7,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBu import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanHang; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanRelease; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -149,6 +151,16 @@ public class DualMotorSliderSubsystem { } + public void toSpecimanHangHeight(){ + setTargetPosition(slideSpecimanHang); + } + + public void toSpecimanReleaseHeight(){ + setTargetPosition(slideSpecimanRelease); + } + + + public int getFixedPosition() { return liftSlideLeft.getCurrentPosition(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java index 09c574d..d33bb7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java @@ -94,11 +94,11 @@ public class LiftArmWrist extends LinearOpMode { } if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) { - lift.toHighBucketPosition(); + lift.toSpecimanHangHeight(); } if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) { - lift.toLowBucketPosition(); + lift.toSpecimanReleaseHeight(); } if (currentGamepad1.y && !previousGamepad1.y) { @@ -111,11 +111,11 @@ public class LiftArmWrist extends LinearOpMode { if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { - lift.toHighBucketPosition(); + lift.setTargetPosition(lift.getCurrentPosition() + 250); } if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { - lift.toLowBucketPosition(); + lift.setTargetPosition(lift.getCurrentPosition() - 250); } if(currentGamepad1.left_trigger > 0){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java index 73615d5..617c433 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java @@ -9,6 +9,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlu import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenHang; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenPrep; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel; import androidx.annotation.NonNull; @@ -74,6 +76,16 @@ public class WristSubsystem { wrist.setPosition(wristBucket); } + public void toSpecimenPrep() { + setState(WristState.SPECIMEN); + wrist.setPosition(wristSpecimenPrep); + } + + public void toSpecimenHang() { + setState(WristState.SPECIMEN); + wrist.setPosition(wristSpecimenHang); + } + public void toPickupPosition() { setState(WristState.PICKUP); wrist.setPosition(wristPickup);