Arm Motor and Collector Wrist

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java

@@ -3,6 +3,7 @@ package org.timecrafters.testing.states;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
 
@@ -14,6 +15,8 @@ public class PrototypeBot1 {
 
         public CRServo collectorLeft, collectorRight;
 
+        public Servo collectorWrist;
+
         public PrototypeBot1(CyberarmEngine engine) {
             this.engine = engine;
 
@@ -32,6 +35,7 @@ public class PrototypeBot1 {
             // servo configuration
             collectorLeft = engine.hardwareMap.crservo.get("collector left");
             collectorRight = engine.hardwareMap.crservo.get("collector right");
+            collectorWrist = engine.hardwareMap.servo.get("collector wrist");
 
             //motors direction and encoders
             frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);

TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java

@@ -4,6 +4,8 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
 
+import java.lang.annotation.Target;
+
 public class PrototypeTeleOPState extends CyberarmState {
 
     private final PrototypeBot1 robot;
@@ -13,6 +15,7 @@ public class PrototypeTeleOPState extends CyberarmState {
     private boolean bprev;
     private double drivePower = 1;
     private boolean UpDPad;
+    private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400;
 
     public PrototypeTeleOPState(PrototypeBot1 robot) {
         this.robot = robot;
@@ -20,16 +23,18 @@ public class PrototypeTeleOPState extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("Arm Position", robot.armMotor.getCurrentPosition());
+        engine.telemetry.addData("Arm Power", robot.armMotor.getPower());
+        engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition());
+        engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition());
     }
 
     @Override
     public void init() {
         robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        robot.armMotor.setTargetPosition(600);
+        robot.armMotor.setTargetPosition(armTargetPosition);
         robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
+        robot.collectorWrist.setPosition(1);
 
     }
 
@@ -72,7 +77,7 @@ public class PrototypeTeleOPState extends CyberarmState {
         double frontRightPower = (y - x - rx) / denominator;
         double backRightPower = (y + x - rx) / denominator;
 
-        // As I programed this and ran it, I realized everything was backwards
+        // As I programmed this and ran it, I realized everything was backwards
         // in direction so to fix that i either went in the robot object state and reversed
         // directions on drive motors or put a negative in behind the joystick power to reverse it.
         // I put negatives in to reverse it because it was the easiest at the moment.
@@ -81,36 +86,83 @@ public class PrototypeTeleOPState extends CyberarmState {
         robot.backLeftDrive.setPower(backLeftPower);
         robot.frontRightDrive.setPower(-frontRightPower);
         robot.backRightDrive.setPower(backRightPower);
-        robot.armMotor.setPower(engine.gamepad1.left_stick_y * 0.5);
 
-        if (engine.gamepad1.y) {
+
+
+        robot.armMotor.setPower(engine.gamepad2.left_stick_y * 0.5);
+
+
+        if (engine.gamepad2.y) {
 
             robot.collectorLeft.setPower(0);
             robot.collectorRight.setPower(0);
 
         }
 
-        if (engine.gamepad1.b) {
+        if (engine.gamepad2.b) {
 
             robot.collectorLeft.setPower(1);
             robot.collectorRight.setPower(-1);
 
         }
 
-        if (engine.gamepad1.x) {
+        if (engine.gamepad2.x) {
 
             robot.collectorLeft.setPower(-1);
             robot.collectorRight.setPower(1);
 
         }
 
-        if (engine.gamepad1.dpad_up) {
-
-            robot.armMotor.setPower(1);
-        } else {
-            robot.armMotor.setPower(0);
+        if (engine.gamepad2.dpad_left) {
 
+            robot.collectorWrist.setPosition(0.2);
 
         }
+
+        if (engine.gamepad2.dpad_right) {
+
+            robot.collectorWrist.setPosition(1);
+
+        }
+
+        if (engine.gamepad2.dpad_up) {
+
+            robot.armMotor.setPower(1);
+        } else if (engine.gamepad2.dpad_down) {
+            robot.armMotor.setPower(0);
+
+        }
+//        int armTargetPosition = robot.armMotor.getCurrentPosition();
+
+        if (engine.gamepad2.left_trigger > 0.1) {
+            armTargetPosition += 1;
+            if (armTargetPosition > 600) {
+                armTargetPosition = 600;
+            }
+        }
+
+        if (engine.gamepad2.right_trigger > 0.1) {
+            armTargetPosition -= 1;
+            if (armTargetPosition <= 0) {
+                armTargetPosition = 0;
+
+            }
+
+        }
+
+        if (engine.gamepad2.left_bumper) {
+
+            armTargetPosition = armDeliverPosition;
+
+        }
+
+        if (engine.gamepad2.right_bumper) {
+
+            armTargetPosition = armCollectPosition;
+
+        }
+
+        robot.armMotor.setTargetPosition(armTargetPosition);
+
     }
 }