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

Subsystem work-in-progress

Browse Source
This commit is contained in:
Carlos
2024-10-30 08:24:40 -07:00
parent 657ec8e624
commit eb0042a5f6
15 changed files with 896 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java
View File

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

120
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBucket.java Normal file
View File

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

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometDrive.java Normal file
View File

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

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java Normal file
View File

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

132
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java Normal file
View File

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

90
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java Normal file
View File

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

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java Normal file
View File

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

67
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java Normal file
View File

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

125
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java Normal file
View File

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

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Action.java Normal file
View File

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

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java Normal file
View File

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

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/FieldConstants.java Normal file
View File

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

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/RunAction.java Normal file
View File

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

43
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SequentialAction.java Normal file
View File

@ -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<Action> actions;
public SequentialAction(List<Action> 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);
}
}
}

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/SleepAction.java Normal file
View File

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