From 39094d531e184d7cb872a1b287c5ab99640e922e Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Sun, 3 Nov 2024 11:18:50 -0800 Subject: [PATCH] Added working lift subsystem --- .../firstinspires/ftc/teamcode/LiftTest.java | 54 ++------ .../ftc/teamcode/configs/RobotConstants.java | 18 +-- .../ftc/teamcode/subsystem/LiftSubsystem.java | 116 ++++++------------ 3 files changed, 49 insertions(+), 139 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java index dfe93a7..fa085a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java @@ -36,6 +36,8 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem; + @TeleOp(name = "Lift Test", group = "Debug") public class LiftTest extends LinearOpMode { @@ -43,6 +45,9 @@ public class LiftTest extends LinearOpMode { private final ElapsedTime runtime = new ElapsedTime(); private final int MIN_POINT = 0; + // 2000 ~ 2500 + + // 3750 max private final int MAX_POINT = 6500; @Override @@ -51,9 +56,7 @@ public class LiftTest extends LinearOpMode { /* * Instantiate Lift */ - DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor"); - liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - liftDrive.setDirection(DcMotorSimple.Direction.REVERSE); + LiftSubsystem lift = new LiftSubsystem(hardwareMap); /* * Instantiate gamepad state holders @@ -71,60 +74,25 @@ public class LiftTest extends LinearOpMode { previousGamepad1.copy(currentGamepad1); currentGamepad1.copy(gamepad1); - liftDrive.setPower(.5); - - // Max position is 6800, safely setting to 6500 - if (currentGamepad1.square && !previousGamepad1.square) { - liftDrive.setTargetPosition(MIN_POINT); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lift.toZero(); } if (currentGamepad1.triangle && !previousGamepad1.triangle) { - liftDrive.setTargetPosition(2000); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lift.toHighBucket(); } if (currentGamepad1.circle && !previousGamepad1.circle) { - liftDrive.setTargetPosition(4000); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lift.toLowBucket(); } if (currentGamepad1.cross && !previousGamepad1.cross) { - liftDrive.setTargetPosition(MAX_POINT); - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lift.switchState(); } - if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { - int newPosition = liftDrive.getCurrentPosition() - 250; - if (newPosition < MIN_POINT) { - liftDrive.setTargetPosition(MIN_POINT); - } else { - liftDrive.setTargetPosition(newPosition); - } - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { - int newPosition = liftDrive.getCurrentPosition() + 250; - if (newPosition > MAX_POINT) { - liftDrive.setTargetPosition(MAX_POINT); - } else { - liftDrive.setTargetPosition(newPosition); - } - liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - - // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. -// double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value - - // Send calculated power to wheels - - // Show the elapsed game time and wheel power. telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition()); + telemetry.addData("Lift Drive Position", lift.getPosition()); telemetry.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 index 819ebfe..18d77d4 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 @@ -13,20 +13,8 @@ public class RobotConstants { public static double wristFloor = 0.625; public static double wristBucket = 0.25; - - 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; + public static int liftToLowBucketPos = 2250; + public static int liftToHighBucketPos = 3700; + public static double liftPower = .45; } \ 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 index 02b6d0c..08c5e5b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -1,124 +1,78 @@ 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.liftPower; 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.DcMotorSimple; 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 RunAction toZero, toLowBucket, toHighBucket; + public enum LiftState { + FLOOR, LOW_BUCKET, HIGH_BUCKET + } + private LiftState liftState; - 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); + public LiftSubsystem(HardwareMap hardwareMap) { + lift = hardwareMap.get(DcMotor.class, "lift-motor"); 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; + lift.setTargetPosition(b); + lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); } public void toZero() { setTarget(liftZeroPos); + setState(LiftState.FLOOR); + } + + public void switchState() { + if(this.liftState == LiftState.FLOOR) { + this.toLowBucket(); + } else if (this.liftState == LiftState.LOW_BUCKET) { + this.toHighBucket(); + } else if (this.liftState == LiftState.HIGH_BUCKET) { + this.toZero(); + } } public void toLowBucket() { setTarget(liftToLowBucketPos); + setState(LiftState.LOW_BUCKET); } public void toHighBucket() { setTarget(liftToHighBucketPos); + setState(LiftState.HIGH_BUCKET); } - 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); + lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lift.setDirection(DcMotorSimple.Direction.REVERSE); + lift.setPower(liftPower); + } + + private void setState(LiftState liftState) { + this.liftState = liftState; + } + + public int getPosition() { + return lift.getCurrentPosition(); } public void start() { - initalPos = lift.getCurrentPosition(); setTarget(10); }