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

Massive changes to branch:

Browse Source 
- Using Actions
- Rewire logic
- Add usage of telemetry
- Initial Field states
This commit is contained in:
Carlos Rivas
2024-11-05 22:32:09 -08:00
parent 7a42724b44
commit 5b3c92c82c
20 changed files with 331 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CONTROLS.md Normal file
View File

@ -0,0 +1,12 @@
# Controller 1
## Motor Controls
- Left Joystick
- Forward & Backwards
- Right Joystick
- Strafe & Turning
## Arm Controls
-

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

@ -31,10 +31,10 @@ public class DevTeleOpRemix extends OpMode {
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap, telemetry);
arm = new ArmSubsystem(hardwareMap, telemetry);
wrist = new WristSubsystem(hardwareMap, telemetry);
lift = new LiftSubsystem(hardwareMap, telemetry);
motors = new MotorsSubsystem(hardwareMap, telemetry, power);
claw.init();
@ -51,8 +51,8 @@ public class DevTeleOpRemix extends OpMode {
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
wrist.toFloorPosition();
arm.toFloorPosition();
}
}
@ -64,15 +64,15 @@ public class DevTeleOpRemix extends OpMode {
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
arm.toParkPosition();
wrist.toBucketPosition();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
wrist.toBucketPosition();
}
}

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

@ -36,11 +36,11 @@ public class DevTeleOpRemixDeux extends OpMode {
follower = new Follower(hardwareMap);
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
claw = new ClawSubsystem(hardwareMap, telemetry);
arm = new ArmSubsystem(hardwareMap, telemetry);
wrist = new WristSubsystem(hardwareMap, telemetry);
motors = new MotorsSubsystem(hardwareMap, telemetry);
lift = new LiftSubsystem(hardwareMap);
lift = new LiftSubsystem(hardwareMap, telemetry);
claw.init();
arm.init();
@ -60,8 +60,8 @@ public class DevTeleOpRemixDeux extends OpMode {
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
wrist.toFloorPosition();
arm.toFloorPosition();
}
}
@ -73,15 +73,15 @@ public class DevTeleOpRemixDeux extends OpMode {
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
arm.toParkPosition();
wrist.toBucketPosition();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
wrist.toBucketPosition();
}
}

166
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java
View File

@ -1,180 +1,24 @@
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
import android.graphics.Point;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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 org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunMode;
@TeleOp(name = "Dev Teleop", group = "Debug")
public class DevTeleop extends OpMode {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public DcMotor frontLeftMotor;
public DcMotor backLeftMotor;
public DcMotor frontRightMotor;
public DcMotor backRightMotor;
public DevTeleopRunMode runMode;
private double MAX_POWER = .6;
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
claw.init();
arm.init();
wrist.init();
lift.init();
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
this.runMode = new DevTeleopRunMode(hardwareMap, telemetry, gamepad1, gamepad2);
this.runMode.init();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad2.y && !previousGamepad2.y) {
lift.toLowBucket();
arm.bucketArm();
wrist.bucketWrist();
}
}
public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad2.a && !previousGamepad2.a) {
lift.toHighBucket();
arm.bucketArm();
wrist.bucketWrist();
}
}
public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down){
lift.toFloor();
arm.bucketArm();
wrist.floorWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
theHighBucketScore(lift, wrist, arm);
theTravel(lift, arm, wrist);
double max;
// 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
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;
// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// Send calculated power to wheels
frontLeftMotor.setPower(leftFrontPower * MAX_POWER);
frontRightMotor.setPower(rightFrontPower * MAX_POWER);
backLeftMotor.setPower(leftBackPower * MAX_POWER);
backRightMotor.setPower(rightBackPower * MAX_POWER);
// Show the elapsed game time and wheel power.
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Current Lift Position", lift.getPosition());
this.runMode.update();
telemetry.update();
}
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java
View File

@ -10,6 +10,7 @@ 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.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@ -225,6 +226,14 @@ public class BluebAutoV1 extends OpMode {
)
.setConstantHeadingInterpolation(Math.toRadians(90)).build();
follower.holdPoint(
new BezierPoint(
new Point(13.821, 134.839, Point.CARTESIAN)
),
90
);
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
@ -239,7 +248,7 @@ public class BluebAutoV1 extends OpMode {
public void loop() {
follower.update();
if (follower.atParametricEnd()) {
follower.followPath(path);
follower.followPath(path, true);
}
follower.telemetryDebug(telemetryA);
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java
View File

@ -48,7 +48,7 @@ public class ArmTest extends LinearOpMode {
/*
* Instantiate Arm
*/
ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
ArmSubsystem arm = new ArmSubsystem(hardwareMap, telemetry);
/*
* Instantiate gamepad state holders
@ -67,11 +67,11 @@ public class ArmTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.circle && !previousGamepad1.circle) {
arm.parkArm();
arm.toParkPosition();
}
if (currentGamepad1.square && !previousGamepad1.square) {
arm.engageArm();
arm.toBucketPosition();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
View File

@ -48,7 +48,7 @@ public class ClawTest extends LinearOpMode {
/*
* Instantiate Claw
*/
ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN);
ClawSubsystem claw = new ClawSubsystem(hardwareMap, telemetry);
/*
* Instantiate gamepad state holders

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java
View File

@ -54,7 +54,7 @@ public class LiftTest extends LinearOpMode {
/*
* Instantiate Lift
*/
LiftSubsystem lift = new LiftSubsystem(hardwareMap);
LiftSubsystem lift = new LiftSubsystem(hardwareMap, telemetry);
/*
* Instantiate gamepad state holders

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java
View File

@ -48,7 +48,7 @@ public class WristTest extends LinearOpMode {
/*
* Instantiate Wrist
*/
WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
WristSubsystem wrist = new WristSubsystem(hardwareMap, telemetry);
/*
* Instantiate gamepad state holders
@ -67,11 +67,11 @@ public class WristTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
wrist.bucketWrist();
wrist.toBucketPosition();
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
wrist.floorWrist();
wrist.toFloorPosition();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {

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

@ -7,7 +7,7 @@ public class RobotConstants {
public static double clawClose = 1.00;
public static double clawOpen = 0.25;
public static double armEngage = 0.5;
public static double armFloor = 0.5;
public static double armPark = 0.125;
public static double armBucket = 0.175;

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
View File

@ -36,9 +36,9 @@ public class Auto {
public int caseState = 1;
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
claw = new ClawSubsystem(hardwareMap, telemetry);
arm = new ArmSubsystem(hardwareMap, telemetry);
wrist = new WristSubsystem(hardwareMap, telemetry);
this.follower = follower;
this.telemetry = telemetry;
@ -77,13 +77,13 @@ public class Auto {
break;
case 2:
if (clawTimer.getElapsedTimeSeconds() > 2) {
arm.engageArm();
arm.toFloorPosition();
caseState = 3;
}
break;
case 3:
if (armTimer.getElapsedTimeSeconds() > 4) {
wrist.floorWrist();
wrist.toFloorPosition();
caseState = 4;
}
break;
@ -95,8 +95,8 @@ public class Auto {
break;
case 5:
if (armTimer.getElapsedTimeSeconds() > 8) {
arm.bucketArm();
wrist.bucketWrist();
arm.toBucketPosition();
wrist.toBucketPosition();
caseState = 6;
}
break;

165
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java Normal file
View File

@ -0,0 +1,165 @@
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.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
public class DevTeleopRunMode {
/*
Subsystems
*/
private MotorsSubsystem motors;
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
/*
Controllers
*/
public Gamepad GP1;
public Gamepad GP2;
public Gamepad currentGP1;
public Gamepad previousGP1;
public Gamepad currentGP2;
public Gamepad previousGP2;
public FieldStates fieldStates;
public DevTeleopRunMode(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry);
this.claw = new ClawSubsystem(hardwareMap, telemetry);
this.arm = new ArmSubsystem(hardwareMap, telemetry);
this.wrist = new WristSubsystem(hardwareMap, telemetry);
this.lift = new LiftSubsystem(hardwareMap, telemetry);
this.GP1 = gp1;
this.GP2 = gp2;
this.currentGP1 = new Gamepad();
this.currentGP2 = new Gamepad();
this.previousGP1 = new Gamepad();
this.previousGP2 = new Gamepad();
this.fieldStates = new FieldStates();
}
public void init() {
this.motors.init();
this.claw.init();
this.arm.init();
this.wrist.init();
this.lift.init();
}
public void update() {
this.previousGP1.copy(currentGP1);
this.currentGP1.copy(this.GP1);
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.theDrop();
this.theLift();
this.thePickup();
this.theTravel();
this.theLowBucketScore();
this.theHighBucketScore();
}
/*
Controller: 1
Button: A
Action: On button press, Arm hovers the floor with wrist parallel to arm
*/
public void theDrop() {
if (this.currentGP1.a && !this.previousGP1.a) {
if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) {
Actions.runBlocking(new SequentialAction(
this.arm.toFloorPosition,
this.wrist.toFloorPosition
));
}
}
}
/*
Controller: 1
Button: Right Bumper
Action: On button press, open and closes claw
*/
public void thePickup() {
if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
this.claw.switchState();
}
}
/*
Controller: 1
Button: B
Action: On button press, lift arm and prepare wrist for bucket
*/
public void theLift() {
if (this.currentGP1.b && !this.previousGP1.b) {
Actions.runBlocking(new SequentialAction(
this.arm.toParkPosition,
this.wrist.toBucketPosition
));
}
}
/*
Controller: 2
Button: Y
Action: On button press, lift to low bucket height,
arm to bucket position, wrist to bucket position
*/
public void theLowBucketScore() {
if (this.currentGP2.y && !this.previousGP2.y) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
lift.toLowBucket,
arm.toBucketPosition,
wrist.toBucketPosition
));
}
}
/*
Controller: 2
Button: A
Action: On button press, lift to low bucket height,
arm to bucket position, wrist to bucket position
*/
public void theHighBucketScore() {
if (this.currentGP2.a && !this.previousGP2.a) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
lift.toHighBucket,
arm.toBucketPosition,
wrist.toBucketPosition
));
}
}
/*
Controller: 2
Button: Direction Pad DOWN
Action: On directional press, lift to floor height,
arm to bucket position, wrist to floor position
*/
public void theTravel(){
if (this.currentGP2.dpad_down && !this.previousGP2.dpad_down){
fieldStates.setFieldLocation(FieldStates.FieldLocation.FIELD);
Actions.runBlocking(new SequentialAction(
lift.toFloor,
arm.toBucketPosition,
wrist.toFloorPosition
));
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java
View File

@ -36,7 +36,7 @@ public class Teleop {
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
claw = new ClawSubsystem(hardwareMap, telemetry);
initMotors(hardwareMap);
this.follower = follower;

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java Normal file
View File

@ -0,0 +1,18 @@
package org.firstinspires.ftc.teamcode.states;
public class FieldStates {
public enum FieldLocation {
BUCKET, SUBMARINE, FIELD
}
private FieldLocation fieldLocation;
public FieldLocation getFieldLocation() {
return fieldLocation;
}
public void setFieldLocation(FieldLocation fieldLocation) {
this.fieldLocation = fieldLocation;
}
}

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
View File

@ -1,68 +1,66 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class ArmSubsystem {
public enum ArmState {
PARK, ENGAGE, BUCKET
PARK, FLOOR, BUCKET
}
public ServoImplEx arm;
public ArmState state;
public RunAction engageArm, parkArm, bucketArm;
private ServoImplEx arm;
private ArmState state;
public RunAction toFloorPosition, toParkPosition, toBucketPosition;
private Telemetry telemetry;
public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
arm.resetDeviceConfigurationForOpMode();
this.state = armState;
parkArm = new RunAction(this::parkArm);
engageArm = new RunAction(this::engageArm);
bucketArm = new RunAction(this::bucketArm);
public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
this.toParkPosition = new RunAction(this::toParkPosition);
this.toFloorPosition = new RunAction(this::toFloorPosition);
this.toBucketPosition = new RunAction(this::toBucketPosition);
this.telemetry = telemetry;
}
public void setState(ArmState armState) {
if (armState == ArmState.ENGAGE) {
arm.setPosition(armEngage);
this.state = ArmState.ENGAGE;
if (armState == ArmState.FLOOR) {
this.arm.setPosition(armFloor);
} else if (armState == ArmState.PARK) {
arm.setPosition(armPark);
this.state = ArmState.PARK;
this.arm.setPosition(armPark);
} else if (armState == ArmState.BUCKET) {
arm.setPosition(armBucket);
this.state = ArmState.BUCKET;
this.arm.setPosition(armBucket);
}
this.state = armState;
this.telemetry.addData("Arm State", this.getState());
}
public void engageArm() {
setState(ArmState.ENGAGE);
public void toFloorPosition() {
this.setState(ArmState.FLOOR);
}
public void parkArm() {
setState(ArmState.PARK);
public void toParkPosition() {
this.setState(ArmState.PARK);
}
public void bucketArm() {
setState(ArmState.BUCKET);
public void toBucketPosition() {
this.setState(ArmState.BUCKET);
}
public void switchState() {
if (state == ArmState.ENGAGE) {
setState(ArmState.PARK);
} else if (state == ArmState.PARK) {
setState(ArmState.BUCKET);
} else if (state == ArmState.BUCKET) {
setState(ArmState.ENGAGE);
if (this.state == ArmState.FLOOR) {
this.setState(ArmState.PARK);
} else if (this.state == ArmState.PARK) {
this.setState(ArmState.BUCKET);
} else if (this.state == ArmState.BUCKET) {
this.setState(ArmState.FLOOR);
}
}
@ -71,11 +69,12 @@ public class ArmSubsystem {
}
public void init() {
Actions.runBlocking(parkArm);
this.arm.resetDeviceConfigurationForOpMode();
Actions.runBlocking(this.toParkPosition);
}
public void start() {
Actions.runBlocking(parkArm);
Actions.runBlocking(this.toParkPosition);
}
public double getPosition() {

22
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java
View File

@ -1,12 +1,14 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
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 {
@ -17,23 +19,23 @@ public class ClawSubsystem {
private Servo claw;
private ClawState state;
public RunAction openClaw, closeClaw;
public Telemetry telemetry;
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 ClawSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.claw = hardwareMap.get(Servo.class, "claw-servo");
this.openClaw = new RunAction(this::openClaw);
this.closeClaw = new RunAction(this::closeClaw);
this.telemetry = telemetry;
}
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;
}
this.state = clawState;
this.telemetry.addData("Claw State", this.getState());
}
public ClawState getState() {

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

@ -10,29 +10,35 @@ 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.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class LiftSubsystem {
public DcMotor lift;
public RunAction toFloor, toLowBucket, toHighBucket;
public RunAction toFloor, toLowBucket, toHighBucket, toFloat;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT
}
private LiftState liftState;
private Telemetry telemetry;
public LiftSubsystem(HardwareMap hardwareMap) {
public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
lift = hardwareMap.get(DcMotor.class, "lift-motor");
toFloor = new RunAction(this::toFloor);
toLowBucket = new RunAction(this::toLowBucket);
toHighBucket = new RunAction(this::toHighBucket);
toFloat = new RunAction(this::toFloat);
this.telemetry = telemetry;
}
public void setTarget(int b) {
lift.setTargetPosition(b);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.telemetry.addData("Lift State", this.getState());
}
public void switchState() {
@ -77,12 +83,14 @@ public class LiftSubsystem {
this.liftState = liftState;
}
private LiftState getState() { return this.liftState; }
public int getPosition() {
return lift.getCurrentPosition();
}
public void start() {
this.toFloor();
Actions.runBlocking(toFloor);
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java
View File

@ -26,7 +26,7 @@ public class MotorsSubsystem {
public DcMotor backRightMotor;
public enum TravelState {
PARKED, BUCKET, SUBMARINE
STOPPED, MOVING
}
public TravelState travelState;
@ -60,6 +60,8 @@ public class MotorsSubsystem {
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.setState(TravelState.STOPPED);
}
public void setFrontLeftMotorPower(double power) {
@ -115,8 +117,8 @@ public class MotorsSubsystem {
// Show the elapsed game time and wheel power.
this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
this.telemetry.update();
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
this.telemetry.addData("Current State", this.getState());
}
public void setState(TravelState travelState) {
@ -127,4 +129,12 @@ public class MotorsSubsystem {
return this.travelState;
}
public void setPower(DcMotor motor, double power) {
motor.setPower(power);
if (power < 0.05) {
this.setState(TravelState.MOVING);
} else {
this.setState(TravelState.STOPPED);
}
}
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
View File

@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
@ -17,33 +18,31 @@ public class WristSubsystem {
public ServoImplEx wrist;
public WristState state;
public RunAction floorWrist, bucketWrist;
public RunAction toFloorPosition, toBucketPosition;
public Telemetry telemetry;
public WristSubsystem(HardwareMap hardwareMap, WristState wristState) {
wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
wrist.resetDeviceConfigurationForOpMode();
this.state = wristState;
bucketWrist = new RunAction(this::bucketWrist);
floorWrist = new RunAction(this::floorWrist);
public WristSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
this.telemetry = telemetry;
toBucketPosition = new RunAction(this::toBucketPosition);
toFloorPosition = new RunAction(this::toFloorPosition);
}
public void setState(WristState wristState) {
if (wristState == WristState.FLOOR) {
wrist.setPosition(wristFloor);
this.state = WristState.FLOOR;
} else if (wristState == WristState.BUCKET) {
wrist.setPosition(wristBucket);
this.state = WristState.BUCKET;
}
this.state = wristState;
this.telemetry.addData("Wrist State", this.getState());
}
public void floorWrist() {
public void toFloorPosition() {
setState(WristState.FLOOR);
}
public void bucketWrist() {
public void toBucketPosition() {
setState(WristState.BUCKET);
}
@ -60,11 +59,12 @@ public class WristSubsystem {
}
public void init() {
Actions.runBlocking(floorWrist);
wrist.resetDeviceConfigurationForOpMode();
Actions.runBlocking(toFloorPosition);
}
public void start() {
Actions.runBlocking(floorWrist);
Actions.runBlocking(toFloorPosition);
}
public double getPosition() {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/action/Actions.java
View File

@ -15,9 +15,7 @@ public class Actions {
while (b && !Thread.currentThread().isInterrupted()) {
TelemetryPacket p = new TelemetryPacket();
p.fieldOverlay().getOperations().addAll(c.getOperations());
b = a.run(p);
dash.sendTelemetryPacket(p);
}
}