From 93abf54ee32f8d2231f7ce46e8ace983e7997a5e Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 4 Feb 2023 19:06:38 -0600 Subject: [PATCH] Autonomous work for driving straight, And rotation --- .../Engines/LeftSideAutonomousEngine.java | 2 +- .../Engines/LeftTwoConeAutonomousEngine.java | 2 +- .../Engines/RightSideAutonomousEngine.java | 2 +- .../Engines/RightStateAutoEngine.java | 49 +-- .../Engines/RightTwoConeAutonomousEngine.java | 2 +- .../Autonomous/States/BottomArm.java | 2 +- .../States/CollectorDistanceState.java | 2 +- .../Autonomous/States/CollectorState.java | 2 +- .../Autonomous/States/ConeIdentification.java | 2 +- .../States/DriverParkPlaceState.java | 2 +- .../Autonomous/States/DriverState.java | 2 +- .../States/DriverStateWithOdometer.java | 359 ++++++++++++------ .../States/JunctionAllignmentAngleState.java | 2 +- .../JunctionAllignmentDistanceState.java | 4 +- .../Autonomous/States/PathDecision.java | 2 +- .../Autonomous/States/RotationState.java | 113 ++++-- .../Autonomous/States/ServoCameraRotate.java | 2 +- .../Autonomous/States/TopArm.java | 2 +- .../TeleOp/engine/CameraTestEngine.java | 2 +- .../TeleOp/engine/DriveDebugEngine.java | 4 +- .../TeleOp/engine/DynamicSetupEngine.java | 2 +- .../TeleOp/engine/PhoenixTeleOP.java | 9 +- .../TeleOp/engine/PhoenixWingEngine.java | 2 +- .../TeleOp/engine/SodiEngine.java | 4 +- .../TeleOp/engine/SpeedrunEngine.java | 6 +- .../TeleOp/engine/SterringDriveEngine.java | 7 +- .../TeleOp/states/CameraTestCommon.java | 2 +- .../TeleOp/states/CameraTestState.java | 2 +- .../TeleOp/states/DynamicSetupState.java | 4 +- .../TeleOp/states/LaserServoHeight.java | 2 +- .../TeleOp/states/LaserState.java | 2 +- .../TeleOp/states/PhoenixBot1.java | 11 +- .../TeleOp/states/PhoenixTeleOPState.java | 4 +- .../TeleOp/states/PhoenixTeleOPv2.java | 2 +- .../TeleOp/states/PhoenixWingState.java | 2 +- .../TeleOp/states/SodiLEDState.java | 2 +- .../TeleOp/states/SodiState.java | 2 +- .../states/SteeringDriveExperiment.java | 2 +- .../TeleOp/states/TeleOPArmDriver.java | 4 +- .../TeleOp/states/TeleOPSpeedrunState.java | 2 +- .../TeleOp/states/TeleOPTankDriver.java | 3 +- 41 files changed, 391 insertions(+), 244 deletions(-) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/CameraTestEngine.java (75%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/DriveDebugEngine.java (65%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/DynamicSetupEngine.java (94%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/PhoenixTeleOP.java (61%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/PhoenixWingEngine.java (93%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/SodiEngine.java (78%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/SpeedrunEngine.java (89%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/engine/SterringDriveEngine.java (71%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/CameraTestCommon.java (97%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/CameraTestState.java (74%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/DynamicSetupState.java (85%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/LaserServoHeight.java (96%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/LaserState.java (97%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/PhoenixBot1.java (95%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/PhoenixTeleOPState.java (99%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/PhoenixTeleOPv2.java (96%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/PhoenixWingState.java (74%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/SodiLEDState.java (96%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/SodiState.java (96%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/SteeringDriveExperiment.java (97%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/TeleOPArmDriver.java (98%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/TeleOPSpeedrunState.java (96%) rename TeamCode/src/main/java/org/timecrafters/{ => Autonomous}/TeleOp/states/TeleOPTankDriver.java (98%) diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java index d4e0451..eee529c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -14,7 +14,7 @@ import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; @Autonomous (name = "Left Side") @Disabled diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java index bab0c18..0307501 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java @@ -15,7 +15,7 @@ import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; @Autonomous (name = "left 2 cone auto") diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index f956beb..2b61508 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -13,7 +13,7 @@ import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; @Autonomous (name = "Right ") @Disabled 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 d9de5b5..c29d2c8 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java @@ -3,18 +3,9 @@ package org.timecrafters.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.cyberarm.engine.V2.CyberarmEngine; -import org.cyberarm.engine.V2.CyberarmEngine; -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.DriverParkPlaceState; import org.timecrafters.Autonomous.States.DriverStateWithOdometer; -import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; -import org.timecrafters.Autonomous.States.ServoCameraRotate; -import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; @Autonomous(name = "Right Side") public class RightStateAutoEngine extends CyberarmEngine { @@ -23,22 +14,32 @@ public class RightStateAutoEngine extends CyberarmEngine { @Override public void setup() { robot = new PhoenixBot1(this); + robot.imu.resetYaw(); - // driving towards Low +// setupFromConfig( +// robot.configuration, +// "org.timecrafters.Autonomous.States", +// robot, +// PhoenixBot1.class, +// "Right State Auto"); + +// // driving forward to low junction addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0")); - // rotate towards low - addState(new RotationState(robot, "Right State Auto", "02-1")); - // drive forwards or backwards to adjust to pole - addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0")); - // 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")); - // 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 left towards low junction +// addState(new RotationState(robot, "Right State Auto", "02-1")); +// // driving forward/ backwards to adjust +// 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 +// 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 // addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0")); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java index 9d16545..8ed808c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java @@ -15,7 +15,7 @@ import org.timecrafters.Autonomous.States.PathDecision; import org.timecrafters.Autonomous.States.RotationState; import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; @Autonomous (name = "Right 2 cone auto") diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java index 0975501..e5dfcfa 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -1,7 +1,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class BottomArm extends CyberarmState { 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 eab12b1..e57ef7b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -4,7 +4,7 @@ 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; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class CollectorDistanceState extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java index 9ff8ae3..36f88b9 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java @@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class CollectorState extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java index dc64fc8..892f409 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; import java.util.List; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java index 92b3cdf..8bb3f6b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -3,7 +3,7 @@ package org.timecrafters.Autonomous.States; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class DriverParkPlaceState extends CyberarmState { private final boolean stateDisabled; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index ace0d85..183128c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -3,7 +3,7 @@ package org.timecrafters.Autonomous.States; import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class DriverState extends CyberarmState { private final boolean stateDisabled; 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 c7a94f5..08a5c47 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -1,12 +1,10 @@ package org.timecrafters.Autonomous.States; -import android.util.Log; - import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class DriverStateWithOdometer extends CyberarmState { private final boolean stateDisabled; @@ -16,10 +14,20 @@ public class DriverStateWithOdometer extends CyberarmState { private double maximumTolerance; private float direction; private boolean targetAchieved = false; - private double CurrentPosition; public final double WHEEL_CIRCUMFERENCE = 7.42108499; public final int COUNTS_PER_REVOLUTION = 8192; - public final double distanceMultiplier; + public double startOfRampUpRight; + public double startOfRampDownRight; + public double startOfRampUpLeft; + public double startOfRampDownLeft; + public double endOfRampUpRight; + public double endOfRampDownRight; + public double endOfRampUpLeft; + public double endOfRampDownLeft; + public int driveStage; + public float currentAngle; + public double currentHorizontalEncoder; + public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { this.robot = robot; this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); @@ -28,10 +36,10 @@ public class DriverStateWithOdometer extends CyberarmState { this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value(); this.direction = robot.configuration.variable(groupName, actionName, "direction").value(); - this.distanceMultiplier = robot.configuration.variable(groupName, actionName, "distanceMultiplier").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } + private double drivePower, targetDrivePower; private int traveledDistance; @@ -44,143 +52,257 @@ public class DriverStateWithOdometer extends CyberarmState { 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.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier); - RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier); - RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier); - maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier); + traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER); + if (targetDrivePower > 0) { + startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition(); + endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance; + startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance; + endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance; + + startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition(); + endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance; + startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance; + endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance; + + } else { + + startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition(); + endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance; + startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance; + endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance; + + startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition(); + endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance; + startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance; + endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance; + + } + + driveStage = 0; } @Override public void exec() { + if (stateDisabled) { setHasFinished(true); return; } - double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition()); - double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition()); + double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition(); + double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition(); - if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition; - if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition; + // Driving Forward + if (targetDrivePower > 0 && driveStage == 0) { - - if (Math.abs(CurrentPosition) <= RampUpDistance){ // ramping up -// double ratio = (Math.abs(CurrentPosition) / RampUpDistance); - if (targetDrivePower > 0) { - drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25; - } else { - drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25; - } - } - else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){ - // ramping down - if (targetDrivePower > 0){ - drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25); - } else { - drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25); + if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) || + (LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) { + + drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER; + } - } else { - // middle ground - drivePower = targetDrivePower; - } + // Driving Normal + else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) || + (LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) { - if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ - // This is limiting drive power to the targeted drive power - drivePower = targetDrivePower; - } + drivePower = targetDrivePower; - if (targetDrivePower < 0 && drivePower > 0) { - drivePower = drivePower * -1; - } - - if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){ - if (targetAchieved) { - drivePower = drivePower * 0.25; - - if (Math.abs(drivePower) < 0.25){ - if (drivePower < 0) { - drivePower = -0.25; - } else { - drivePower = 0.25; - } - } - } - robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); - robot.frontRightDrive.setPower(drivePower); - - } - else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) { - targetAchieved = true; - - drivePower = targetDrivePower * -0.25; - - if (Math.abs(drivePower) < 0.25){ - if (drivePower < 0) { - drivePower = -0.25; - } else { - drivePower = 0.25; - } } - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(drivePower); + // Ramping down going forward + else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) || + (LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) { + drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER; - - } - - else { - - if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){ - - if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) { - drivePower = 0; - } else { - drivePower = 0.25; - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); - } - } - - if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){ - - if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){ - drivePower = 0; - } else { - drivePower = 0.25; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - else { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); + } else if (driveStage == 0){ + driveStage = 1; robot.frontRightDrive.setPower(0); - setHasFinished(true); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); } } -// + // Driving Backwards .................................................................................................................................Backwards + if (targetDrivePower < 0 && driveStage == 0) { + + // ramping up + if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) || + (LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) { + + drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER; + + } + + // Driving Normal + else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) || + (LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) { + + drivePower = targetDrivePower; + + } + + // Ramping down going backward + else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) || + (LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) { + + drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) * + (Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER; + } else if (driveStage == 0){ + driveStage = 1; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + } + + // end of ramp down } + // Forwards distance adjust...............................................................................................................................STAGE 1 + if (driveStage == 1 && targetDrivePower > 0) { + + if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) && + RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) { + + drivePower = robot.DRIVETRAIN_MINIMUM_POWER; + + } else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) && + RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) { + + drivePower = -robot.DRIVETRAIN_MINIMUM_POWER; + + } else { + driveStage = 2; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + } + } + + // backwards distance adjust + if (driveStage == 1 && targetDrivePower < 0) { + + if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) && + RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) { + + drivePower = -robot.DRIVETRAIN_MINIMUM_POWER; + + } else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) && + RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) { + + drivePower = robot.DRIVETRAIN_MINIMUM_POWER; + + } else { + driveStage = 2; + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + } + } + + if (driveStage == 0 || driveStage == 1) { + robot.frontRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); + robot.backRightDrive.setPower(drivePower); + robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL); + } + // Heading adjustment + if (driveStage == 2 || driveStage == 4) { + + currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + if (currentAngle - direction > robot.ROTATION_TOLERANCE) { + + robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); + robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + + } + else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) { + + robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + + } else { + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + driveStage ++; + } + } + + // .................................................................................................................................................Strafe Adjustment + if ( driveStage == 3 ){ + + currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition(); + if (currentHorizontalEncoder > 200){ + + robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + + } + else if (currentHorizontalEncoder < -200){ + + robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER ); + robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER ); + + } else { + + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + driveStage = 4; + + } + } + + if (driveStage == 5) { +// setHasFinished(true); + } + } + @Override - public void telemetry() { + public void telemetry () { + engine.telemetry.addData("Stage", driveStage); + engine.telemetry.addData("maximumTolerance", maximumTolerance); + engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight); + engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight); + engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight); + engine.telemetry.addData("endOfRampDownRight", endOfRampDownRight); + engine.telemetry.addData("startOfRampUpLeft", startOfRampUpLeft); + engine.telemetry.addData("endOfRampUpLeft", endOfRampUpLeft); + engine.telemetry.addData("startOfRampDownLeft", startOfRampDownLeft); + engine.telemetry.addData("endOfRampDownLeft", endOfRampDownLeft); engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); @@ -189,15 +311,13 @@ 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("Odometer", robot.OdometerEncoderRight.getCurrentPosition()); - engine.telemetry.addData("imu yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - engine.telemetry.addData("imu pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)); - engine.telemetry.addData("imu roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)); - + engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition()); + 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); - engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("targetDrivePower", targetDrivePower); @@ -205,9 +325,6 @@ public class DriverStateWithOdometer extends CyberarmState { engine.telemetry.addData("RampUpDistance", RampUpDistance); engine.telemetry.addData("RampDownDistance", RampDownDistance); - Log.i("TELEMETRY", "imu yaw:: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - Log.i("TELEMETRY", "imu pitch:: " + robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)); - Log.i("TELEMETRY", "imu roll:: " + robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java index 21a7af8..d33a641 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java @@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class JunctionAllignmentAngleState extends CyberarmState { private final boolean stateDisabled; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java index c3b680a..1dbf266 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java @@ -1,10 +1,8 @@ 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; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class JunctionAllignmentDistanceState extends CyberarmState { private final boolean stateDisabled; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java index 87923bf..45bf43b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -1,7 +1,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class PathDecision extends CyberarmState { PhoenixBot1 robot; 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 12749d4..cb1ae2b 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class RotationState extends CyberarmState { private final boolean stateDisabled; @@ -11,7 +11,7 @@ public class RotationState extends CyberarmState { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value(); - drivePowerVariable = drivePower; + this.ClockWiseRotation = robot.configuration.variable(groupName, actionName, "ClockWiseRotation").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -19,69 +19,99 @@ public class RotationState extends CyberarmState { private double drivePower; private float targetRotation; - float RobotRotation; - private double RotationTarget; - private double RotationDirectionMinimum; + float CurrentRotation; private String debugStatus = "?"; private double drivePowerVariable; private double leftCompensator; private double RightCompensator; + private boolean ClockWiseRotation; + private int RotationStage; + private double rotationDirection; + private long lastStepTime = 0; @Override public void start() { leftCompensator = robot.OdometerEncoderLeft.getCurrentPosition(); RightCompensator = robot.OdometerEncoderRight.getCurrentPosition(); + + RotationStage = 0; } @Override public void exec() { - if (stateDisabled){ + if (stateDisabled) { setHasFinished(true); return; - } // end of if + } // - RobotRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){ - drivePowerVariable = 0.4 * drivePower; - if (Math.abs(drivePowerVariable) < 0.4) { - if (drivePowerVariable < 0){ - drivePowerVariable = -0.4; - } else { - drivePowerVariable = 0.4; - } + CurrentRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + if (RotationStage == 0) { + + drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER; + + if (ClockWiseRotation) { rotationDirection = 1;} else { rotationDirection = -1;} + + + robot.backLeftDrive.setPower( drivePowerVariable * rotationDirection ); + robot.backRightDrive.setPower( -drivePowerVariable * rotationDirection ); + robot.frontLeftDrive.setPower( drivePowerVariable * rotationDirection ); + robot.frontRightDrive.setPower( -drivePowerVariable * rotationDirection ); + + if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE && + (RotationStage == 0) && + (CurrentRotation - targetRotation <= robot.ROTATION_TOLERANCE)) { + RotationStage = 1; + lastStepTime = System.currentTimeMillis(); + } + } + + if (RotationStage == 1){ + robot.backLeftDrive.setPower( 0 ); + robot.backRightDrive.setPower( 0 ); + robot.frontLeftDrive.setPower( 0 ); + robot.frontRightDrive.setPower( 0 ); + if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION ){ + RotationStage = 2; } - debugStatus = "Rotate Slow"; - } // end of if - else { - drivePowerVariable = drivePower * 0.75; - debugStatus = "Rotate Fast"; } - if (RobotRotation >= targetRotation + 1){ - drivePowerVariable = Math.abs(drivePowerVariable); - } else { - drivePowerVariable = -1 * Math.abs(drivePowerVariable); + if (RotationStage == 2) { + + if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) { + // CW + + robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); + robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER ); + robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER ); + + } + else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) { + // CCW + + robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER); + robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER); + + } else { + robot.frontRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.backLeftDrive.setPower(0); + + RotationStage ++; +// setHasFinished(true); + } } - if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) { - robot.backLeftDrive.setPower(drivePowerVariable); - robot.backRightDrive.setPower(-drivePowerVariable); - robot.frontLeftDrive.setPower(drivePowerVariable); - robot.frontRightDrive.setPower(-drivePowerVariable); - } else - { - robot.backLeftDrive.setPower(0); - robot.backRightDrive.setPower(0); - robot.frontLeftDrive.setPower(0); - robot.frontRightDrive.setPower(0); - PhoenixBot1.leftCompensatorGlobal = (leftCompensator + robot.OdometerEncoderLeft.getCurrentPosition()) - leftCompensator; - PhoenixBot1.RightCompensatorGlobal = (RightCompensator + robot.OdometerEncoderRight.getCurrentPosition()) - RightCompensator; - setHasFinished(true); + } - } + @Override public void telemetry() { @@ -89,13 +119,14 @@ public class RotationState extends CyberarmState { engine.telemetry.addLine(); - engine.telemetry.addData("Robot IMU Rotation", RobotRotation); + engine.telemetry.addData("Robot IMU Rotation", CurrentRotation); 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()); + engine.telemetry.addData("RotationStage", RotationStage); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java index d89327b..ede974a 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java @@ -1,7 +1,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class ServoCameraRotate extends CyberarmState { private final boolean stateDisabled; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java index 081a466..a34cac5 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java @@ -1,7 +1,7 @@ package org.timecrafters.Autonomous.States; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class TopArm extends CyberarmState { diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/CameraTestEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/CameraTestEngine.java similarity index 75% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/CameraTestEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/CameraTestEngine.java index 650d286..d71d94b 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/CameraTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/CameraTestEngine.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import org.cyberarm.engine.V2.CyberarmEngine; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DriveDebugEngine.java similarity index 65% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DriveDebugEngine.java index ac6cec4..4e37154 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DriveDebugEngine.java @@ -1,7 +1,7 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; public class DriveDebugEngine extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DynamicSetupEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DynamicSetupEngine.java similarity index 94% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DynamicSetupEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DynamicSetupEngine.java index 3311e2b..a16d1d1 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DynamicSetupEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DynamicSetupEngine.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixTeleOP.java similarity index 61% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixTeleOP.java index ba9bb76..97c0697 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixTeleOP.java @@ -1,12 +1,11 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.TeleOp.states.PhoenixBot1; -import org.timecrafters.TeleOp.states.PhoenixTeleOPState; -import org.timecrafters.TeleOp.states.TeleOPArmDriver; -import org.timecrafters.TeleOp.states.TeleOPTankDriver; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.TeleOPTankDriver; +import org.timecrafters.Autonomous.TeleOp.states.TeleOPArmDriver; @TeleOp (name = "APhoenixTeleOP") diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixWingEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixWingEngine.java similarity index 93% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixWingEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixWingEngine.java index e842f5e..45b5d71 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixWingEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixWingEngine.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.CRServo; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SodiEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SodiEngine.java similarity index 78% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SodiEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SodiEngine.java index 499b633..7cce9a6 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SodiEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SodiEngine.java @@ -1,10 +1,10 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.TeleOp.states.LaserState; +import org.timecrafters.Autonomous.TeleOp.states.LaserState; @Disabled @TeleOp(name = "Wheel") diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SpeedrunEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SpeedrunEngine.java similarity index 89% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SpeedrunEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SpeedrunEngine.java index 51d22d2..a0c5fba 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SpeedrunEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SpeedrunEngine.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -6,9 +6,9 @@ import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmEngine; import org.cyberarm.engine.V2.GamepadChecker; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; -import org.timecrafters.TeleOp.states.PhoenixBot1; -import org.timecrafters.TeleOp.states.TeleOPSpeedrunState; +import org.timecrafters.Autonomous.TeleOp.states.TeleOPSpeedrunState; @Disabled @TeleOp (name = "Speedrun Engine") diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SterringDriveEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SterringDriveEngine.java similarity index 71% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SterringDriveEngine.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SterringDriveEngine.java index 247fcff..c26bfff 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SterringDriveEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SterringDriveEngine.java @@ -1,12 +1,11 @@ -package org.timecrafters.TeleOp.engine; +package org.timecrafters.Autonomous.TeleOp.engine; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.TeleOp.states.LaserState; -import org.timecrafters.TeleOp.states.PhoenixBot1; -import org.timecrafters.TeleOp.states.SteeringDriveExperiment; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; +import org.timecrafters.Autonomous.TeleOp.states.SteeringDriveExperiment; @Disabled @TeleOp(name = "Steering Drive Test") diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestCommon.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestCommon.java similarity index 97% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestCommon.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestCommon.java index 6cc53f0..0841190 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestCommon.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestCommon.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import org.cyberarm.engine.V2.CyberarmEngine; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestState.java similarity index 74% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestState.java index 604bb06..994e31f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestState.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import org.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/DynamicSetupState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/DynamicSetupState.java similarity index 85% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/DynamicSetupState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/DynamicSetupState.java index d04c96c..c284bb4 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/DynamicSetupState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/DynamicSetupState.java @@ -1,7 +1,7 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.TeleOp.engine.DynamicSetupEngine; +import org.timecrafters.Autonomous.TeleOp.engine.DynamicSetupEngine; public class DynamicSetupState extends CyberarmState { private long delay; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserServoHeight.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserServoHeight.java similarity index 96% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserServoHeight.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserServoHeight.java index 5844f69..1965c1b 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserServoHeight.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserServoHeight.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.robotcore.hardware.Servo; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserState.java similarity index 97% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserState.java index 868dedc..4bc6717 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserState.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java similarity index 95% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java index 603e19d..0e49e1f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor; import com.qualcomm.hardware.bosch.BNO055IMU; @@ -25,7 +25,7 @@ import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm; public class PhoenixBot1 { private static final float mmPerInch = 25.4f; - public static final double WHEEL_CIRCUMFERENCE = 7.42108499; + public static double WHEEL_CIRCUMFERENCE; public static final int COUNTS_PER_REVOLUTION = 8192; public static double leftCompensatorGlobal; public static double RightCompensatorGlobal; @@ -35,6 +35,8 @@ public class PhoenixBot1 { public double STRAFE_MINIMUM_POWER; public double DRIVE_TOLERANCE; public double ROTATION_TOLERANCE; + public long PAUSE_ON_ROTATION; + public double DISTANCE_MULTIPLIER; // private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite"; private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite"; @@ -99,6 +101,9 @@ public class PhoenixBot1 { STRAFE_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "STRAFE_MINIMUM_POWER").value(); DRIVE_TOLERANCE = configuration.variable("Robot", "Tuning", "DRIVE_TOLERANCE").value(); ROTATION_TOLERANCE = configuration.variable("Robot", "Tuning", "ROTATION_TOLERANCE").value(); + 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(); } private void initVuforia(){ @@ -244,6 +249,8 @@ public class PhoenixBot1 { CameraServo.setPosition(0.775); + + } public int AngleToTicks(double angle) { diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPState.java similarity index 99% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPState.java index 8846446..8ebe41d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPState.java @@ -1,9 +1,7 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import android.annotation.SuppressLint; -import android.widget.Switch; -import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPv2.java similarity index 96% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPv2.java index ec67479..eb35a9b 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPv2.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import org.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixWingState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixWingState.java similarity index 74% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixWingState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixWingState.java index 1418adb..bcc76bb 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixWingState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixWingState.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import org.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiLEDState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiLEDState.java similarity index 96% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiLEDState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiLEDState.java index 772bd2c..90c5d49 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiLEDState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiLEDState.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiState.java similarity index 96% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiState.java index 5fde6a5..777409f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiState.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/SteeringDriveExperiment.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SteeringDriveExperiment.java similarity index 97% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/SteeringDriveExperiment.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SteeringDriveExperiment.java index 0f48d57..9876abe 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/SteeringDriveExperiment.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SteeringDriveExperiment.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import org.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java similarity index 98% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java index 8cece68..c7026f2 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java @@ -1,13 +1,11 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.GamepadChecker; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class TeleOPArmDriver extends CyberarmState { private final PhoenixBot1 robot; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPSpeedrunState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPSpeedrunState.java similarity index 96% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPSpeedrunState.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPSpeedrunState.java index af02237..6f526c5 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPSpeedrunState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPSpeedrunState.java @@ -1,4 +1,4 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; import com.qualcomm.robotcore.hardware.DcMotor; diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPTankDriver.java similarity index 98% rename from TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java rename to TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPTankDriver.java index bc0243f..0f0769f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPTankDriver.java @@ -1,6 +1,5 @@ -package org.timecrafters.TeleOp.states; +package org.timecrafters.Autonomous.TeleOp.states; -import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.hardware.Gamepad; import org.cyberarm.engine.V2.CyberarmState;