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 c6ccea2..5cc8001 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java @@ -7,13 +7,8 @@ import org.timecrafters.Autonomous.States.BottomArm; import org.timecrafters.Autonomous.States.CollectorDistanceState; import org.timecrafters.Autonomous.States.CollectorState; import org.timecrafters.Autonomous.States.ConeIdentification; -import org.timecrafters.Autonomous.States.DriverState; import org.timecrafters.Autonomous.States.DriverStateWithOdometer; -import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState; -import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState2nd; -import org.timecrafters.Autonomous.States.DriverStateWithOdometerUpperArmParallelState; import org.timecrafters.Autonomous.States.JunctionAllignmentState; -import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; @@ -39,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")); @@ -50,6 +45,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine { // 6-1 drive forward or backwards if needed this is customizable so we can adapt 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")); + //pause addState(new BottomArm(robot, "RightFourCone", "06-2")); @@ -98,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")); @@ -148,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/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index 545fe1a..00c5539 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -115,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState { distanceDelta = LastDistanceRead - currentDistance; - if (distanceDelta >= -50.0 || currentDistance > 500) { + if (distanceDelta >= -15.0 || currentDistance > 500) { // I am moving forward // and im close too my target. LastDistanceRead = currentDistance; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java deleted file mode 100644 index d4d7805..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState.java +++ /dev/null @@ -1,112 +0,0 @@ -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 DriverStateWithOdometerLowerArmParallelState extends CyberarmState { - private final boolean stateDisabled; - PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; - private int maximumTolerance; - public DriverStateWithOdometerLowerArmParallelState(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.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").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() { - - addParallelState(new BottomArm(robot, "RightFourCone", "06-1")); - 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 - maximumTolerance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); - } 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); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java deleted file mode 100644 index afe9718..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java +++ /dev/null @@ -1,116 +0,0 @@ -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 DriverStateWithOdometerLowerArmParallelState2nd extends CyberarmState { - private final boolean stateDisabled; - PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; - private int maximumTolerance; - public DriverStateWithOdometerLowerArmParallelState2nd(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.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").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.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); - - - - } - - @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 - maximumTolerance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); - } 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); - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java deleted file mode 100644 index a6d4258..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerUpperArmParallelState.java +++ /dev/null @@ -1,144 +0,0 @@ -package org.timecrafters.Autonomous.States; - -import android.util.Log; - -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; - -public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState { - private final boolean stateDisabled; - PhoenixBot1 robot; - private int RampUpDistance; - private int RampDownDistance; - private int maximumTolerance; - public DriverStateWithOdometerUpperArmParallelState(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.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").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); - - addParallelState(new TopArm(robot, "RightFourCone", "04-1")); - - } - - @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 - maximumTolerance){ - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) { - robot.backLeftDrive.setPower(targetDrivePower * -0.25); - robot.backRightDrive.setPower(targetDrivePower * -0.25); - robot.frontLeftDrive.setPower(targetDrivePower * -0.25); - robot.frontRightDrive.setPower(targetDrivePower * -0.25); - } else { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - setHasFinished(true); - } - if (!getHasFinished()){ - float angle = robot.imu.getAngularOrientation().firstAngle; - - if (angle < -0.25){ - robot.backLeftDrive.setPower(drivePower * 0.25); - robot.backRightDrive.setPower(drivePower * 1.25); - robot.frontLeftDrive.setPower(drivePower * 0.25); - robot.frontRightDrive.setPower(drivePower * 1.25); - } - if (angle > 0.25){ - robot.backLeftDrive.setPower(drivePower * 1.25); - robot.backRightDrive.setPower(drivePower * 0.25); - robot.frontLeftDrive.setPower(drivePower * 1.25); - robot.frontRightDrive.setPower(drivePower * 0.25); - } - } - - - } - - @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("frontRightDrive", robot.frontRightDrive.getPower()); - engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower()); - engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower()); - engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower()); - engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition()); - engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle); - engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle); - engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle); - - - 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); - - Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle); - Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle); - Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle); - - } -} 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 86a5e0b..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; @@ -7,15 +9,34 @@ import org.timecrafters.TeleOp.states.PhoenixBot1; public class JunctionAllignmentState extends CyberarmState { private final boolean stateDisabled; PhoenixBot1 robot; - private double TargetSensorDistance; - private final String targetedJunction; private final double drivePower; - private int whatToDo; + private int loopsTotal; + private float rotationAmount; + private float currentAngle; + 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) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); - this.targetedJunction = robot.configuration.variable(groupName, actionName, "targetedJunction").value(); + 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; } @@ -23,133 +44,178 @@ public class JunctionAllignmentState extends CyberarmState { 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("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); + + + + + } + + @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 public void exec() { + currentAngle = robot.imu.getAngularOrientation().firstAngle; + + if (stateDisabled){ setHasFinished(true); } else { double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM); double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM); + boolean rightInRange = rightDistance > minDistance && rightDistance < maxDistance; + boolean leftInRange = leftDistance > minDistance && leftDistance < maxDistance; - // TODO: 12/11/2022 Make sure these are the correct values for the distance from low, mid, and high junctions!!! - switch (targetedJunction) { - case "low": - TargetSensorDistance = 90.0; - break; - case "mid": - TargetSensorDistance = 135.0; - break; - case "high": - TargetSensorDistance = 200.0; - break; - } + // The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0 + if (loopsCurrent >= loopsTotal){ + setHasFinished(finishedEnabled); + } else if (System.currentTimeMillis() - checkTime >= 250 ) { + + whereAmI = "post 250 ms"; + + checkTime = runTime(); + + loopsCurrent = loopsCurrent + 1; + + if (rightInRange && leftInRange){ + setHasFinished(finishedEnabled); + } + + if (rightInRange && !leftInRange) { + + // rotate 1 degree CW + + targetAngle = currentAngle + rotationAmount; + + if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) { + + robot.frontRightDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + + setHasFinished(finishedEnabled); + + } else { + + if (targetAngle - slop < currentAngle) { + + robot.frontRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.backLeftDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + + } else if (targetAngle + slop > currentAngle) { + + robot.frontRightDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.backLeftDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(-drivePower); + + } - // the state is finished if the distance sensors are at the correct distance. - if ((leftDistance > TargetSensorDistance - 2.0 || leftDistance < TargetSensorDistance + 2.0) && (rightDistance > TargetSensorDistance -2.0 || rightDistance < TargetSensorDistance + 2.0)) { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - setHasFinished(true); } -// whatToDo = (int)((leftDistance > TargetSensorDistance) << 1 ) + (int)(rightDistance > TargetSensorDistance); - - switch (whatToDo){ - case 0: // drive back - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); - break; - - case 1: // rotate CW - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - break; - - case 2: // rotate CCW - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - break; - - case 3: // Drive Forward - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - break; - - } - // the robot is lined up but needs to drive forward till the robot is at the specified distance - if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){ - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); + if (!rightInRange && leftInRange) { + + // rotate 1 degree CCW + + targetAngle = currentAngle - rotationAmount; + + if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) { + + robot.frontRightDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + + } else { + + if (targetAngle - slop < currentAngle) { + + robot.frontRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.backLeftDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + + } else if (targetAngle + slop > currentAngle) { + + robot.frontRightDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.backLeftDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(-drivePower); + + } + } - // the robot is lined up but needs to drive backward till the robot is at the specified distance - if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){ - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(-drivePower); + + } + + 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; } - // the robot is going to rotate CCW until a distance is met - if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) { - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - } - // the robot is going to rotate CW until a distance is met - if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) { - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - } - // The right sensor is aligned but the robot must rotate CW with only the left side powered - if (leftDistance < TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) { - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(0); - } - // The right sensor is aligned but the robot must rotate rotate CCW with only the left side powered - if (leftDistance > TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) { - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(0); - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(0); - } - // The left sensor is aligned but the robot must rotate CW with only the right side powered - if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance < TargetSensorDistance) { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(-drivePower); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(-drivePower); - } - // The left sensor is aligned but the robot must rotate CCW with only the right side powered - if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) { - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(drivePower); - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(drivePower); + + } 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; + } + + } + } + } } } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index dc60c74..e5526b3 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -81,6 +81,10 @@ public class RotationState extends CyberarmState { engine.telemetry.addData("Robot IMU Rotation", RobotRotation); engine.telemetry.addData("Robot Target Rotation", targetRotation); engine.telemetry.addData("Drive Power", drivePowerVariable); + 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()); } } 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); - } } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java index 4864041..da5e880 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java @@ -20,8 +20,8 @@ public class PhoenixTeleOPState extends CyberarmState { private double RobotRotation; private double RotationTarget, DeltaRotation; private double MinimalPower = 0.25, topServoOffset = -0.05; - private double servoCollectLow = 0.45; //Low servos, A button - private double servoCollectHigh = 0.55; //High servos, A button + private double servoCollectLow = 0.40; //Low servos, A button + private double servoCollectHigh = 0.40; //High servos, A button private double servoLowLow = 0.5; //Low servos, X button private double servoLowHigh = 0.75; //High servos, X button private double servoMedLow = 0.5; //Low servos, B button