diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java index d68b0e9..ee034d2 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java +++ b/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")); - -// // driving forward to low junction -// addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0")); -// // rotate left towards low junction +//// addState(new TopArmv2(robot, "Right State Auto", "06-0")); +// +//// // forward to low junction ..........................................................................(I have PreLoaded Cone) +//// addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0")); +// +// // 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 +// +// // counteract distance driven .........................................................................(I'm going for 2nd cone) // addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0")); -// // rotate towards opposing alliance -// addState(new RotationState(robot, "Right State Auto", "04-1")); -// // drive to stack column -// addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0")); -// // rotate at stack -// addState(new RotationState(robot, "Right State Auto", "05-1")); -// // drive at stack +// +// // rotate towards opposing alliance (angle = 0, direction = CW) (I'm going for 2nd cone) +// addState(new RotationState(robot, "Right State Auto", "05-0")); +// +// // drive to tall junction (to adjust to cone stack) (I'm going for 2nd cone) // 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")); + + + diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index e57ef7b..2673dbc 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/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 Distance", currentDistance); + engine.telemetry.addData("Current Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM)); 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,60 +130,41 @@ 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) { - // ramping up - drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.15; - } else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) { - // ramping down - drivePower = ((delta / RampDownDistance) + 0.15); - } else { - // middle ground - drivePower = targetDrivePower; - } + robot.backLeftDrive.setPower(targetDrivePower); + robot.backRightDrive.setPower(targetDrivePower); + robot.frontLeftDrive.setPower(targetDrivePower); + robot.frontRightDrive.setPower(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); - robot.backLeftDrive.setPower(0); - - if (!inRange){ - inRange = true; - inRangeTime = runTime(); } else { - 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); + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + if (!inRange){ + inRange = true; + inRangeTime = runTime(); + } else { + + 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); + } } - } + + } } } - } - diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index 22d2911..3c556de 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/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 int traveledDistance; + private double drivePower, targetDrivePower, 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("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition()); - engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition()); + engine.telemetry.addData("maximumTolerance", maximumTolerance); engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("Target Achieved", targetAchieved); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java index 0e49e1f..5ad7768 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java +++ b/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);