Added new Wrist subsystem and example test file

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java

@@ -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.WristSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 
 /*
  * This file contains an example of a Linear "OpMode".
@@ -96,10 +76,7 @@ public class WristTest extends LinearOpMode {
         /*
          * Instantiate Wrist
          */
-        ServoImplEx wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
+        WristSubsystem wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
-//        wrist.resetDeviceConfigurationForOpMode();
-        wrist.setDirection(Servo.Direction.FORWARD);
-        wrist.setPosition(0);
 
         /*
          * 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;
+                wrist.setPosition(wrist.getPosition() - .05);
-                if (newPosition < 0.1) {
-                    wrist.setPosition(0.1);
-                } else {
-                    wrist.setPosition(newPosition);
-                }
             }
 
             if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-                double newPosition = wrist.getPosition() + .05;
+                wrist.setPosition(wrist.getPosition() + .05);
-                if (newPosition > .9) {
-                    wrist.setPosition(.9);
-                } else {
-                    wrist.setPosition(newPosition);
-                }
             }
 
+
             // 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();
         }
-    }}
+    }
+}

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

@@ -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);
+    }
+
+
+}