From eb0042a5f667fa2ccc28f634bffa52125d6f0e96 Mon Sep 17 00:00:00 2001 From: Carlos Date: Wed, 30 Oct 2024 08:24:40 -0700 Subject: [PATCH] Subsystem work-in-progress --- .../ftc/teamcode/BasicOmniOpMode_Linear.java | 59 ++++++++ .../ftc/teamcode/BlueBucket.java | 120 ++++++++++++++++ .../ftc/teamcode/CometDrive.java | 36 +++++ .../ftc/teamcode/configs/RobotConstants.java | 29 ++++ .../ftc/teamcode/runmodes/Auto.java | 132 ++++++++++++++++++ .../ftc/teamcode/runmodes/Teleop.java | 90 ++++++++++++ .../ftc/teamcode/subsystem/ArmSubsystem.java | 59 ++++++++ .../ftc/teamcode/subsystem/ClawSubsystem.java | 67 +++++++++ .../ftc/teamcode/subsystem/LiftSubsystem.java | 125 +++++++++++++++++ .../ftc/teamcode/util/action/Action.java | 11 ++ .../ftc/teamcode/util/action/Actions.java | 24 ++++ .../teamcode/util/action/FieldConstants.java | 39 ++++++ .../ftc/teamcode/util/action/RunAction.java | 30 ++++ .../util/action/SequentialAction.java | 43 ++++++ .../ftc/teamcode/util/action/SleepAction.java | 32 +++++ 15 files changed, 896 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBucket.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java index 18aa94e..abbd6e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java @@ -49,9 +49,15 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.PwmControl; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoImplEx; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; /* * This file contains an example of a Linear "OpMode". @@ -87,6 +93,10 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { // Declare OpMode members for each of the 4 motors. private final ElapsedTime runtime = new ElapsedTime(); + private ClawSubsystem.ClawState clawState; + +// private WristSubsystem.WristState wristState; + @Override public void runOpMode() { @@ -97,7 +107,25 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); + /* + * Instantiate Claw + */ + ClawSubsystem claw = new ClawSubsystem(hardwareMap, clawState); + /* + * Instantiate Arm + */ + ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); + //arm.setPwmRange(PwmControl.PwmRange.defaultRange); + + /* + * Instantiate gamepad state holders + */ + Gamepad currentGamepad1 = new Gamepad(); + Gamepad previousGamepad1 = new Gamepad(); + + + arm.resetDeviceConfigurationForOpMode(); // TODO: replace these with your encoder ports Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); @@ -134,8 +162,37 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { waitForStart(); runtime.reset(); + claw.setState(ClawSubsystem.ClawState.CLOSED); + + // run until the end of the match (driver presses STOP) while (opModeIsActive()) { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); +// +// if (gamepad1.cross) { +// claw.switchState(); +// } + + if (currentGamepad1.cross && !previousGamepad1.cross) { + // This will set intakeToggle to true if it was previously false + // and intakeToggle to false if it was previously true, + // providing a toggling behavior. + claw.switchState(); + } + + if (currentGamepad1.circle && !previousGamepad1.circle) { +// wrist.switchState(); + arm.setPosition(0); + } + + if (currentGamepad1.square && !previousGamepad1.square) { +// wrist.switchState(); + arm.setPosition(.5); + } + + double max; // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. @@ -193,6 +250,8 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition()); telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition()); telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition()); + telemetry.addData("Claw State", claw.getState()); + telemetry.addData("Wrist Servo Position", arm.getPosition()); telemetry.update(); } }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBucket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBucket.java new file mode 100644 index 0000000..f19d44c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBucket.java @@ -0,0 +1,120 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.runmodes.Auto; +import org.firstinspires.ftc.teamcode.util.action.Actions; +import org.firstinspires.ftc.teamcode.util.action.SequentialAction; + + +@Autonomous(name = "BlueBucket", group = "B") +public class BlueBucket extends OpMode { + public int pathState; + public Auto auto; + + @Override + public void init() { + auto = new Auto(hardwareMap, telemetry, new Follower(hardwareMap), true, true); + } + + @Override + public void start() { + auto.start(); +// setPathState(0); + } + + @Override + public void loop() { + auto.update(); + pathUpdate(); + + telemetry.addData("state", pathState); + telemetry.addData("x", auto.follower.getPose().getX()); + telemetry.addData("y", auto.follower.getPose().getY()); + telemetry.addData("h", auto.follower.getPose().getHeading()); + } + + public void pathUpdate() { + switch (pathState) { + case 0: + Actions.runBlocking(auto.claw.closeClaw); + //Actions.runBlocking(auto.lift.toHighChamber); + //auto.follower.followPath(auto.preload); + //setPathState(1); + break; + case 1: + if (!auto.follower.isBusy()) { + + // if (auto.lift.isAtTarget()) { + // Actions.runBlocking(auto.lift.releaseHighChamber); + // } + +// if (auto.lift.isAtTarget()) { +// Actions.runBlocking(auto.claw.openClaw); +// } + +// Actions.runBlocking(auto.lift.toZero); +// auto.follower.followPath(auto.element1); +// Actions.runBlocking(auto.intake.pivotGround); +// Actions.runBlocking(auto.intake.spinIn); + setPathState(2); + } + break; + case 2: + if (!auto.follower.isBusy()) { + auto.follower.followPath(auto.score1); +// Actions.runBlocking(auto.intake.spinStop); +// Actions.runBlocking(auto.intake.pivotTransfer); + setPathState(3); + } + break; + case 3: + if (!auto.follower.isBusy()) { + Actions.runBlocking( + new SequentialAction( +// auto.transfer, +// auto.lift.toLowBucket + ) + ); + +// if (auto.lift.isAtTarget()) { +// Actions.runBlocking(auto.box.toScoring); +// } + + auto.follower.followPath(auto.element2); + setPathState(4); + } + break; + case 4: + if (!auto.follower.isBusy()) { + auto.follower.followPath(auto.score2); + setPathState(5); + } + break; + case 5: + if (!auto.follower.isBusy()) { + auto.follower.followPath(auto.element3); + setPathState(6); + } + break; + case 6: + if (!auto.follower.isBusy()) { + auto.follower.followPath(auto.score3); + setPathState(7); + } + break; + case 7: + if (!auto.follower.isBusy()) { + auto.follower.followPath(auto.park); + setPathState(-1); + } + break; + } + } + + public void setPathState(int x) { + pathState = x; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java new file mode 100644 index 0000000..aeea4d1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode; + +import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.runmodes.Teleop; + +@TeleOp(name="Drive", group="A") +public class CometDrive extends OpMode { + + private Teleop teleop; + + @Override + public void init() { + teleop = new Teleop(hardwareMap, + telemetry, + new Follower(hardwareMap), + blueBucketStartPose, + false, + gamepad1, + gamepad2); + } + + @Override + public void start() { + teleop.start(); + } + + @Override + public void loop() { + teleop.update(); + } +} 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 new file mode 100644 index 0000000..b5931a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.configs; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class RobotConstants { + public static double clawClose = 0.95; + public static double clawOpen = 0.5; + + public static double wristEngage = 0.95; + public static double wristHold = 0.50; + public static double wristDisengage = 0.05; + + public static double intakeSpinInPwr = 1; + public static double intakeSpinOutPwr = -0.25; + public static double intakeSpinStopPwr = 0; + public static double intakePivotTransferPos= 0.965; + public static double intakePivotGroundPos = 0.4; + public static double boxTransferPos= 0.95; + public static double boxScoringPos = 0.5; + public static int liftZeroPos = 0; + public static int liftToHumanPlayerPos = 750; + public static int liftToHighChamberPos = 2576; + public static int liftReleaseHighChamberPos = 2276; + public static int liftToLowChamberPos = 2000; + public static int liftReleaseLowChamberPos = 1900; + public static int liftToLowBucketPos = 2230; + public static int liftToHighBucketPos = 2230; +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java new file mode 100644 index 0000000..494c69c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java @@ -0,0 +1,132 @@ +package org.firstinspires.ftc.teamcode.runmodes; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.*; + +import org.firstinspires.ftc.teamcode.util.action.Action; +import org.firstinspires.ftc.teamcode.util.action.RunAction; +import org.firstinspires.ftc.teamcode.util.action.SequentialAction; +import org.firstinspires.ftc.teamcode.util.action.SleepAction; + +public class Auto { + + private RobotStart startLocation; + + public ClawSubsystem claw; + public ClawSubsystem.ClawState clawState; + + public Follower follower; + public Telemetry telemetry; + + public RunAction transfer; + public Path preload, element1, score1, element2, score2, element3, score3, park; + private Pose startPose, preloadPose, element1Pose, element1ControlPose, element2Pose, element2ControlPose, element3Pose, element3ControlPose, elementScorePose, parkControlPose, parkPose; + + public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, boolean isBlue, boolean isBucket) { + claw = new ClawSubsystem(hardwareMap, clawState); + + this.follower = follower; + this.telemetry = telemetry; + + startLocation = isBlue ? (isBucket ? RobotStart.BLUE_BUCKET : RobotStart.BLUE_OBSERVATION) : (isBucket ? RobotStart.RED_BUCKET : RobotStart.RED_OBSERVATION); + + createPoses(); + buildPaths(); + + transfer = new RunAction(this::transfer); + } + + public void init_loop() { + } + + public void createPoses() { + switch (startLocation) { + case BLUE_BUCKET: + startPose = blueBucketStartPose; + preloadPose = blueBucketPreloadPose; + element1ControlPose = blueBucketLeftSampleControlPose; + element1Pose = blueBucketLeftSamplePose; + element2ControlPose = blueBucketMidSampleControlPose; + element2Pose = blueBucketMidSamplePose; + element3ControlPose = blueBucketRightSampleControlPose; + element3Pose = blueBucketRightSamplePose; + elementScorePose = blueBucketScorePose; + parkControlPose = blueBucketParkControlPose; + parkPose = blueBucketParkPose; + break; + + case BLUE_OBSERVATION: + startPose = blueObservationStartPose; + //parkPose = blueObservationPark; + break; + + case RED_BUCKET: + startPose = redBucketStartPose; + //parkPose = redBucketPark; + break; + + case RED_OBSERVATION: + startPose = redObservationStartPose; + //parkPose = redObservationPark; + break; + } + + } + + public void buildPaths() { + follower.setStartingPose(startPose); + + preload = new Path(new BezierLine(new Point(startPose), new Point(preloadPose))); + preload.setLinearHeadingInterpolation(startPose.getHeading(), preloadPose.getHeading()); + + element1 = new Path(new BezierCurve(new Point(preloadPose), new Point(element1ControlPose), new Point(element1Pose))); + element1.setLinearHeadingInterpolation(preloadPose.getHeading(), element1Pose.getHeading()); + + score1 = new Path(new BezierLine(new Point(element1Pose), new Point(elementScorePose))); + score1.setLinearHeadingInterpolation(element1Pose.getHeading(), elementScorePose.getHeading()); + + element2 = new Path(new BezierCurve(new Point(element1Pose), new Point(element2ControlPose), new Point(element2Pose))); + element2.setLinearHeadingInterpolation(element1Pose.getHeading(), element2Pose.getHeading()); + + score2 = new Path(new BezierLine(new Point(element2Pose), new Point(elementScorePose))); + score2.setLinearHeadingInterpolation(element2Pose.getHeading(), elementScorePose.getHeading()); + + element3 = new Path(new BezierCurve(new Point(element2Pose), new Point(element3ControlPose), new Point(element3Pose))); + element3.setLinearHeadingInterpolation(element2Pose.getHeading(), element3Pose.getHeading()); + + score3 = new Path(new BezierLine(new Point(element3Pose), new Point(elementScorePose))); + score3.setLinearHeadingInterpolation(element3Pose.getHeading(), elementScorePose.getHeading()); + + park = new Path(new BezierCurve(new Point(elementScorePose), new Point(parkControlPose), new Point(parkPose))); + park.setLinearHeadingInterpolation(elementScorePose.getHeading(), parkPose.getHeading()); + } + + public void start() { + claw.start(); + //lift.start(); + //extend.start(); + //intake.start(); + //box.start(); + } + + public void update() { + follower.update(); + //lift.updatePIDF(); + //extend.updatePIDF(); + } + + private Action transfer() { + return new SequentialAction( + new SleepAction(1) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java new file mode 100644 index 0000000..65111df --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java @@ -0,0 +1,90 @@ +package org.firstinspires.ftc.teamcode.runmodes; + +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.util.action.RunAction; + +public class Teleop { + + private ClawSubsystem claw; + private ClawSubsystem.ClawState clawState; + + private Follower follower; + private Pose startPose; + + private Telemetry telemetry; + + private Gamepad gamepad1; + private Gamepad gamepad2; + private Gamepad currentGamepad1 = new Gamepad(); + private Gamepad currentGamepad2 = new Gamepad(); + private Gamepad previousGamepad1 = new Gamepad(); + private Gamepad previousGamepad2 = new Gamepad(); + + public RunAction stopDrive, startDrive; + + private boolean fieldCentric = true; + + public double speed = 0.75; + + public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, boolean fieldCentric, Gamepad gamepad1, Gamepad gamepad2) { + claw = new ClawSubsystem(hardwareMap, clawState); + + this.follower = follower; + this.startPose = startPose; + + this.fieldCentric = fieldCentric; + this.telemetry = telemetry; + this.gamepad1 = gamepad1; + this.gamepad2 = gamepad2; + + stopDrive = new RunAction(this::stopDrive); + startDrive = new RunAction(this::startDrive); + } + + public void start() { + claw.start(); + follower.setPose(startPose); + follower.startTeleopDrive(); + } + + private void startDrive() { + follower.startTeleopDrive(); + } + + private void stopDrive(){ + follower.breakFollowing(); + } + + public void update() { + previousGamepad1.copy(currentGamepad1); + previousGamepad2.copy(currentGamepad2); + currentGamepad1.copy(gamepad1); + currentGamepad2.copy(gamepad2); + + if (gamepad1.right_bumper) + speed = 1; + else if (gamepad1.left_bumper) + speed = 0.25; + else + speed = 0.75; + + if (currentGamepad1.a && !previousGamepad1.a) + claw.switchState(); + + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed, !fieldCentric); + follower.update(); + + telemetry.addData("X", follower.getPose().getX()); + telemetry.addData("Y", follower.getPose().getY()); + telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading())); + + telemetry.addData("Claw State", clawState); + telemetry.update(); + } +} 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 new file mode 100644 index 0000000..90be386 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristDisengage; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristEngage; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHold; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoImplEx; + +import org.firstinspires.ftc.teamcode.util.action.RunAction; + +public class ArmSubsystem { + + public enum ArmState { + DISENGAGE, ENGAGE + } + + public ServoImplEx wrist; + public ArmState state; + public RunAction engageWrist, disengageWrist; + + public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) { + wrist = hardwareMap.get(ServoImplEx.class, "arm-servo"); + wrist.resetDeviceConfigurationForOpMode(); + + this.state = armState; + + engageWrist = new RunAction(this::engageArm); + disengageWrist = new RunAction(this::disengageArm); + } + + public void setState(ArmState wristState) { + if (wristState == ArmState.ENGAGE) { + wrist.setPosition(wristEngage); + this.state = ArmState.ENGAGE; + } else if (wristState == ArmState.DISENGAGE) { + wrist.setPosition(wristDisengage); + this.state = ArmState.DISENGAGE; + } + } + + public void engageArm() { + setState(ArmState.ENGAGE); + } + + public void disengageArm() { + setState(ArmState.DISENGAGE); + } + + public void switchState() { + if (state == ArmState.ENGAGE) { + setState(ArmState.DISENGAGE); + } else if (state == ArmState.DISENGAGE) { + setState(ArmState.ENGAGE); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java new file mode 100644 index 0000000..b508ee4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.util.action.Actions; +import org.firstinspires.ftc.teamcode.util.action.RunAction; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose; + +public class ClawSubsystem { + + public enum ClawState { + CLOSED, OPEN + } + + private Servo claw; + private ClawState state; + public RunAction openClaw, closeClaw; + + public ClawSubsystem(HardwareMap hardwareMap, ClawState clawState) { + claw = hardwareMap.get(Servo.class, "claw-servo"); + this.state = clawState; + + openClaw = new RunAction(this::openClaw); + closeClaw = new RunAction(this::closeClaw); + } + + public void setState(ClawState clawState) { + if (clawState == ClawState.CLOSED) { + claw.setPosition(clawClose); + this.state = ClawState.CLOSED; + } else if (clawState == ClawState.OPEN) { + claw.setPosition(clawOpen); + this.state = ClawState.OPEN; + } + } + + public ClawState getState() { + return this.state; + } + + public void switchState() { + if (state == ClawState.CLOSED) { + setState(ClawState.OPEN); + } else if (state == ClawState.OPEN) { + setState(ClawState.CLOSED); + } + } + + public void openClaw() { + setState(ClawState.OPEN); + } + + public void closeClaw() { + setState(ClawState.CLOSED); + } + + public void init() { + Actions.runBlocking(closeClaw); + } + + public void start() { + Actions.runBlocking(closeClaw); + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java new file mode 100644 index 0000000..02b6d0c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -0,0 +1,125 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftReleaseHighChamberPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftReleaseLowChamberPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighChamberPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHumanPlayerPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowChamberPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftZeroPos; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.util.action.RunAction; + +public class LiftSubsystem { + private Telemetry telemetry; + + public DcMotor lift; + private int pos, initalPos; + public RunAction toZero, toLowBucket, toHighBucket, toLowChamber, releaseLowChamber, toHighChamber, releaseHighChamber, toHumanPlayer; + + public static int target; + + public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.telemetry = telemetry; + this.telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + lift = hardwareMap.get(DcMotor.class, "lift"); + lift.setDirection(DcMotor.Direction.REVERSE); + lift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + toZero = new RunAction(this::toZero); + toLowBucket = new RunAction(this::toLowBucket); + toHighBucket = new RunAction(this::toHighBucket); + toLowChamber = new RunAction(this::toLowChamber); + releaseLowChamber = new RunAction(this::releaseLowChamber); + toHighChamber = new RunAction(this::toHighChamber); + releaseHighChamber = new RunAction(this::releaseHighChamber); + toHumanPlayer = new RunAction(this::toHumanPlayer); + } + + // Manual Control // + public void manual(double n) { //(int liftPos, boolean negative) { + lift.setPower(n); + } + + public void setTarget(int b) { + target = b; + } + + public void addToTarget(int b) { + target += b; + } + + public void toZero() { + setTarget(liftZeroPos); + } + + public void toLowBucket() { + setTarget(liftToLowBucketPos); + } + + public void toHighBucket() { + setTarget(liftToHighBucketPos); + } + + public void toLowChamber() { + setTarget(liftToLowChamberPos); + } + + public void releaseLowChamber() { + setTarget(liftReleaseLowChamberPos); + } + + public void toHighChamber() { + setTarget(liftToHighChamberPos); + } + + public void releaseHighChamber() { + setTarget(liftReleaseHighChamberPos); + } + + public void toHumanPlayer() { + setTarget(liftToHumanPlayerPos); + } + + // Util // + public double getPos() { + updatePos(); + return pos; + } + + public void updatePos() { + pos = lift.getCurrentPosition() - initalPos; + } + + public boolean isAtTarget() { + return Math.abs(pos - target) < 10; + } + + public void resetEncoder() { + lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + // Init + Start // + + public void init() { + resetEncoder(); + initalPos = lift.getCurrentPosition(); + lift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void start() { + initalPos = lift.getCurrentPosition(); + setTarget(10); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java new file mode 100644 index 0000000..ee19186 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java @@ -0,0 +1,11 @@ +package org.firstinspires.ftc.teamcode.util.action; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +public interface Action { + boolean run(TelemetryPacket p); + + default void preview(Canvas fieldOverlay) { + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java new file mode 100644 index 0000000..5e40e32 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.util.action; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +public class Actions { + + public static void runBlocking(Action a) { + FtcDashboard dash = FtcDashboard.getInstance(); + Canvas c = new Canvas(); + a.preview(c); + + boolean b = true; + while (b && !Thread.currentThread().isInterrupted()) { + TelemetryPacket p = new TelemetryPacket(); + p.fieldOverlay().getOperations().addAll(c.getOperations()); + + b = a.run(p); + + dash.sendTelemetryPacket(p); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java new file mode 100644 index 0000000..5e425cc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.util.action; + +import com.acmerobotics.dashboard.config.Config; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; + +@Config +public class FieldConstants { + + public enum RobotStart { + BLUE_BUCKET, + BLUE_OBSERVATION, + RED_BUCKET, + RED_OBSERVATION + } + + public static final Pose blueBucketStartPose = new Pose(8, 79.5, Math.toRadians(180)); + public static final Pose blueObservationStartPose = new Pose(8, 36, Math.toRadians(180)); + public static final Pose redBucketStartPose = new Pose(144-8, 79.5, 0); + public static final Pose redObservationStartPose = new Pose(144-8, 36, 0); + + // Blue Preload Poses + public static final Pose blueBucketPreloadPose = new Pose(34.5, 79.5, Math.toRadians(180)); + + // Blue Bucket Sample Poses + public static final Pose blueBucketLeftSamplePose = new Pose(34.75, 113.5, Math.toRadians(66)); + public static final Pose blueBucketLeftSampleControlPose = new Pose(32, 108); + public static final Pose blueBucketMidSamplePose = new Pose(33, 125.5, Math.toRadians(73)); + public static final Pose blueBucketMidSampleControlPose = new Pose(47.5, 110); + public static final Pose blueBucketRightSamplePose = new Pose(33, 133, Math.toRadians(74)); + public static final Pose blueBucketRightSampleControlPose = new Pose(46, 101); + + public static final Pose blueBucketScorePose = new Pose(16, 128, Math.toRadians(-45)); + + public static final Pose blueBucketParkPose = new Pose(65, 97.75, Math.toRadians(90)); + public static final Pose blueBucketParkControlPose = new Pose(60.25, 123.5); + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java new file mode 100644 index 0000000..207dd65 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.teamcode.util.action; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +public class RunAction implements Action { + + private final Runnable runnable; + private Runnable callback; + + public RunAction(Runnable runnable) { + this.runnable = runnable; + } + + public void runAction() { + runnable.run(); + if (callback != null) { + callback.run(); + } + } + + public void setCallback(Runnable callback) { + this.callback = callback; + } + + // Adapter to make Action compatible with the Action interface + public boolean run(TelemetryPacket p) { + runAction(); + return false; // Regular actions complete after one execution + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java new file mode 100644 index 0000000..948887c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java @@ -0,0 +1,43 @@ +package org.firstinspires.ftc.teamcode.util.action; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class SequentialAction implements Action { + private List actions; + + public SequentialAction(List actions) { + this.actions = new ArrayList<>(actions); + } + + public SequentialAction(Action... actions) { + this(Arrays.asList(actions)); + } + + @Override + public boolean run(TelemetryPacket p) { + if (actions.isEmpty()) { + return false; + } + + if (actions.get(0).run(p)) { + return true; + } else { + actions.remove(0); + return run(p); + } + } + + @Override + public void preview(Canvas fieldOverlay) { + for (Action a : actions) { + a.preview(fieldOverlay); + } + } + + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java new file mode 100644 index 0000000..c1dfa00 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.util.action; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import java.util.Timer; +import java.util.TimerTask; + +public class SleepAction implements Action { + private final double dt; + private boolean isFinished = false; + + public SleepAction(double dt) { + this.dt = dt; + } + + @Override + public boolean run(TelemetryPacket p) { + if (!isFinished) { + Timer timer = new Timer(); + timer.schedule(new TimerTask() { + @Override + public void run() { + isFinished = true; + } + }, (long) (dt * 1000)); + } else { + isFinished = false; // Reset the flag after sleep is complete + return false; // Indicate that the action is finished + } + return true; // Indicate that the action is still running + } +} \ No newline at end of file