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

Implemented Arm Subsytem wholeheartedly

Browse Source
This commit is contained in:
Carlos Rivas
2024-10-30 20:36:38 -07:00
parent 284263a43b
commit b5c6e03ef3
3 changed files with 42 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,6 +53,7 @@ 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.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
/* /*
@ -95,9 +96,7 @@ public class ArmTest extends LinearOpMode {
/* /*
* Instantiate Arm * Instantiate Arm
*/ */
ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
arm.resetDeviceConfigurationForOpMode();
arm.setPosition(.15);
/* /*
* Instantiate gamepad state holders * Instantiate gamepad state holders
@ -105,6 +104,7 @@ public class ArmTest extends LinearOpMode {
Gamepad currentGamepad1 = new Gamepad(); Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad(); Gamepad previousGamepad1 = new Gamepad();
arm.init();
waitForStart(); waitForStart();
runtime.reset(); runtime.reset();
@ -115,34 +115,20 @@ public class ArmTest extends LinearOpMode {
currentGamepad1.copy(gamepad1); currentGamepad1.copy(gamepad1);
if (currentGamepad1.circle && !previousGamepad1.circle) { if (currentGamepad1.circle && !previousGamepad1.circle) {
arm.setPosition(0.3); arm.parkArm();
} }
if (currentGamepad1.square && !previousGamepad1.square) { if (currentGamepad1.square && !previousGamepad1.square) {
arm.setPosition(.45); arm.engageArm();
} }
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) { if (currentGamepad1.cross && !previousGamepad1.cross) {
double newPosition = arm.getPosition() - .05; arm.switchState();
if (newPosition < 0.15) {
arm.setPosition(0.15);
} else {
arm.setPosition(newPosition);
}
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
double newPosition = arm.getPosition() + .05;
if (newPosition > .6) {
arm.setPosition(.6);
} else {
arm.setPosition(newPosition);
}
} }
// Show the elapsed game time and wheel power. // Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Arm Servo Position", arm.getPosition()); telemetry.addData("Arm State", arm.getState());
telemetry.update(); telemetry.update();
} }
}} }}

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

@ -7,9 +7,8 @@ public class RobotConstants {
public static double clawClose = 0.95; public static double clawClose = 0.95;
public static double clawOpen = 0.5; public static double clawOpen = 0.5;
public static double wristEngage = 0.95; public static double armEngage = 0.5;
public static double wristHold = 0.50; public static double armPark = 0.125;
public static double wristDisengage = 0.05;
public static double intakeSpinInPwr = 1; public static double intakeSpinInPwr = 1;
public static double intakeSpinOutPwr = -0.25; public static double intakeSpinOutPwr = -0.25;

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

@ -1,42 +1,41 @@
package org.firstinspires.ftc.teamcode.subsystem; package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristDisengage; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristEngage; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHold;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.ServoImplEx; import com.qualcomm.robotcore.hardware.ServoImplEx;
import org.firstinspires.ftc.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction; import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class ArmSubsystem { public class ArmSubsystem {
public enum ArmState { public enum ArmState {
DISENGAGE, ENGAGE PARK, ENGAGE
} }
public ServoImplEx wrist; public ServoImplEx arm;
public ArmState state; public ArmState state;
public RunAction engageWrist, disengageWrist; public RunAction engageArm, parkArm;
public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) { public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
wrist = hardwareMap.get(ServoImplEx.class, "arm-servo"); arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
wrist.resetDeviceConfigurationForOpMode(); arm.resetDeviceConfigurationForOpMode();
this.state = armState; this.state = armState;
engageWrist = new RunAction(this::engageArm); parkArm = new RunAction(this::parkArm);
disengageWrist = new RunAction(this::disengageArm); engageArm = new RunAction(this::engageArm);
} }
public void setState(ArmState wristState) { public void setState(ArmState armState) {
if (wristState == ArmState.ENGAGE) { if (armState == ArmState.ENGAGE) {
wrist.setPosition(wristEngage); arm.setPosition(armEngage);
this.state = ArmState.ENGAGE; this.state = ArmState.ENGAGE;
} else if (wristState == ArmState.DISENGAGE) { } else if (armState == ArmState.PARK) {
wrist.setPosition(wristDisengage); arm.setPosition(armPark);
this.state = ArmState.DISENGAGE; this.state = ArmState.PARK;
} }
} }
@ -44,16 +43,28 @@ public class ArmSubsystem {
setState(ArmState.ENGAGE); setState(ArmState.ENGAGE);
} }
public void disengageArm() { public void parkArm() {
setState(ArmState.DISENGAGE); setState(ArmState.PARK);
} }
public void switchState() { public void switchState() {
if (state == ArmState.ENGAGE) { if (state == ArmState.ENGAGE) {
setState(ArmState.DISENGAGE); setState(ArmState.PARK);
} else if (state == ArmState.DISENGAGE) { } else if (state == ArmState.PARK) {
setState(ArmState.ENGAGE); setState(ArmState.ENGAGE);
} }
} }
public ArmState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(parkArm);
}
public void start() {
Actions.runBlocking(parkArm);
}
} }