Camera work has been done using open CV and i was succesfully able to break the camera into looking through 3 sections but still need to figure out how to determine color

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java

@@ -0,0 +1,56 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "Competition Blue Close")
+
+public class CompetitionBlueClose extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.imu.resetYaw();
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("Competition Blue Close");
+        this.robot.setup();
+        addState(new ClawFingerState(robot,"Competition Blue Close", "01-0-00"));
+        // drive to and face towards the right scenario
+        addState(new DriveToCoordinatesState(robot,"Competition Blue Close", "02-0-01"));
+        // check if its in the correct Position;
+        addState(new ClawArmState(robot,"Competition Blue Close", "03-0-02"));
+        addState(new ClawFingerState(robot,"Competition Blue Close", "04-0-04"));
+        addState(new DriveToCoordinatesState(robot,"Competition Blue Close", "05-0-01"));
+        addState(new ClawArmState(robot,"Competition Blue Close", "06-0-06"));
+
+
+
+
+
+
+
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java

@@ -1,8 +1,10 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
 
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState;
 import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
 import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
@@ -15,16 +17,40 @@ public class CompetitionRedClose extends CyberarmEngine {
 
     CompetitionRobotV1 robot;
 
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.imu.resetYaw();
+    }
 
     @Override
     public void setup() {
         this.robot = new CompetitionRobotV1("Competition Red Close");
         this.robot.setup();
+        this.robot.setup();
+        addState(new ClawFingerState(robot,"Competition Red Close", "01-0-00"));
+        // drive to and face towards the right scenario
+        addState(new DriveToCoordinatesState(robot,"Competition Red Close", "02-0-01"));
+        // check if its in the correct Position;
+        addState(new ClawArmState(robot,"Competition Red Close", "03-0-02"));
+        addState(new ClawFingerState(robot,"Competition Red Close", "04-0-04"));
+        addState(new DriveToCoordinatesState(robot,"Competition Red Close", "05-0-01"));
+        addState(new ClawArmState(robot,"Competition Red Close", "06-0-06"));
+
+
+
+
+
+
+
+
+
+
+
 
-        addState(new DriveToCoordinatesState(robot,"Competition Red Close", "00-1"));
-        addState(new DistanceCheckState(robot,"Competition Red Close", "00-2"));
 
-        addState(new ClawArmState(robot,"Competition Red Close", "01-0"));
     }
 
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java

@@ -0,0 +1,119 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+import org.openftc.easyopencv.OpenCvPipeline;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+public class CameraVisionState extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+    ExamplePipeline pipeline;
+
+    public CameraVisionState(CompetitionRobotV1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        super.init();
+
+        int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        robot.webcam1 = OpenCvCameraFactory.getInstance().createWebcam(robot.webCamName, cameraMonitorViewId);
+        robot.webcam1.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            public void onOpened() {
+                robot.webcam1.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT);
+            }
+
+            public void onError(int errorCode) {
+            }
+        });
+
+        pipeline = new ExamplePipeline();
+        robot.webcam1.setPipeline(pipeline);
+
+    }
+
+    @Override
+    public void exec() {
+        // odometry driving ALWAYS
+//        robot.DriveToCoordinates();
+//        robot.OdometryLocalizer();
+
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("object pos", pipeline.objectPos());
+        engine.telemetry.addData("left side pixel amount", pipeline.leftavgfin);
+        engine.telemetry.addData("middle side pixel amount", pipeline.middleavgfin);
+        engine.telemetry.addData("right side pixel amount", pipeline.rightavgfin);
+        engine.telemetry.addData("object pos", pipeline.objectPos());
+    }
+
+    class ExamplePipeline extends OpenCvPipeline {
+        Mat YCbCr = new Mat();
+        Mat leftCrop;
+        Mat middleCrop;
+        Mat rightCrop;
+        double leftavgfin;
+        double middleavgfin;
+        double rightavgfin;
+        Mat output = new Mat();
+        Scalar rectColor = new Scalar(255.0, 0.0, 0.0);
+
+        public Mat processFrame(Mat input) {
+            Imgproc.cvtColor(input, YCbCr, Imgproc.COLOR_RGB2YCrCb);
+
+            Rect leftRect = new Rect(1, 1, 212, 359);
+            Rect rightRect = new Rect(213, 1, 212, 359);
+            Rect middleRect = new Rect(426, 1, 212, 359);
+
+            input.copyTo(output);
+            Imgproc.rectangle(output, leftRect, rectColor, 2);
+            Imgproc.rectangle(output, rightRect, rectColor, 2);
+            Imgproc.rectangle(output, middleRect, rectColor, 2);
+
+            leftCrop = YCbCr.submat(leftRect);
+            rightCrop = YCbCr.submat(rightRect);
+            middleCrop = YCbCr.submat(middleRect);
+
+            Core.extractChannel(leftCrop, leftCrop, 2);
+            Core.extractChannel(middleCrop, middleCrop, 2);
+            Core.extractChannel(rightCrop, rightCrop, 2);
+
+            Scalar leftavg = Core.mean(leftCrop);
+            Scalar middleavg = Core.mean(middleCrop);
+            Scalar rightavg = Core.mean(rightCrop);
+
+            leftavgfin = leftavg.val[0];
+            rightavgfin = rightavg.val[0];
+            middleavgfin = middleavg.val[0];
+
+            return (output);
+
+        }
+
+        public String objectPos() {
+            if (leftavgfin > rightavgfin && leftavgfin > middleavgfin) {
+                return "LEFT";
+            }
+            else if (rightavgfin > leftavgfin && rightavgfin > middleavgfin) {
+                return "RIGHT";
+            }
+            else {
+                return "MIDDLE";
+            }
+        }
+    }
+}
+

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java

@@ -61,8 +61,8 @@ public class ClawArmState extends CyberarmState {
         robot.armPos = armPos;
         robot.clawArmControl();
 
-        if (robot.clawArm.getCurrentPosition() >= robot.target - 20 && robot.clawArm.getCurrentPosition() <= robot.target + 20) {
-//            setHasFinished(true);
+        if (robot.power < 0.1) {
+            setHasFinished(true);
         }
 
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawFingerState.java

@@ -0,0 +1,84 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+@Config
+public class ClawFingerState extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+    public boolean leftOpen;
+    public boolean rightOpen;
+    public long waitTime;
+    public boolean armDrive;
+    public long initTime;
+
+
+
+    public ClawFingerState(CompetitionRobotV1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.leftOpen = robot.configuration.variable(groupName, actionName, "leftOpen").value();
+        this.rightOpen = robot.configuration.variable(groupName, actionName, "rightOpen").value();
+        this.waitTime = robot.configuration.variable(groupName, actionName, "waitTime").value();
+        this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
+
+
+    }
+
+    @Override
+    public void start() {
+        initTime = System.currentTimeMillis();
+    }
+
+    @Override
+    public void exec() {
+        // odometry driving ALWAYS
+        robot.DriveToCoordinates();
+        robot.OdometryLocalizer();
+        if (armDrive) {
+            robot.clawArmControl();
+        }
+
+        if (!leftOpen){
+            robot.leftClaw.setPosition(0.25);
+        } else {
+            robot.leftClaw.setPosition(0.6);
+        }
+
+        if (!rightOpen){
+            robot.rightClaw.setPosition(0.6);
+        } else {
+            robot.rightClaw.setPosition(0.25);
+        }
+
+        if (runTime() > waitTime){
+            setHasFinished(true);
+        }
+
+
+
+
+    }
+
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("x pos", robot.positionX);
+        engine.telemetry.addData("y pos", robot.positionY);
+        engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH));
+        engine.telemetry.addData("aux encoder", robot.currentAuxPosition);
+        engine.telemetry.addData("left encoder", robot.currentLeftPosition);
+        engine.telemetry.addData("right encoder", robot.currentRightPosition);
+        engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("front right power", robot.frontRightPower);
+        engine.telemetry.addData("front left power", robot.frontLeftPower);
+        engine.telemetry.addData("back right power", robot.backRightPower);
+        engine.telemetry.addData("back left power", robot.backLeftPower);
+        engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition());
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java

@@ -12,11 +12,12 @@ import dev.cyberarm.engine.V2.CyberarmState;
 public class DistanceCheckState extends CyberarmState {
 
     CompetitionRobotV1 robot;
-    public String armPos;
     public long lastCheckedTIme = System.currentTimeMillis();
+    public String objectPosCheck;
 
     public DistanceCheckState(CompetitionRobotV1 robot, String groupName, String actionName) {
         this.robot = robot;
+        this.objectPosCheck = robot.configuration.variable(groupName, actionName, "objectPosCheck").value();
     }
 
     @Override
@@ -25,32 +26,26 @@ public class DistanceCheckState extends CyberarmState {
         robot.DriveToCoordinates();
         robot.OdometryLocalizer();
 
-        if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > -47 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < -43){
-            robot.objectPos = "Right";
-        } else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000){
-            robot.hTarget = 0;
-            }
-        if (robot.objectPos != "right")
-        if (Math.abs(robot.backLeftPower) < 0.15 &&
+        if (robot.armPos.equals("right")  || robot.armPos.equals("middle") || robot.armPos.equals("left")){
+            setHasFinished(true);
+        } else {
+            if (Math.abs(robot.backLeftPower) < 0.15 &&
                 Math.abs(robot.backRightPower) < 0.15 &&
                 Math.abs(robot.frontLeftPower) < 0.15 &&
-                Math.abs(robot.frontRightPower) < 0.15 && System.currentTimeMillis() - lastCheckedTIme > 2000){
-            if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 2000){
-                robot.objectPos = "middle";
-            } else {
-                robot.hTarget = 45;
-                robot.objectPos = "left";
+                Math.abs(robot.frontRightPower) < 0.15) {
+                if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 1000) {
+                    robot.objectPos = objectPosCheck;
+                    setHasFinished(true);
+                } else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000 && System.currentTimeMillis() - lastCheckedTIme > 1000) {
+                    if (robot.loopCheck == 1 && robot.objectPos != objectPosCheck){
+                        robot.objectPos = "left";
+                        setHasFinished(true);
+                    }
+                    robot.loopCheck += 1;
+                    setHasFinished(true);
+                }
             }
         }
-
-        if (robot.objectPos.equals("left") || robot.objectPos.equals("right") || robot.objectPos.equals("middle")){
-            setHasFinished(true);
-        }
-
-
-
-
-
     }
 
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java

@@ -12,11 +12,13 @@ import dev.cyberarm.engine.V2.CyberarmState;
 public class DriveToCoordinatesState extends CyberarmState {
 
     CompetitionRobotV1 robot;
-    public static double xTarget = 0;
-    public static double yTarget = 0;
-    public static double hTarget= 0;
+    public double xTarget = 0;
+    public double yTarget = 0;
+    public double hTarget = 0;
     public boolean posAchieved = false;
     public boolean armDrive;
+    public String objectPos;
+//    public boolean posSpecific;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -24,6 +26,8 @@ public class DriveToCoordinatesState extends CyberarmState {
         this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
         this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value();
         this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
+        this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value();
+//        this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value();
     }
 
     @Override
@@ -33,23 +37,24 @@ public class DriveToCoordinatesState extends CyberarmState {
         robot.xTarget = xTarget;
         robot.DriveToCoordinates();
         robot.OdometryLocalizer();
-
-
-        if (armDrive){
+        if (armDrive) {
             robot.clawArmControl();
         }
 
         if (posAchieved){
             setHasFinished(true);
         } else {
-            if (Math.abs(robot.backLeftPower) < 0.15 &&
-                    Math.abs(robot.backRightPower) < 0.15 &&
-                    Math.abs(robot.frontLeftPower) < 0.15 &&
-                    Math.abs(robot.frontRightPower) < 0.15){
-                posAchieved = true;
+//            if (objectPos.equals(robot.objectPos) || objectPos.equals("everything") ) {
+                if (Math.abs(robot.backLeftPower) < 0.15 &&
+                   Math.abs(robot.backRightPower) < 0.15 &&
+                   Math.abs(robot.frontLeftPower) < 0.15 &&
+                   Math.abs(robot.frontRightPower) < 0.15) {
+                    posAchieved = true;
+//                }
+            }
             }
         }
-    }
+
 
 
     @Override
@@ -65,6 +70,8 @@ public class DriveToCoordinatesState extends CyberarmState {
         engine.telemetry.addData("front left power", robot.frontLeftPower);
         engine.telemetry.addData("back right power", robot.backRightPower);
         engine.telemetry.addData("back left power", robot.backLeftPower);
+        engine.telemetry.addData("global object position", robot.objectPos);
+        engine.telemetry.addData("local object position", objectPos);
 
     }
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/CameraTestEngine.java

@@ -0,0 +1,40 @@
+package org.timecrafters.CenterStage.Autonomous.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "camera test")
+
+public class CameraTestEngine extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.imu.resetYaw();
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("Competition camera test");
+        this.robot.setup();
+        addState(new CameraVisionState(robot));
+
+
+
+
+
+
+
+
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java

@@ -12,7 +12,12 @@ import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+import org.openftc.easyopencv.OpenCvWebcam;
 import org.timecrafters.Library.Robot;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
@@ -28,13 +33,17 @@ public class CompetitionRobotV1 extends Robot {
     public String objectPos;
 
     // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup:
+    public double power;
 
+    public OpenCvWebcam webcam1 = null;
+    public WebcamName webCamName;
     public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp;
     public DcMotor odometerR, odometerL, odometerA;
 
     public IMU imu;
-    public Servo shoulder, elbow, leftClaw, rightClaw;
+    public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo;
     public DistanceSensor customObject;
+    public int loopCheck = 0;
 
     // ----------------------------------------------------------------------------------------------------------------- odometry variables:
     public static double Hp = 0.8, Hi = 0, Hd = 0;
@@ -47,9 +56,9 @@ public class CompetitionRobotV1 extends Robot {
     public double positionX = 1000;
     public double positionY = 1000;
     public double positionH = 0;
-    public double xTarget;
-    public double yTarget;
-    public double hTarget;
+    public double xTarget = 1000;
+    public double yTarget = 1000;
+    public double hTarget = 0;
 
     public int currentRightPosition = 0;
     public int currentLeftPosition = 0;
@@ -79,15 +88,20 @@ public class CompetitionRobotV1 extends Robot {
     public double frontRightPower;
     public double backRightPower;
 
+    public double yMaxPower = 1;
+    public double xMaxPower = 1;
+    public double pidX;
+    public double pidY;
+
     //-------------------------------------------------------------------------------------------------------------- arm sequence variables:
     PIDController pidController;
     public String armPos;
     public int target;
     public static double p = 0.0015, i = 0,  d = 0, f = 0;
-    public static double shoulderCollect = 0.25;
+    public static double shoulderCollect = 0.27;
     public static double shoulderDeposit = 0.32;
     public static double shoulderPassive = 0.8;
-    public static double elbowCollect = 0;
+    public static double elbowCollect = 0.02;
     public static double elbowDeposit = 0;
     public static double elbowPassive = 0;
 
@@ -114,6 +128,8 @@ public class CompetitionRobotV1 extends Robot {
         this.hardwareMap = CyberarmEngine.instance.hardwareMap;
         this.engine = CyberarmEngine.instance;
 
+        webCamName = hardwareMap.get(WebcamName.class, "Webcam 1");
+
         imu = engine.hardwareMap.get(IMU.class, "imu");
 
         //-------------------------------------------------------------------------------------------------------------------------- MOTORS:
@@ -137,7 +153,6 @@ public class CompetitionRobotV1 extends Robot {
         frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
         backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -153,6 +168,7 @@ public class CompetitionRobotV1 extends Robot {
         backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         chinUp.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         //IMU
         IMU.Parameters parameters = new IMU.Parameters(
@@ -162,8 +178,6 @@ public class CompetitionRobotV1 extends Robot {
 
         imu.initialize(parameters);
 
-        imu.resetYaw();
-
         configuration = new TimeCraftersConfiguration("Blue Crab");
 
         initConstants();
@@ -177,6 +191,8 @@ public class CompetitionRobotV1 extends Robot {
         elbow = hardwareMap.servo.get("elbow");
         leftClaw = hardwareMap.servo.get("leftClaw");
         rightClaw = hardwareMap.servo.get("rightClaw");
+        chinUpServo = hardwareMap.servo.get("chinUpServo");
+
 
         elbow.setDirection(Servo.Direction.REVERSE);
 
@@ -225,6 +241,14 @@ public class CompetitionRobotV1 extends Robot {
         }
         return radians;
     }
+
+    /**
+     * A Controller taking error of the heading position and converting to a power in the direction of a target.
+     * @param reference reference is the target position
+     * @param current  current is the measured sensor value.
+     * @return A power to the target the position
+     *
+     */
     public double HeadingPIDControl(double reference, double current){
         double error = angleWrap(current - reference);
         headingIntegralSum += error * headingTimer.seconds();
@@ -265,8 +289,20 @@ public class CompetitionRobotV1 extends Robot {
         double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
         double rx = HeadingPIDControl(Math.toRadians(hTarget), heading);
 
-        double pidX = YPIDControl(yTarget, positionY);
-        double pidY = XPIDControl(xTarget, positionX);
+        if (xMaxPower > XPIDControl(xTarget, positionX)){
+            pidX = xMaxPower;
+        } else {
+            pidX = YPIDControl(yTarget, positionY);
+        }
+
+        if (yMaxPower > YPIDControl(yTarget, positionY)){
+            pidY = yMaxPower;
+        } else {
+            pidY = XPIDControl(xTarget, positionX);
+        }
+
+        pidY = XPIDControl(xTarget, positionX);
+
 
         double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
 
@@ -294,7 +330,7 @@ public class CompetitionRobotV1 extends Robot {
         int armPos = clawArm.getCurrentPosition();
         double pid = pidController.calculate(armPos, target);
 
-        double power = pid;
+        power = pid;
 
         clawArm.setPower(power);
 

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java

@@ -60,6 +60,10 @@ public class CompetitionTeleOpState extends CyberarmState {
 
     // ---------------------------------------------------------------------------------------------------------------Arm control Variables:
     public String armPos = "passive";
+    // chin up servo
+    public static double chinUpServoUp = 1;
+    public static double chinUpServoDown = 0.25;
+
 
 
 
@@ -197,6 +201,7 @@ public class CompetitionTeleOpState extends CyberarmState {
 
     public void ArmPosControl(){
 
+
         if (engine.gamepad2.a){
             armPos = "collect";
             depositMode = false;
@@ -210,15 +215,23 @@ public class CompetitionTeleOpState extends CyberarmState {
         else if (engine.gamepad2.b){
             armPos = "hover";
             depositMode = true;
+        } else if (engine.gamepad2.dpad_left){
+            armPos = "lift up";
+            depositMode = true;
+        } else if (engine.gamepad2.dpad_right){
+            armPos = "lift down";
+            depositMode = false;
         }
 
         if (armPos == "collect"){
             if (robot.lift.getCurrentPosition() >= 1){
+                robot.chinUpServo.setPosition(chinUpServoDown);
                 robot.lift.setPower(-0.6);
             } else {
                 robot.lift.setPower(0);
                 robot.shoulder.setPosition(robot.shoulderCollect);
                 robot.elbow.setPosition(robot.elbowCollect);
+                robot.chinUpServo.setPosition(chinUpServoDown);
                 target = 10;
 
             }
@@ -226,8 +239,10 @@ public class CompetitionTeleOpState extends CyberarmState {
         if (armPos == "passive"){
             if (robot.lift.getCurrentPosition() >= 1){
                 robot.lift.setPower(-0.6);
+                robot.chinUpServo.setPosition(chinUpServoDown);
             } else {
                 robot.lift.setPower(0);
+                robot.chinUpServo.setPosition(chinUpServoDown);
                 robot.shoulder.setPosition(robot.shoulderPassive);
                 robot.elbow.setPosition(robot.elbowPassive);
                 target = 850;
@@ -237,23 +252,43 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.shoulder.setPosition(robot.shoulderDeposit);
                 robot.elbow.setPosition(robot.elbowDeposit);
                 target = 370;
+                robot.chinUpServo.setPosition(chinUpServoDown);
+
 
         }
-        if (armPos == "hover"){
-
+        if (armPos == "hover") {
             robot.shoulder.setPosition(robot.shoulderCollect);
             robot.elbow.setPosition(robot.elbowCollect);
             target = 120;
 
-
         }
+            if (armPos == "lift up") {
+                robot.shoulder.setPosition(robot.shoulderDeposit);
+                robot.elbow.setPosition(robot.elbowDeposit);
+                target = 120;
+                robot.chinUpServo.setPosition(chinUpServoUp);
+            }
+
+        if (armPos == "lift down"){
+            if (robot.lift.getCurrentPosition() >= 1){
+                robot.lift.setPower(-0.6);
+                robot.chinUpServo.setPosition(chinUpServoDown);
+            } else {
+                robot.lift.setPower(0);
+                robot.chinUpServo.setPosition(chinUpServoDown);
+                robot.shoulder.setPosition(robot.shoulderPassive);
+                robot.elbow.setPosition(robot.elbowPassive);
+                target = 850;
+            }
+        }
+
         }
 
     @Override
     public void init() {
-        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        pidController = new PIDController(p, i, d);
+            super.init();
+            pidController = new PIDController(p, i, d);
+
 
     }