Merge remote-tracking branch 'origin/master'

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

@@ -8,7 +8,8 @@ import org.timecrafters.Autonomous.States.CollectorDistanceState;
 import org.timecrafters.Autonomous.States.CollectorState;
 import org.timecrafters.Autonomous.States.ConeIdentification;
 import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
-import org.timecrafters.Autonomous.States.JunctionAllignmentState;
+import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
+import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
 import org.timecrafters.Autonomous.States.PathDecision;
 import org.timecrafters.Autonomous.States.RotationState;
 import org.timecrafters.Autonomous.States.ServoCameraRotate;
@@ -47,7 +48,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
         addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
 
         // 6-3 align to junction with rotation or skip if it looks like it won't be able to
-        addState(new JunctionAllignmentState(robot, "RightFourCone", "06-3"));
+        addState(new JunctionAllignmentDistanceState(robot, "RightFourCone", "06-3"));
+        addState(new JunctionAllignmentAngleState(robot, "RightFourCone", "06-4"));
 
         //pause
         addState(new BottomArm(robot, "RightFourCone", "06-2"));
@@ -55,16 +57,22 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
 
         // 7 Drop bottom arm down on the junction to place cone
         addState(new BottomArm(robot, "RightFourCone", "07-0"));
+        addParallelStateToLastState(new TopArm(robot, "RightFourCone", "07-1"));
 
         // 7-1 drive back slightly
-        addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-1"));
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-2"));
 
         // 8 Drop cone as soon as arm is in position
         addState(new CollectorState(robot, "RightFourCone", "08-0"));
+        // 8-1 drive back to lose contact
+        addState(new DriverStateWithOdometer(robot, "RightFourCone", "08-1"));
+        // 8-1 realign to old angle
+        addState(new RotationState(robot, "RightFourCone", "08-2"));
 
         // 9 Raise bottom arm to clear junction
-        addState(new BottomArm(robot, "RightFourCone", "09-0"));
+        addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "09-0"));
-//
+        addParallelStateToLastState(new TopArm(robot, "RightFourCone", "09-1"));
+
 //        // 10 Back up and bring lower arm down (parallel state)
         addState(new DriverStateWithOdometer(robot, "RightFourCone", "10-0"));
         addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "10-1"));

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

@@ -6,7 +6,7 @@ import org.cyberarm.engine.V2.CyberarmState;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.timecrafters.TeleOp.states.PhoenixBot1;
 
-public class JunctionAllignmentState extends CyberarmState {
+public class JunctionAllignmentAngleState extends CyberarmState {
     private final boolean stateDisabled;
     PhoenixBot1 robot;
     private final double drivePower;
@@ -25,7 +25,7 @@ public class JunctionAllignmentState extends CyberarmState {
     private boolean finishedEnabled;
 
 
-    public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
+    public JunctionAllignmentAngleState(PhoenixBot1 robot, String groupName, String actionName) {
         this.robot = robot;
         this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
         this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
@@ -114,6 +114,11 @@ public class JunctionAllignmentState extends CyberarmState {
                 loopsCurrent = loopsCurrent + 1;
 
                 if (rightInRange && leftInRange){
+
+                    robot.frontRightDrive.setPower(0);
+                    robot.backRightDrive.setPower(0);
+                    robot.backLeftDrive.setPower(0);
+                    robot.frontLeftDrive.setPower(0);
                     setHasFinished(finishedEnabled);
                 }
 
@@ -188,30 +193,32 @@ public class JunctionAllignmentState extends CyberarmState {
 
                 }
 
-                if (!rightInRange && !leftInRange && driveLoops < 2){
+                if (!rightInRange && !leftInRange /*&& driveLoops < 2*/){
 
-                    if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
+//                    if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
-                        robot.backLeftDrive.setPower(drivePower);
+//                        robot.backLeftDrive.setPower(drivePower);
-                        robot.backRightDrive.setPower(drivePower);
+//                        robot.backRightDrive.setPower(drivePower);
-                        robot.frontLeftDrive.setPower(drivePower);
+//                        robot.frontLeftDrive.setPower(drivePower);
-                        robot.frontRightDrive.setPower(drivePower);
+//                        robot.frontRightDrive.setPower(drivePower);
+//
+//                        driveLoops += 1;
+//                    }
+//
+//                    } else {
+//
+//                        robot.backLeftDrive.setPower(0);
+//                        robot.backRightDrive.setPower(0);
+//                        robot.frontLeftDrive.setPower(0);
+//                        robot.frontRightDrive.setPower(0);
+//
+//                        robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+//                        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+//
+//                        if (driveLoops == 2) {
+//                            loopsCurrent = loopsTotal;
+//                        }
 
-                        driveLoops += 1;
+                    setHasFinished(true);
-                    }
-
-                    } else {
-
-                        robot.backLeftDrive.setPower(0);
-                        robot.backRightDrive.setPower(0);
-                        robot.frontLeftDrive.setPower(0);
-                        robot.frontRightDrive.setPower(0);
-
-                        robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-                        robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-                        if (driveLoops == 2) {
-                            loopsCurrent = loopsTotal;
-                        }
 
 
                     }

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

@@ -0,0 +1,144 @@
+package org.timecrafters.Autonomous.States;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.timecrafters.TeleOp.states.PhoenixBot1;
+
+public class JunctionAllignmentDistanceState extends CyberarmState {
+    private final boolean stateDisabled;
+    PhoenixBot1 robot;
+    private final double drivePower;
+    private int loopsTotal;
+    private float slop;
+    private double checkTime = 250;
+    private int traveledDistance;
+    private int loopsCurrent;
+    private double minDistance;
+    private double maxDistance;
+    private String whereAmI = "init";
+    private int driveLoops;
+    private boolean finishedEnabled;
+    double leftDistance;
+    double rightDistance;
+    boolean rightInRange;
+    boolean leftInRange;
+
+
+    public JunctionAllignmentDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
+        this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
+        this.slop = robot.configuration.variable(groupName, actionName, "slop").value();
+        this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveled distance").value();
+        this.minDistance = robot.configuration.variable(groupName, actionName, "minDistance").value();
+        this.maxDistance = robot.configuration.variable(groupName, actionName, "maxDistance").value();
+        this.finishedEnabled = robot.configuration.variable(groupName, actionName, "finishedEnabled").value();
+
+
+        this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("right sensor distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("left sensor distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
+        engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
+        engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
+        engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
+        engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
+        engine.telemetry.addData("drive power", drivePower);
+        engine.telemetry.addData("traveled distance", traveledDistance);
+        engine.telemetry.addData("Loops", loopsCurrent);
+        engine.telemetry.addData("where am i??", whereAmI);
+        engine.telemetry.addData("time", System.currentTimeMillis() - checkTime);
+
+
+    }
+
+    @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.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);
+
+        checkTime = System.currentTimeMillis() - 500;
+
+
+        loopsCurrent = 0;
+
+    }
+
+    @Override
+    public void exec() {
+
+
+        if (stateDisabled) {
+            setHasFinished(true);
+        } else {
+
+            if (loopsCurrent >= loopsTotal ) {
+
+                whereAmI = "exceeded loops";
+
+                robot.backLeftDrive.setPower(0);
+                robot.backRightDrive.setPower(0);
+                robot.frontLeftDrive.setPower(0);
+                robot.frontRightDrive.setPower(0);
+                setHasFinished(finishedEnabled);
+
+            } else if (System.currentTimeMillis() - checkTime >= 100) {
+
+                checkTime = System.currentTimeMillis();
+
+                loopsCurrent += 1;
+
+                leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
+                rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
+                rightInRange = (rightDistance >= minDistance) && (rightDistance <= maxDistance);
+                leftInRange = (leftDistance >= minDistance) && (leftDistance <= maxDistance);
+
+                if (rightInRange || leftInRange) {
+
+                    whereAmI = "in range";
+
+                    robot.backLeftDrive.setPower(0);
+                    robot.backRightDrive.setPower(0);
+                    robot.frontLeftDrive.setPower(0);
+                    robot.frontRightDrive.setPower(0);
+                    setHasFinished(finishedEnabled);
+
+                }
+
+                else if (!rightInRange && !leftInRange) {
+
+                    whereAmI = "Out of range";
+
+//                    if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
+                        robot.backLeftDrive.setPower(drivePower);
+                        robot.backRightDrive.setPower(drivePower);
+                        robot.frontLeftDrive.setPower(drivePower);
+                        robot.frontRightDrive.setPower(drivePower);
+
+//                    }
+
+                }
+
+
+
+
+            }
+        }
+    }
+}