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

Reconcile telemetry data and moved all actions to gamepad 1

Browse Source
This commit is contained in:
Carlos Rivas
2024-11-09 20:06:44 -08:00
parent 90bcfbb787
commit 399a21c547
6 changed files with 45 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public static double clawClose = 1.00;
public static double clawOpen = 0.25;
public static double clawOpen = 0.0625;
public static double armFloor = 0.5;
public static double armPark = 0.125;
public static double armBucket = 0.175;
public static double armFloor = 0.375;
public static double armPark = 0.0;
public static double armBucket = 0.15;
public static double wristFloor = 0.625;
public static double wristBucket = 0.215;
@ -17,5 +17,5 @@ public class RobotConstants {
public static int liftToFloatPos = 150;
public static int liftToLowBucketPos = 2250;
public static int liftToHighBucketPos = 3850;
public static double liftPower = .45;
public static double liftPower = .375;
}

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java
View File

@ -12,6 +12,7 @@ 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;
import org.firstinspires.ftc.teamcode.util.action.SleepAction;
public class DevTeleopRunMode {
@ -33,16 +34,18 @@ public class DevTeleopRunMode {
public Gamepad previousGP1;
public Gamepad currentGP2;
public Gamepad previousGP2;
private Telemetry telemetry;
public FieldStates fieldStates;
public DevTeleopRunMode(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry);
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
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.telemetry = telemetry;
this.currentGP1 = new Gamepad();
this.currentGP2 = new Gamepad();
this.previousGP1 = new Gamepad();
@ -64,11 +67,19 @@ public class DevTeleopRunMode {
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.theDrop();
this.theLift();
this.thePickup();
this.theTravel();
this.theLowBucketScore();
this.theHighBucketScore();
this.toHold();
this.motors.calculateTrajectory(this.GP1);
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
this.telemetry.addData("Claw State", this.claw.getState());
this.telemetry.addData("Claw Position", this.claw.getPosition());
this.telemetry.addData("Wrist State", this.wrist.getState());
this.telemetry.addData("Arm State", this.arm.getState());
this.telemetry.addData("Lift State", this.lift.getState());
this.telemetry.addData("Lift Position", this.lift.getPosition());
}
/*
@ -77,11 +88,13 @@ public class DevTeleopRunMode {
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) {
if (this.currentGP1.dpad_down && !this.previousGP1.dpad_down) {
if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD &&
this.lift.getPosition() < 40) {
Actions.runBlocking(new SequentialAction(
this.arm.toFloorPosition,
this.wrist.toFloorPosition
this.wrist.toFloorPosition,
new SleepAction(.75),
this.arm.toFloorPosition
));
}
}
@ -99,16 +112,16 @@ public class DevTeleopRunMode {
}
/*
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) {
Controller: 1
Button: Right Bumper
Action: On button press, open and closes claw
*/
public void toHold() {
if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) {
Actions.runBlocking(new SequentialAction(
this.arm.toParkPosition,
this.wrist.toBucketPosition
arm.toParkPosition,
wrist.toFloorPosition
));
}
}
@ -121,7 +134,7 @@ public class DevTeleopRunMode {
arm to bucket position, wrist to bucket position
*/
public void theLowBucketScore() {
if (this.currentGP2.y && !this.previousGP2.y) {
if (this.currentGP1.a && !this.previousGP1.a) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
lift.toLowBucket,
@ -138,7 +151,7 @@ public class DevTeleopRunMode {
arm to bucket position, wrist to bucket position
*/
public void theHighBucketScore() {
if (this.currentGP2.a && !this.previousGP2.a) {
if (this.currentGP1.b && !this.previousGP1.b) {
fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
Actions.runBlocking(new SequentialAction(
lift.toHighBucket,
@ -155,7 +168,7 @@ public class DevTeleopRunMode {
arm to bucket position, wrist to floor position
*/
public void theTravel(){
if (this.currentGP2.dpad_down && !this.previousGP2.dpad_down){
if (this.currentGP1.dpad_right && !this.previousGP1.dpad_right){
if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.BUCKET) {
Actions.runBlocking(new SequentialAction(
lift.toFloor,

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

@ -39,7 +39,6 @@ public class ArmSubsystem {
this.arm.setPosition(armBucket);
}
this.state = armState;
this.telemetry.addData("Arm State", this.getState());
}
public void toFloorPosition() {

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

@ -35,7 +35,6 @@ public class ClawSubsystem {
claw.setPosition(clawOpen);
}
this.state = clawState;
this.telemetry.addData("Claw State", this.getState());
}
public ClawState getState() {
@ -66,4 +65,8 @@ public class ClawSubsystem {
Actions.runBlocking(closeClaw);
}
public double getPosition() {
return this.claw.getPosition();
}
}

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

@ -38,12 +38,11 @@ public class LiftSubsystem {
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() {
if (this.liftState == LiftState.FLOOR) {
this.toFloor();
this.toFloat();
} else if (this.liftState == LiftState.FLOAT) {
this.toLowBucket();
} else if (this.liftState == LiftState.LOW_BUCKET) {
@ -54,23 +53,23 @@ public class LiftSubsystem {
}
public void toFloor() {
this.setTarget(liftToFloorPos);
this.setState(LiftState.FLOOR);
this.setTarget(liftToFloorPos);
}
public void toFloat() {
this.setTarget(liftToFloatPos);
this.setState(LiftState.FLOAT);
this.setTarget(liftToFloatPos);
}
public void toLowBucket() {
this.setTarget(liftToLowBucketPos);
this.setState(LiftState.LOW_BUCKET);
this.setTarget(liftToLowBucketPos);
}
public void toHighBucket() {
this.setTarget(liftToHighBucketPos);
this.setState(LiftState.HIGH_BUCKET);
this.setTarget(liftToHighBucketPos);
}
public void init() {
@ -83,7 +82,7 @@ public class LiftSubsystem {
this.liftState = liftState;
}
private LiftState getState() { return this.liftState; }
public LiftState getState() { return this.liftState; }
public int getPosition() {
return lift.getCurrentPosition();

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

@ -35,7 +35,6 @@ public class WristSubsystem {
wrist.setPosition(wristBucket);
}
this.state = wristState;
this.telemetry.addData("Wrist State", this.getState());
}
public void toFloorPosition() {