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

Added new Wrist subsystem and example test file

Browse Source
This commit is contained in:
Carlos Rivas
2024-10-30 21:31:52 -07:00
parent 8e99d1672e
commit 600e63a52b
2 changed files with 96 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java
View File

@ -29,32 +29,12 @@
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 com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
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 com.qualcomm.robotcore.hardware.Servo;
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.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
/*
* This file contains an example of a Linear "OpMode".
@ -84,7 +64,7 @@ import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*/
@TeleOp(name="Wrist Test", group="Debug")
@TeleOp(name = "Wrist Test", group = "Debug")
public class WristTest extends LinearOpMode {
// Declare OpMode members for each of the 4 motors.
@ -96,10 +76,7 @@ public class WristTest extends LinearOpMode {
/*
* Instantiate Wrist
*/
ServoImplEx wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
// wrist.resetDeviceConfigurationForOpMode();
wrist.setDirection(Servo.Direction.FORWARD);
wrist.setPosition(0);
WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
/*
* Instantiate gamepad state holders
@ -107,6 +84,7 @@ public class WristTest extends LinearOpMode {
Gamepad currentGamepad1 = new Gamepad();
Gamepad previousGamepad1 = new Gamepad();
wrist.init();
waitForStart();
runtime.reset();
@ -117,34 +95,31 @@ public class WristTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
wrist.setPosition(0.25);
wrist.bucketWrist();
}
if (currentGamepad1.circle && !previousGamepad1.circle) {
wrist.setPosition(0.75);
wrist.floorWrist();
}
if (currentGamepad1.cross && !previousGamepad1.cross) {
wrist.switchState();
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
double newPosition = wrist.getPosition() - .05;
if (newPosition < 0.1) {
wrist.setPosition(0.1);
} else {
wrist.setPosition(newPosition);
}
wrist.setPosition(wrist.getPosition() - .05);
}
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
double newPosition = wrist.getPosition() + .05;
if (newPosition > .9) {
wrist.setPosition(.9);
} else {
wrist.setPosition(newPosition);
}
wrist.setPosition(wrist.getPosition() + .05);
}
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Wrist Servo Position", wrist.getPosition());
telemetry.addData("Wrist State", wrist.getState());
telemetry.addData("Wrist Position", wrist.getPosition());
telemetry.update();
}
}}
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java Normal file
View File

@ -0,0 +1,79 @@
package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
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.teamcode.util.action.Actions;
import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class WristSubsystem {
public enum WristState {
FLOOR, BUCKET
}
public ServoImplEx wrist;
public WristState state;
public RunAction floorWrist, bucketWrist;
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 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;
}
}
public void floorWrist() {
setState(WristState.FLOOR);
}
public void bucketWrist() {
setState(WristState.BUCKET);
}
public void switchState() {
if (state == WristState.FLOOR) {
setState(WristState.BUCKET);
} else if (state == WristState.BUCKET) {
setState(WristState.FLOOR);
}
}
public WristState getState() {
return this.state;
}
public void init() {
Actions.runBlocking(floorWrist);
}
public void start() {
Actions.runBlocking(floorWrist);
}
public double getPosition() {
return this.wrist.getPosition();
}
public void setPosition(double position) {
this.wrist.setPosition(position);
}
}