diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java index 9b4d1ee..5cc8001 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -34,8 +34,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0")); - addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1")); - +// addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1")); + addState(new TopArm(robot, "RightFourCone", "04-1")); // 6 Raise lower arm while slowly driving at the junction (parallel state) addState(new BottomArm(robot, "RightFourCone", "05-0")); @@ -96,23 +96,23 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 15 Drive All the way back to the tall Junction and raise upper arm (parallel state) addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0")); // -// // 16 Rotate and use sensor to find junction -// addState(new RotationState(robot, "RightFourCone", "16-0")); -// -// // 17 Drive Towards Junction (This is optional, idk if this is needed atm) -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0")); + // 16 Rotate and use sensor to find junction + addState(new RotationState(robot, "RightFourCone", "16-0")); // + // 17 Drive Towards Junction (This is optional, idk if this is needed atm) + addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0")); + // // 18 Bring upper arm down -// addState(new TopArm(robot, "RightFourCone", "18-0")); + addState(new TopArm(robot, "RightFourCone", "18-0")); // // // 19 Drop cone -// addState(new CollectorState(robot, "RightFourCone", "19-0")); + addState(new CollectorState(robot, "RightFourCone", "19-0")); // // // 20 Bring upper arm up // addState(new TopArm(robot, "RightFourCone", "20-0")); // // // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier) -// addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0")); + addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0")); // // // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state) // addState(new TopArm(robot, "RightFourCone", "22-0")); @@ -146,7 +146,7 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0")); // // // 32 Rotate towards Stack of cones -// addState(new RotationState(robot, "RightFourCone", "32-0")); + addState(new RotationState(robot, "RightFourCone", "32-0")); // // // 33 Decide which path after scanning image from earlier // addState(new PathDecision(robot, "RightFourCone", "33-0")); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java index 3fcb3b6..d12cd54 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java @@ -1,5 +1,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.DistanceUnit; import org.timecrafters.TeleOp.states.PhoenixBot1; @@ -14,7 +16,13 @@ public class JunctionAllignmentState extends CyberarmState { private float targetAngle; private float slop; private double checkTime = 250; - + private int traveledDistance; + private int loopsCurrent = 0; + private double minDistance; + private double maxDistance; + private String whereAmI = "init"; + private int driveLoops; + private boolean finishedEnabled; public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) { @@ -23,6 +31,11 @@ public class JunctionAllignmentState extends CyberarmState { this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value(); this.rotationAmount = robot.configuration.variable(groupName, actionName, "rotationAmount").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; } @@ -38,6 +51,11 @@ public class JunctionAllignmentState extends CyberarmState { engine.telemetry.addData("target angle", targetAngle); engine.telemetry.addData("current angle", currentAngle); 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", runTime() - checkTime); + @@ -48,6 +66,23 @@ public class JunctionAllignmentState extends CyberarmState { @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() - 250; + + driveLoops = 0; + } @Override @@ -62,24 +97,24 @@ public class JunctionAllignmentState extends CyberarmState { double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM); double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM); - boolean rightInRange = rightDistance > 170 && rightDistance < 200; - boolean leftInRange = leftDistance > 170 && leftDistance < 200; + boolean rightInRange = rightDistance > minDistance && rightDistance < maxDistance; + boolean leftInRange = leftDistance > minDistance && leftDistance < maxDistance; // The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0 - if (loopsTotal >= 5){ - setHasFinished(true); - } else if (runTime() - checkTime >= 250 ) { + if (loopsCurrent >= loopsTotal){ + setHasFinished(finishedEnabled); + } else if (System.currentTimeMillis() - checkTime >= 250 ) { + + whereAmI = "post 250 ms"; checkTime = runTime(); - loopsTotal = loopsTotal + 1; - - } else { + loopsCurrent = loopsCurrent + 1; if (rightInRange && leftInRange){ - setHasFinished(true); + setHasFinished(finishedEnabled); } if (rightInRange && !leftInRange) { @@ -95,6 +130,8 @@ public class JunctionAllignmentState extends CyberarmState { robot.backLeftDrive.setPower(0); robot.frontLeftDrive.setPower(0); + setHasFinished(finishedEnabled); + } else { if (targetAngle - slop < currentAngle) { @@ -151,9 +188,31 @@ public class JunctionAllignmentState extends CyberarmState { } - if (!rightInRange && !leftInRange){ + if (!rightInRange && !leftInRange && driveLoops < 2){ + + if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) { + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.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; + } - setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java index 1176455..09ab5d0 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java @@ -73,8 +73,36 @@ public class PhoenixBot1 { setupRobot(); } + private void initVuforia(){ + /* + * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. + */ + int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + parameters.vuforiaLicenseKey = VUFORIA_KEY; + parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + + + // Instantiate the Vuforia engine + vuforia = ClassFactory.getInstance().createVuforia(parameters); + } + + private void initTfod() { + int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + tfodParameters.minResultConfidence = 0.75f; + tfodParameters.isModelTensorFlow2 = true; + tfodParameters.inputSize = 300; + tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); + tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS); + } + private void setupRobot () { + + collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance"); leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance"); @@ -170,34 +198,10 @@ public class PhoenixBot1 { HighRiserLeft.setPosition(0.45); HighRiserRight.setPosition(0.45); - CameraServo.setPosition(0.8); + CameraServo.setPosition(0.775); } - private void initVuforia(){ - /* - * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. - */ - int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); - VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); - parameters.vuforiaLicenseKey = VUFORIA_KEY; - parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); - - - // Instantiate the Vuforia engine - vuforia = ClassFactory.getInstance().createVuforia(parameters); - } - - private void initTfod() { - int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( - "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); - TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId); - tfodParameters.minResultConfidence = 0.75f; - tfodParameters.isModelTensorFlow2 = true; - tfodParameters.inputSize = 300; - tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); - tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS); - } }