Autonomous work

TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java

@@ -24,6 +24,10 @@ public class CollectorDistanceState extends CyberarmState {
     private double inRangeTime;
     private boolean stateDisabled;
     private double distanceLimit;
+    private long maximumLookTime;
+    private long startOfSequencerTime;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
 
 
     public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -34,6 +38,7 @@ public class CollectorDistanceState extends CyberarmState {
         this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
         this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value();
         this.distanceLimit = robot.configuration.variable(groupName, actionName, "distanceLimit").value();
+        this.maximumLookTime = robot.configuration.variable(groupName, actionName, "maximumLookTime").value();
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
 
 
@@ -41,6 +46,8 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void telemetry() {
+        engine.telemetry.addData("Time left", System.currentTimeMillis() - startOfSequencerTime);
+
         engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
@@ -81,8 +88,10 @@ public class CollectorDistanceState extends CyberarmState {
         robot.collectorRight.setPower(1);
 
         lastMeasuredTime = System.currentTimeMillis();
+        startOfSequencerTime = System.currentTimeMillis();
         LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM);
 
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
 
 
     }
@@ -91,13 +100,14 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void exec() {
-        if (stateDisabled) {
+        if (stateDisabled || System.currentTimeMillis() - startOfSequencerTime > maximumLookTime) {
+            robot.collectorLeft.setPower(0);
+            robot.collectorRight.setPower(0);
             robot.frontRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.backRightDrive.setPower(0);
             robot.backLeftDrive.setPower(0);
             setHasFinished(true);
-            return;
         }
 
         if (System.currentTimeMillis() - lastMeasuredTime > 150) {
@@ -125,8 +135,6 @@ public class CollectorDistanceState extends CyberarmState {
                 robot.backLeftDrive.setPower(0);
 
                 setHasFinished(true);
-
-                return;
             }
         }
 

TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java

@@ -3,6 +3,7 @@ package org.timecrafters.Autonomous.States;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 
 public class DriverParkPlaceState extends CyberarmState {
@@ -11,6 +12,25 @@ public class DriverParkPlaceState extends CyberarmState {
     private int RampUpDistance;
     private int RampDownDistance;
     private String intendedPlacement;
+    public final double WHEEL_CIRCUMFERENCE = 7.42108499;
+    public final double COUNTS_PER_REVOLUTION = 8192;
+    private double maximumTolerance;
+    private float direction;
+    private boolean targetAchieved = false;
+    public double startOfRampUpRight;
+    public double startOfRampDownRight;
+    public double startOfRampUpLeft;
+    public double startOfRampDownLeft;
+    public double endOfRampUpRight;
+    public double endOfRampDownRight;
+    public double endOfRampUpLeft;
+    public double endOfRampDownLeft;
+    public int driveStage;
+    public float currentAngle;
+    public double currentHorizontalEncoder;
+
+
+
 
     public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
@@ -18,6 +38,7 @@ public class DriverParkPlaceState extends CyberarmState {
         this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
         this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
         this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+        this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
         this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
     }
@@ -35,6 +56,38 @@ public class DriverParkPlaceState extends CyberarmState {
         robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+        maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
+
+        if (targetDrivePower > 0) {
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
+
+        } else {
+
+            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
+            endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
+            startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
+
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
+            endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
+            startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
+            endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
+
+        }
+
+        driveStage = 0;
     }
 
     @Override
@@ -44,45 +97,201 @@ public class DriverParkPlaceState extends CyberarmState {
             return;
         }
         String placement = engine.blackboardGetString("parkPlace");
-        if (placement != null) {
+        if (placement == null || !placement.equals(intendedPlacement)) {
-            if (!placement.equals(intendedPlacement)){
+
-                    setHasFinished(true);
+            setHasFinished(true);
-            }
+
-            if (placement.equals(intendedPlacement)) {
+        } else {
-                double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
+
+                double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
+                double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
+
+                // Driving Forward
+                if (targetDrivePower > 0 && driveStage == 0) {
 
-                if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
                     // ramping up
-                    drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.25;
+                    if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
-                } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
+                            (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
-                    // ramping down
+
-                    drivePower = ((delta / RampDownDistance) + 0.25);
+                        drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
-                } else {
+                                (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
-                    // middle ground
+
-                    drivePower = targetDrivePower;
+                    }
+
+                    // Driving Normal
+                    else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
+                            (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
+
+                        drivePower = targetDrivePower;
+
+                    }
+
+                    // Ramping down going forward
+                    else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
+                            (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
+                        drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (driveStage == 0){
+                        driveStage = 1;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
                 }
 
-                if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
+                // Driving Backwards .................................................................................................................................Backwards
-                    // This is limiting drive power to the targeted drive power
+                if (targetDrivePower < 0 && driveStage == 0) {
-                    drivePower = targetDrivePower;
+
+                    // ramping up
+                    if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
+                            (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
+
+                        drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    }
+
+                    // Driving Normal
+                    else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
+                            (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
+
+                        drivePower = targetDrivePower;
+
+                    }
+
+                    // Ramping down going backward
+                    else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
+                            (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
+
+                        drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
+                                (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
+                    } else if (driveStage == 0){
+                        driveStage = 1;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                    }
+
+                    // end of ramp down
                 }
 
-                if (targetDrivePower < 0 && drivePower != targetDrivePower) {
+                // Forwards distance adjust...............................................................................................................................STAGE 1
-                    drivePower = drivePower * -1;
+                if (driveStage == 1 && targetDrivePower > 0) {
+
+                    if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                            RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                        drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                            RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                        drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else {
+                        driveStage = 2;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
                 }
 
-                if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
+                // backwards distance adjust
-                    robot.backLeftDrive.setPower(drivePower);
+                if (driveStage == 1 && targetDrivePower < 0) {
-                    robot.backRightDrive.setPower(drivePower);
+
-                    robot.frontLeftDrive.setPower(drivePower);
+                    if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
+                            RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
+
+                        drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
+                            RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
+
+                        drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
+
+                    } else {
+                        driveStage = 2;
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+                    }
+                }
+
+                if (driveStage == 0 || driveStage == 1) {
                     robot.frontRightDrive.setPower(drivePower);
-                } else {
+                    robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
-                    robot.backLeftDrive.setPower(0);
+                    robot.backRightDrive.setPower(drivePower);
-                    robot.backRightDrive.setPower(0);
+                    robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
-                    robot.frontLeftDrive.setPower(0);
+                }
-                    robot.frontRightDrive.setPower(0);
+                // Heading adjustment
+                if (driveStage == 2 || driveStage == 4) {
+
+                    currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
+
+                    if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
+
+                        robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                        robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                        robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER  );
+
+                    }
+                    else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
+
+                        robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                        robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+                        robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
+                        robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
+
+                    } else {
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                        driveStage ++;
+                    }
+                }
+
+                // ...........................................................................................................................................Strafe Adjustment
+                if ( driveStage == 3 ){
+
+                    currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
+                    if (currentHorizontalEncoder > 200){
+
+                        robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+
+                    }
+                    else if (currentHorizontalEncoder < -200){
+
+                        robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+                        robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
+                        robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
+
+                    } else {
+
+                        robot.frontRightDrive.setPower(0);
+                        robot.frontLeftDrive.setPower(0);
+                        robot.backRightDrive.setPower(0);
+                        robot.backLeftDrive.setPower(0);
+
+                        driveStage = 4;
+
+                    }
+
+                if (driveStage == 5) {
                     setHasFinished(true);
-                } // else ending
+                }
             } // placement equals if statement
 //            setHasFinished(true);
         } // end of placement doesn't equal null

TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java

@@ -7,6 +7,7 @@ import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 public class RotationState extends CyberarmState {
     private final boolean stateDisabled;
     PhoenixBot1 robot;
+
     public RotationState(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
@@ -52,13 +53,17 @@ public class RotationState extends CyberarmState {
 
             drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER;
 
-            if (ClockWiseRotation) { rotationDirection = 1;} else { rotationDirection = -1;}
+            if (ClockWiseRotation) {
+                rotationDirection = 1;
+            } else {
+                rotationDirection = -1;
+            }
 
 
-                robot.backLeftDrive.setPower( drivePowerVariable * rotationDirection );
+            robot.backLeftDrive.setPower(drivePowerVariable * rotationDirection);
-                robot.backRightDrive.setPower( -drivePowerVariable * rotationDirection );
+            robot.backRightDrive.setPower(-drivePowerVariable * rotationDirection);
-                robot.frontLeftDrive.setPower( drivePowerVariable * rotationDirection );
+            robot.frontLeftDrive.setPower(drivePowerVariable * rotationDirection);
-                robot.frontRightDrive.setPower( -drivePowerVariable * rotationDirection );
+            robot.frontRightDrive.setPower(-drivePowerVariable * rotationDirection);
 
             if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE &&
                     (RotationStage == 0) &&
@@ -66,14 +71,14 @@ public class RotationState extends CyberarmState {
                 RotationStage = 1;
                 lastStepTime = System.currentTimeMillis();
             }
-            }
+        }
 
-        if (RotationStage == 1){
+        if (RotationStage == 1) {
-            robot.backLeftDrive.setPower( 0 );
+            robot.backLeftDrive.setPower(0);
-            robot.backRightDrive.setPower( 0 );
+            robot.backRightDrive.setPower(0);
-            robot.frontLeftDrive.setPower( 0 );
+            robot.frontLeftDrive.setPower(0);
-            robot.frontRightDrive.setPower( 0 );
+            robot.frontRightDrive.setPower(0);
-            if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION ){
+            if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
                 RotationStage = 2;
             }
         }
@@ -83,13 +88,14 @@ public class RotationState extends CyberarmState {
             if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) {
                 // CW
 
-                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
-                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
+                robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER);
-                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER  );
+                robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
-                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER  );
+                robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER);
 
-            }
+                lastStepTime = System.currentTimeMillis();
-            else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
+
+            } else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
                 // CCW
 
                 robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
@@ -97,18 +103,24 @@ public class RotationState extends CyberarmState {
                 robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
                 robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
 
+                lastStepTime = System.currentTimeMillis();
+
             } else {
                 robot.frontRightDrive.setPower(0);
                 robot.frontLeftDrive.setPower(0);
                 robot.backRightDrive.setPower(0);
                 robot.backLeftDrive.setPower(0);
 
-                RotationStage ++;
+                if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
-                setHasFinished(true);
+                    RotationStage = 3;
+                }
             }
         }
 
-
+        if (RotationStage == 3) {
+            RotationStage ++;
+            setHasFinished(true);
+            }
         }
 
 

TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmResetState.java

@@ -0,0 +1,48 @@
+package org.timecrafters.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
+
+public class TopArmResetState extends CyberarmState {
+    private final PhoenixBot1 robot;
+    private double targetPower;
+    private int targetPosition;
+    private int tolerance;
+    private long lastMeasuredTime;
+    private long pausingTime;
+
+
+    public TopArmResetState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
+        this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
+        this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
+        this.pausingTime = robot.configuration.variable(groupName, actionName, "pausingTime").value();
+    }
+
+    @Override
+    public void start() {
+        robot.ArmMotor.setTargetPosition(targetPosition);
+        robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.ArmMotor.setPower(targetPower);
+        lastMeasuredTime = System.currentTimeMillis();
+
+    }
+
+    @Override
+    public void exec() {
+
+        if (System.currentTimeMillis() - lastMeasuredTime > pausingTime) {
+
+            robot.ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            robot.ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            robot.ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            robot.ArmMotor.setTargetPosition(0);
+            robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            robot.ArmMotor.setPower(0.2);
+            setHasFinished(true);
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java

@@ -24,6 +24,7 @@ public class TopArmv2 extends CyberarmState {
         robot.ArmMotor.setTargetPosition(targetPosition);
         robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         robot.ArmMotor.setPower(targetPower);
+
     }
 
     @Override

TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java

@@ -240,7 +240,7 @@ public class PhoenixBot1 {
             ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
             ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
             ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-            ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+            ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
             CameraServo.setDirection(Servo.Direction.FORWARD);
 

TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java

@@ -21,7 +21,7 @@ public class TeleOPArmDriver extends CyberarmState {
     //    private double servoLowHigh = 0.75; //High servos, X button
     private double servoMed = 0.45; //Low servos, B button
     //    private double servoMedHigh = 0.9; //High servos, B button
-    private double servoHigh = 0.7; //Low servos, Y button
+    private double servoHigh = 0.8; //Low servos, Y button
     //    private double servoHighHigh = 0.9; //High servos, Y button
     private double ArmNeededPower;
     private int armMotorCollect = -100;