Copied Drive State for autonomous and modified the copy so it runs off of the Odometer Encoder to get distance driven instead of motor encoder

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

@@ -0,0 +1,114 @@
+package org.timecrafters.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
+
+public class DriverStateWithOdometer extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private int RampUpDistance;
+    private int RampDownDistance;
+    public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
+        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.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+    private double drivePower, targetDrivePower;
+    private int traveledDistance;
+
+    @Override
+    public void start() {
+        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        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.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);
+    }
+
+    @Override
+    public void exec() {
+        if (stateDisabled) {
+            setHasFinished(true);
+            return;
+        }
+
+        double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
+        double delta = traveledDistance - Math.abs(CurrentPosition);
+
+        if (Math.abs(CurrentPosition) <= RampUpDistance){
+            // ramping up
+           drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
+        }
+        else if (Math.abs(CurrentPosition) >= delta){
+            // ramping down
+            drivePower = ((delta / RampDownDistance) + 0.25);
+            } else {
+            // middle ground
+            drivePower = targetDrivePower;
+        }
+
+        if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
+            // This is limiting drive power to the targeted drive power
+            drivePower = targetDrivePower;
+        }
+
+        if (targetDrivePower < 0 && drivePower != targetDrivePower) {
+            drivePower = drivePower * -1;
+        }
+
+        if (Math.abs(CurrentPosition) < traveledDistance){
+            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);
+            robot.frontLeftDrive.setPower(0);
+            robot.frontRightDrive.setPower(0);
+            setHasFinished(true);
+        }
+
+
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
+        engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
+        engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
+        engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+        engine.telemetry.addData("drivePower", drivePower);
+        engine.telemetry.addData("targetDrivePower", targetDrivePower);
+
+        engine.telemetry.addData("traveledDistance", traveledDistance);
+        engine.telemetry.addData("RampUpDistance", RampUpDistance);
+        engine.telemetry.addData("RampDownDistance", RampDownDistance);
+
+    }
+}

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

@@ -42,7 +42,7 @@ public class PhoenixBot1 {
 
     public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance;
 
-        public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
+        public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder;
 
         public CRServo collectorLeft, collectorRight;
 
@@ -125,12 +125,19 @@ public class PhoenixBot1 {
             backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
             backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
+            // Dead Wheel encoder for driving
+
+            OdometerEncoder.setDirection(DcMotorSimple.Direction.FORWARD);
+            OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
 
             HighRiserLeft.setDirection(Servo.Direction.REVERSE);
             HighRiserRight.setDirection(Servo.Direction.FORWARD);
             LowRiserLeft.setDirection(Servo.Direction.FORWARD);
             LowRiserRight.setDirection(Servo.Direction.REVERSE);
 
+
             CameraServo.setDirection(Servo.Direction.FORWARD);
 
             LowRiserLeft.setPosition(0.35);