Subsystem work-in-progress
This commit is contained in:
@ -49,9 +49,15 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
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 com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
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".
|
* 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.
|
// Declare OpMode members for each of the 4 motors.
|
||||||
private final ElapsedTime runtime = new ElapsedTime();
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
private ClawSubsystem.ClawState clawState;
|
||||||
|
|
||||||
|
// private WristSubsystem.WristState wristState;
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() {
|
public void runOpMode() {
|
||||||
|
|
||||||
@ -97,7 +107,25 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
||||||
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_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
|
// TODO: replace these with your encoder ports
|
||||||
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
||||||
@ -134,8 +162,37 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
waitForStart();
|
waitForStart();
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
|
|
||||||
|
claw.setState(ClawSubsystem.ClawState.CLOSED);
|
||||||
|
|
||||||
|
|
||||||
// run until the end of the match (driver presses STOP)
|
// run until the end of the match (driver presses STOP)
|
||||||
while (opModeIsActive()) {
|
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;
|
double max;
|
||||||
|
|
||||||
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
// 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("Left Encoder Value", leftEncoder.getDeltaPosition());
|
||||||
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
||||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
||||||
|
telemetry.addData("Claw State", claw.getState());
|
||||||
|
telemetry.addData("Wrist Servo Position", arm.getPosition());
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}}
|
}}
|
||||||
|
|||||||
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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;
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -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) {
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
@ -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
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user