driving straight added

TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java

@@ -50,6 +50,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
         // 6-1 drive forward or backwards if needed this is customizable so we can adapt
         addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
 
+        //pause
+        addState(new BottomArm(robot, "RightFourCone", "06-2"));
+
 
         // 7 Drop bottom arm down on the junction to place cone
         addState(new BottomArm(robot, "RightFourCone", "07-0"));
@@ -70,18 +73,30 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
         // 11 Rotate towards stack
         addState(new RotationState(robot, "RightFourCone", "11-0"));
 //
-//        // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
+        // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
-//        addState(new TopArm(robot, "RightFourCone", "12-0"));
+        addState(new TopArm(robot, "RightFourCone", "12-0"));
+
+        // drive forward at the stack without sensor
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "12-1"));
+
+        // turn and align at stack
+        addState(new RotationState(robot, "RightFourCone", "12-2"));
+
 //
-//        // 13 Drive at stack while collecting and check to see when we grab it
+        // 13 Drive at stack while collecting and check to see when we grab it
-//        addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
+        addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
+
+
 //
 //        // 14 Back up and raise arm
-//        addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
-//        addState(new TopArm(robot, "RightFourCone", "14-1"));
+        addState(new TopArm(robot, "RightFourCone", "14-1"));
+
+        // 14-2 align perpendicular too the wall
+        addState(new RotationState(robot, "RightFourCone", "14-2"));
 //
-//        // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state)
+        // 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
-//        addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
 //
 //        // 16 Rotate and use sensor to find junction
 //        addState(new RotationState(robot, "RightFourCone", "16-0"));

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

@@ -44,6 +44,7 @@ public class CollectorDistanceState extends CyberarmState {
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoder.getCurrentPosition());
         engine.telemetry.addLine();
 
         engine.telemetry.addData("traveledDistance", traveledDistance);
@@ -74,12 +75,14 @@ public class CollectorDistanceState extends CyberarmState {
         robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
         robot.collectorLeft.setPower(1);
         robot.collectorRight.setPower(1);
@@ -96,6 +99,10 @@ public class CollectorDistanceState extends CyberarmState {
     @Override
     public void exec() {
         if (stateDisabled){
+            robot.frontRightDrive.setPower(0);
+            robot.frontLeftDrive.setPower(0);
+            robot.backRightDrive.setPower(0);
+            robot.backLeftDrive.setPower(0);
             setHasFinished(true);
             return;
         }
@@ -108,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState {
 
             distanceDelta = LastDistanceRead - currentDistance;
 
-            if (distanceDelta >= 0.0 || currentDistance > 500) {
+            if (distanceDelta >= -50.0 || currentDistance > 500) {
                 // I am moving forward
                 // and im close too my target.
                 LastDistanceRead = currentDistance;
@@ -119,18 +126,24 @@ public class CollectorDistanceState extends CyberarmState {
                 debugStatus = "Nothing Collected";
                 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 (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
+        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
-            double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
+            double delta = traveledDistance - Math.abs(robot.OdometerEncoder.getCurrentPosition());
 
-            if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
+            if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) <= RampUpDistance) {
                 // ramping up
-                drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
+                drivePower = (Math.abs((float) robot.OdometerEncoder.getCurrentPosition()) / RampUpDistance) + 0.15;
-            } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
+            } else if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) >= delta) {
                 // ramping down
                 drivePower = ((delta / RampDownDistance) + 0.15);
             } else {
@@ -147,7 +160,7 @@ public class CollectorDistanceState extends CyberarmState {
                 drivePower = drivePower * -1;
             }
 
-            if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
+            if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
                 robot.backLeftDrive.setPower(drivePower);
                 robot.backRightDrive.setPower(drivePower);
                 robot.frontLeftDrive.setPower(drivePower);
@@ -167,6 +180,10 @@ public class CollectorDistanceState extends CyberarmState {
             if (runTime() - inRangeTime >= collectTime){
                 robot.collectorRight.setPower(0);
                 robot.collectorLeft.setPower(0);
+                robot.frontRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.backLeftDrive.setPower(0);
                 setHasFinished(true);
             }
         }

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

@@ -10,10 +10,11 @@ import org.timecrafters.TeleOp.states.PhoenixBot1;
 public class DriverStateWithOdometer extends CyberarmState {
     private final boolean stateDisabled;
     PhoenixBot1 robot;
-    private int RampUpDistance;
+    private double RampUpDistance;
-    private int RampDownDistance;
+    private double RampDownDistance;
     private int maximumTolerance;
     private float direction;
+    private boolean targetAchieved = false;
     public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -34,11 +35,13 @@ public class DriverStateWithOdometer extends CyberarmState {
         robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
         robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
 
 
@@ -52,18 +55,24 @@ public class DriverStateWithOdometer extends CyberarmState {
             return;
         }
 
-        double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
+        double CurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition());
-        double delta = traveledDistance - Math.abs(CurrentPosition);
 
         if (Math.abs(CurrentPosition) <= RampUpDistance){
             // ramping up
-            float ratio = (Math.abs((float)CurrentPosition) / RampUpDistance);
+//            double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
-            drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
+            if (targetDrivePower > 0) {
+                drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
+            } else {
+                drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
+            }
         }
         else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
             // ramping down
-            double ratio = (1.0 - ((traveledDistance - RampDownDistance) / RampDownDistance));
+            if (targetDrivePower > 0){
-            drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
+                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
+            } else {
+                drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
+            }
 
         } else {
             // middle ground
@@ -75,11 +84,22 @@ public class DriverStateWithOdometer extends CyberarmState {
             drivePower = targetDrivePower;
         }
 
-        if (targetDrivePower < 0 && drivePower != targetDrivePower) {
+        if (targetDrivePower < 0 && drivePower > 0) {
             drivePower = drivePower * -1;
         }
 
         if (Math.abs(CurrentPosition) < traveledDistance - maximumTolerance){
+            if (targetAchieved) {
+                drivePower = drivePower * 0.25;
+
+                if (Math.abs(drivePower) < 0.25){
+                    if (drivePower < 0) {
+                        drivePower = -0.25;
+                    } else {
+                        drivePower = 0.25;
+                    }
+                }
+            }
             robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(drivePower);
             robot.frontLeftDrive.setPower(drivePower);
@@ -87,10 +107,24 @@ public class DriverStateWithOdometer extends CyberarmState {
 
         }
         else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
-            robot.backLeftDrive.setPower(targetDrivePower * -0.25);
+            targetAchieved = true;
-            robot.backRightDrive.setPower(targetDrivePower * -0.25);
+
-            robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
+            drivePower = targetDrivePower * -0.25;
-            robot.frontRightDrive.setPower(targetDrivePower * -0.25);
+
+            if (Math.abs(drivePower) < 0.25){
+                if (drivePower < 0) {
+                    drivePower = -0.25;
+                } else {
+                    drivePower = 0.25;
+                }
+            }
+
+            robot.backLeftDrive.setPower(drivePower);
+            robot.backRightDrive.setPower(drivePower);
+            robot.frontLeftDrive.setPower(drivePower);
+            robot.frontRightDrive.setPower(drivePower);
+
+
         } else {
             robot.backLeftDrive.setPower(0);
             robot.backRightDrive.setPower(0);
@@ -98,24 +132,25 @@ public class DriverStateWithOdometer extends CyberarmState {
             robot.frontRightDrive.setPower(0);
             setHasFinished(true);
         }
-        if (!getHasFinished()){
+
+        if (!getHasFinished() && !targetAchieved){
             float angle = robot.imu.getAngularOrientation().firstAngle - direction;
 
+            if (targetDrivePower < 0) { angle = angle * -1;}
+
             if (angle < -0.25){
-                robot.backLeftDrive.setPower(drivePower * 0.25);
+                robot.backLeftDrive.setPower(drivePower * 0);
                 robot.backRightDrive.setPower(drivePower * 1.25);
-                robot.frontLeftDrive.setPower(drivePower * 0.25);
+                robot.frontLeftDrive.setPower(drivePower * 0);
                 robot.frontRightDrive.setPower(drivePower * 1.25);
             }
             if (angle > 0.25) {
                 robot.backLeftDrive.setPower(drivePower * 1.25);
-                robot.backRightDrive.setPower(drivePower * 0.25);
+                robot.backRightDrive.setPower(drivePower * 0);
                 robot.frontLeftDrive.setPower(drivePower * 1.25);
-                robot.frontRightDrive.setPower(drivePower * 0.25);
+                robot.frontRightDrive.setPower(drivePower * 0);
             }
         }
-
-
     }
 
     @Override
@@ -133,6 +168,9 @@ public class DriverStateWithOdometer extends CyberarmState {
         engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
         engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
 
+        engine.telemetry.addData("Target Achieved", targetAchieved);
+
+
 
         engine.telemetry.addData("drivePower", drivePower);
         engine.telemetry.addData("targetDrivePower", targetDrivePower);

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

@@ -36,6 +36,13 @@ public class RotationState extends CyberarmState {
 
         if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
             drivePowerVariable = 0.3 * drivePower;
+            if (Math.abs(drivePowerVariable) < 0.3) {
+                if (drivePowerVariable < 0){
+                    drivePowerVariable = -0.3;
+                } else {
+                    drivePowerVariable = 0.3;
+                }
+            }
             debugStatus = "Rotate Slow";
         } // end of if
         else {

TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java

@@ -0,0 +1,15 @@
+package org.timecrafters.TeleOp.engine;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
+
+public class DriveDebugEngine extends CyberarmEngine {
+
+    PhoenixBot1 robot;
+
+    @Override
+    public void setup() {
+        robot = new PhoenixBot1(this);
+
+    }
+}