Arm

Added arm class
edinabucketbrigade · Oct 30, 2024 · 49ec963 · 49ec963
1 parent cb4546e
commit 49ec963
Show file tree

Hide file tree

Showing 3 changed files with 73 additions and 1 deletion.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotHardware.java
@@ -15,7 +15,7 @@ public class RobotHardware {
 
     public DcMotorEx liftMotor = null;  //5203 series, 384.5 ppr - encoder resolution
     //5204-08139 series, 3895.9 resolution, for other arm thing motor
-
+    public DcMotorEx armMotor = null;
     // Define a constructor that allows the OpMode to pass a reference to itself.
     public RobotHardware(LinearOpMode opMode) {
         myOpMode = opMode;
@@ -33,6 +33,10 @@ public void init() {
         liftMotor.setDirection(DcMotor.Direction.FORWARD);
         liftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
+        armMotor = myOpMode.hardwareMap.get(DcMotorEx.class, "armMotor");
+        armMotor.setDirection(DcMotor.Direction.FORWARD);
+        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
         leftFrontDrive.setDirection(DcMotor.Direction.FORWARD);
         leftBackDrive.setDirection(DcMotor.Direction.FORWARD);
         rightFrontDrive.setDirection(DcMotor.Direction.REVERSE);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/enums/Armposition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/enums/Armposition.java
@@ -0,0 +1,6 @@
+package org.firstinspires.ftc.teamcode.enums;
+
+public enum Armposition {
+    Front,
+    Back
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Arm.java
@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.RobotHardware;
+import org.firstinspires.ftc.teamcode.enums.Armposition;
+import org.firstinspires.ftc.teamcode.enums.LiftPosition;
+
+import java.security.PublicKey;
+
+public class Arm extends SubSystem {
+    public Armposition armState;
+    private RobotHardware robot;
+    public boolean rightBumperPressed = false;
+    public boolean leftBumperPressed = false;
+    private final int ARM_FRONT = 0;
+    private final int ARM_BACK = 384;
+
+    private final double ARM_MAX_POWER = .7;
+    private final int ARM_POSITION_TOLERANCE = 10;
+
+    public Arm(RobotHardware robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        armState = Armposition.Back;
+        robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    @Override
+    public void start() {
+
+    }
+
+    @Override
+    public void update() {
+        switch (armState) {
+            case Back:
+                if (Math.abs(robot.armMotor.getCurrentPosition() - ARM_BACK) < ARM_POSITION_TOLERANCE) {
+                    if (leftBumperPressed) {
+                        robot.armMotor.setTargetPosition(ARM_BACK);
+                        robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                        robot.armMotor.setPower(ARM_MAX_POWER);
+                        armState = Armposition.Front;
+                    }
+                }
+
+            case Front:
+                if (Math.abs(robot.armMotor.getCurrentPosition() - ARM_FRONT) < ARM_POSITION_TOLERANCE) {
+                    if (rightBumperPressed) {
+                        robot.armMotor.setTargetPosition(ARM_FRONT);
+                        robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                        robot.armMotor.setPower(ARM_MAX_POWER);
+                        armState = Armposition.Back;
+                    }
+                }
+        }
+    }
+}