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 org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
/*
@ -95,9 +96,7 @@ public class ArmTest extends LinearOpMode {
/*
* Instantiate Arm
*/
ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
arm.resetDeviceConfigurationForOpMode();
arm.setPosition(.15);
ArmSubsystem arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
/*
* Instantiate gamepad state holders
@ -105,6 +104,7 @@ public class ArmTest extends LinearOpMode {
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
arm.init();
waitForStart();
runtime.reset();
@ -115,34 +115,20 @@ public class ArmTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.circle && !previousGamepad1.circle) {
arm.setPosition(0.3);
arm.parkArm();
}
if (currentGamepad1.square && !previousGamepad1.square) {
arm.setPosition(.45);
arm.engageArm();
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
double newPosition = arm.getPosition() - .05;
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);
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
arm.switchState();
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Arm Servo Position", arm.getPosition());
telemetry.addData("Arm State", arm.getState());
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 clawOpen = 0.5;
public static double wristEngage = 0.95;
public static double wristHold = 0.50;
public static double wristDisengage = 0.05;
public static double armEngage = 0.5;
public static double armPark = 0.125;
public static double intakeSpinInPwr = 1;
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;
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 static org.firstinspires.ftc.teamcode.configs.RobotConstants.armEngage;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
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.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class ArmSubsystem {
public enum ArmState {
DISENGAGE, ENGAGE
PARK, ENGAGE
}
public ServoImplEx wrist;
public ServoImplEx arm;
public ArmState state;
public RunAction engageWrist, disengageWrist;
public RunAction engageArm, parkArm;
public ArmSubsystem(HardwareMap hardwareMap, ArmState armState) {
wrist = hardwareMap.get(ServoImplEx.class, "arm-servo");
wrist.resetDeviceConfigurationForOpMode();
arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
arm.resetDeviceConfigurationForOpMode();
this.state = armState;
engageWrist = new RunAction(this::engageArm);
disengageWrist = new RunAction(this::disengageArm);
parkArm = new RunAction(this::parkArm);
engageArm = new RunAction(this::engageArm);
}
public void setState(ArmState wristState) {
if (wristState == ArmState.ENGAGE) {
wrist.setPosition(wristEngage);
public void setState(ArmState armState) {
if (armState == ArmState.ENGAGE) {
arm.setPosition(armEngage);
this.state = ArmState.ENGAGE;
} else if (wristState == ArmState.DISENGAGE) {
wrist.setPosition(wristDisengage);
this.state = ArmState.DISENGAGE;
} else if (armState == ArmState.PARK) {
arm.setPosition(armPark);
this.state = ArmState.PARK;
}
}
@ -44,16 +43,28 @@ public class ArmSubsystem {
setState(ArmState.ENGAGE);
}
public void disengageArm() {
setState(ArmState.DISENGAGE);
public void parkArm() {
setState(ArmState.PARK);
}
public void switchState() {
if (state == ArmState.ENGAGE) {
setState(ArmState.DISENGAGE);
} else if (state == ArmState.DISENGAGE) {
setState(ArmState.PARK);
} else if (state == ArmState.PARK) {
setState(ArmState.ENGAGE);
}
}
public ArmState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(parkArm);
}
public void start() {
Actions.runBlocking(parkArm);
}
}