SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Added working lift subsystem

Browse Source
This commit is contained in:
Carlos Rivas
2024-11-03 11:18:50 -08:00
parent 1c922f025e
commit 39094d531e
3 changed files with 49 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java
View File

@ -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();
}
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -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;
}

116
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java
View File

@ -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);
}