Autonomous work

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

@@ -3,7 +3,9 @@ package org.timecrafters.Autonomous.Engines;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.Autonomous.States.DriverParkPlaceState;
 import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
+import org.timecrafters.Autonomous.States.PathDecision;
 import org.timecrafters.Autonomous.States.RotationState;
 import org.timecrafters.Autonomous.States.TopArmv2;
 import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@@ -12,6 +14,13 @@ import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
 public class RightStateAutoEngine extends CyberarmEngine {
     PhoenixBot1 robot;
 
+    @Override
+    public void loop() {
+        super.loop();
+
+        telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
+    }
+
     @Override
     public void setup() {
         robot = new PhoenixBot1(this);
@@ -24,26 +33,95 @@ public class RightStateAutoEngine extends CyberarmEngine {
                 PhoenixBot1.class,
                 "Right State Auto");
 
-//        addState(new TopArmv2(robot, "Right State Auto", "06-0"));
+////        addState(new TopArmv2(robot, "Right State Auto", "06-0"));
-
+//
-//        // driving forward to low junction
+////        // forward to low junction ..........................................................................(I have PreLoaded Cone)
-//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
+////        addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
-//        // rotate left towards low junction
+//
+//        // rotate left towards low junction (angle = 45, direction = CCW) (I have PreLoaded Cone)
 //        addState(new RotationState(robot, "Right State Auto", "02-1"));
-//        // driving forward/ backwards to adjust
+//
+//        // driving forward / backwards to adjust (I have PreLoaded Cone)
 //        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
-//        // drive forwards or backwards to adjust to pole
+//
-//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
+//        // counteract distance driven .........................................................................(I'm going for 2nd cone)
-//        // counteract distance driven
 //        addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
-//        // rotate towards opposing alliance
+//
-//        addState(new RotationState(robot, "Right State Auto", "04-1"));
+//        // rotate towards opposing alliance (angle = 0, direction = CW) (I'm going for 2nd cone)
-//        // drive to stack column
+//        addState(new RotationState(robot, "Right State Auto", "05-0"));
-//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
+//
-//        // rotate at stack
+//        // drive to tall junction (to adjust to cone stack)  (I'm going for 2nd cone)
-//        addState(new RotationState(robot, "Right State Auto", "05-1"));
-//        // drive at stack
 //        addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
+//
+//        // rotate at stack (angle = -90, direction = CW) (I'm going for 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "07-0"));
+//
+//        // drive at stack (I'm going for 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "08-0"));
+//
+//
+//        // drive away from stack slightly....................................................................... (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "09-0"));
+//
+//        //drive away to low (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "10-0"));
+//
+//        // rotate at low junction (angle = -135, direction = CW) (I have a 2nd cone)
+//        addState(new RotationState(robot, "Right State Auto", "11-0"));
+//
+//        // driving forward / backwards to adjust (I have a 2nd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "12-0"));
+//
+//        // counteract distance driven .......................................................................(I'm going for 3rd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "13-0"));
+//
+//        // rotate at stack (angle = -90, direction = CCW) (I'm going for 3rd cone)
+//        addState(new RotationState(robot, "Right State Auto", "14-0"));
+//
+//        // drive at stack (I'm going for 3rd cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "15-0"));
+//
+//        // drive away from stack slightly................................................................... (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "16-0"));
+//
+//        //drive away to mid (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "17-0"));
+//
+//        // rotate at mid junction (angle = -135, direction = CW) (I have a 3rd and final cone)
+//        addState(new RotationState(robot, "Right State Auto", "18-0"));
+//
+//        // driving forward / backwards to adjust (I have a 3rd and final cone)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "19-0"));
+//
+//        // counteract distance driven.............................................................................. (I'm parking)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "20-0"));
+//
+//        // rotate at opposing alliance (angle = 0, direction = CCW) (I'm parking)
+//        addState(new RotationState(robot, "Right State Auto", "21-0"));
+//
+//        // Drive back one tile (I'm parking)
+//        addState(new DriverStateWithOdometer(robot, "Right State Auto", "22-0"));
+//
+//        // rotate towards stack side (-90 CW) (I'm parking)
+//        addState(new RotationState(robot, "Right State Auto", "23-0"));
+//
+//        // Choose Parking Spot (I'm parking)
+//        addState(new PathDecision(robot, "RightSideAutonomous", "24-0"));
+//
+//        // case 1 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-1"));
+//
+//        // case 2 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-2"));
+//
+//        // case 3 drive forward (I'm parking)
+//        addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-3"));
+//
+//        // rotate towards opposing alliance (angle = 0, direction = CCW) (I'm parking)
+//        addState(new RotationState(robot, "RightSideAutonomous", "25-0"));
+
+
+
 
 
 

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

@@ -12,7 +12,6 @@ public class CollectorDistanceState extends CyberarmState {
     private int traveledDistance;
     private int RampUpDistance;
     private int RampDownDistance;
-    private double drivePower;
     private double targetDrivePower;
     private double lastMeasuredTime;
     private double currentDistance;
@@ -24,6 +23,7 @@ public class CollectorDistanceState extends CyberarmState {
     private float collectTime;
     private double inRangeTime;
     private boolean stateDisabled;
+    private double distanceLimit;
 
 
     public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -33,6 +33,7 @@ public class CollectorDistanceState extends CyberarmState {
         this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
         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.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
 
 
@@ -44,21 +45,15 @@ 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.OdometerEncoderRight.getCurrentPosition());
-        engine.telemetry.addLine();
 
         engine.telemetry.addData("traveledDistance", traveledDistance);
         engine.telemetry.addData("RampUpDistance", RampUpDistance);
         engine.telemetry.addData("RampDownDistance", RampDownDistance);
 
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("drivePower", drivePower);
         engine.telemetry.addData("targetDrivePower", targetDrivePower);
 
         engine.telemetry.addLine();
-        engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("Current Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
-        engine.telemetry.addData("Current Distance", currentDistance);
         engine.telemetry.addData("last Distance", LastDistanceRead);
         engine.telemetry.addLine();
 
@@ -75,17 +70,11 @@ 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.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.OdometerEncoderHorizontal.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.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
 
         robot.collectorLeft.setPower(1);
@@ -102,7 +91,7 @@ public class CollectorDistanceState extends CyberarmState {
 
     @Override
     public void exec() {
-        if (stateDisabled){
+        if (stateDisabled) {
             robot.frontRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.backRightDrive.setPower(0);
@@ -141,36 +130,17 @@ public class CollectorDistanceState extends CyberarmState {
             }
         }
 
-        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
+        if (robot.collectorDistance.getDistance(DistanceUnit.MM) > distanceLimit) {
+
             double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
 
-            if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
+            robot.backLeftDrive.setPower(targetDrivePower);
-                // ramping up
+            robot.backRightDrive.setPower(targetDrivePower);
-                drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.15;
+            robot.frontLeftDrive.setPower(targetDrivePower);
-            } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
+            robot.frontRightDrive.setPower(targetDrivePower);
-                // ramping down
+
-                drivePower = ((delta / RampDownDistance) + 0.15);
         } 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(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
-                robot.backLeftDrive.setPower(drivePower);
-                robot.backRightDrive.setPower(drivePower);
-                robot.frontLeftDrive.setPower(drivePower);
-                robot.frontRightDrive.setPower(drivePower);
-            }
-    } else {
             robot.frontRightDrive.setPower(0);
             robot.frontLeftDrive.setPower(0);
             robot.backRightDrive.setPower(0);

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

@@ -15,7 +15,7 @@ public class DriverStateWithOdometer extends CyberarmState {
     private float direction;
     private boolean targetAchieved = false;
     public final double WHEEL_CIRCUMFERENCE = 7.42108499;
-    public final int COUNTS_PER_REVOLUTION = 8192;
+    public final double COUNTS_PER_REVOLUTION = 8192;
     public double startOfRampUpRight;
     public double startOfRampDownRight;
     public double startOfRampUpLeft;
@@ -40,8 +40,7 @@ public class DriverStateWithOdometer extends CyberarmState {
         this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
     }
 
-    private double drivePower, targetDrivePower;
+    private double drivePower, targetDrivePower, traveledDistance;
-    private int traveledDistance;
 
     @Override
     public void start() {
@@ -62,24 +61,24 @@ public class DriverStateWithOdometer extends CyberarmState {
 
 
         if (targetDrivePower > 0) {
-            startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
+            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();
+            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();
+            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();
+            startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
             endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
             startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
             endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
@@ -255,7 +254,7 @@ public class DriverStateWithOdometer extends CyberarmState {
             }
         }
 
-        // .................................................................................................................................................Strafe Adjustment
+        // ...........................................................................................................................................Strafe Adjustment
         if ( driveStage == 3 ){
 
             currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
@@ -294,7 +293,10 @@ public class DriverStateWithOdometer extends CyberarmState {
     @Override
     public void telemetry () {
         engine.telemetry.addData("Stage", driveStage);
-        engine.telemetry.addData("maximumTolerance", maximumTolerance);
+
+        engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
+        engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
+        engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
         engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
         engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
         engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
@@ -311,9 +313,7 @@ public class DriverStateWithOdometer extends CyberarmState {
         engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
         engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
         engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
-        engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
+        engine.telemetry.addData("maximumTolerance", maximumTolerance);
-        engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
-        engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
         engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
         engine.telemetry.addData("Target Achieved", targetAchieved);
 

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

@@ -37,6 +37,7 @@ public class PhoenixBot1 {
     public double ROTATION_TOLERANCE;
     public long PAUSE_ON_ROTATION;
     public double DISTANCE_MULTIPLIER;
+    public double CAMERA_INITiAL_POS;
 
 //    private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
     private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -104,6 +105,7 @@ public class PhoenixBot1 {
             PAUSE_ON_ROTATION = configuration.variable("Robot", "Tuning", "PAUSE_ON_ROTATION").value();
             WHEEL_CIRCUMFERENCE = configuration.variable("Robot", "Tuning", "WHEEL_CIRCUMFERENCE").value();
             DISTANCE_MULTIPLIER = configuration.variable("Robot", "Tuning", "DISTANCE_MULTIPLIER").value();
+            CAMERA_INITiAL_POS = configuration.variable("Robot", "Tuning", "CAMERA_INITiAL_POS").value();
         }
 
     private void initVuforia(){
@@ -247,7 +249,7 @@ public class PhoenixBot1 {
 //            HighRiserLeft.setPosition(0.40);
 //            HighRiserRight.setPosition(0.40);
 
-            CameraServo.setPosition(0.775);
+            CameraServo.setPosition(CAMERA_INITiAL_POS);