diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java index 5774812..0349cca 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.DcMotor; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmControlTask; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; @@ -13,7 +14,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Burnsville audience blue") +@Autonomous(name = "Burnsville audience blue", preselectTeleOp = "Competition V1 TeleOp") public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine { @@ -42,15 +43,15 @@ public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine { this.robot.setup(); addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0")); -// addState(new CameraVisionState(robot)); + addState(new CameraVisionState(robot)); addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-1")); // drive to the left, center, or right spike mark + addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-1")); + addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0")); addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-0")); - addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-1")); -// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0")); -// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0")); // place pixel addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1")); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java index ba4b265..c375ef1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceRed.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.DcMotor; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; @@ -12,7 +13,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Burnsville audience red") +@Autonomous(name = "Burnsville audience red", preselectTeleOp = "Competition V1 TeleOp") public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine { @@ -41,15 +42,15 @@ public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine { this.robot.setup(); addState(new ClawArmState(robot,"Burnsville audience red", "0-01-0")); -// addState(new CameraVisionState(robot)); + addState(new CameraVisionState(robot)); addState(new ClawArmState(robot,"Burnsville audience red", "0-01-1")); // drive to the left, center, or right spike mark addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-0")); addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-1")); -// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0")); -// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0")); // place pixel addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-1")); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java index d149e47..4276815 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.DcMotor; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; @@ -13,7 +14,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Burnsville BackDrop blue") +@Autonomous(name = "Burnsville BackDrop blue", preselectTeleOp = "Competition V1 TeleOp") public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine { @@ -42,15 +43,15 @@ public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine { this.robot.setup(); addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0")); -// addState(new CameraVisionState(robot)); + addState(new CameraVisionState(robot)); addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1")); // drive to the left, center, or right spike mark addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0")); addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1")); -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0")); -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0")); addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4")); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java index d72f0c0..fb49c6c 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropRed.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionEngines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.DcMotor; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.CameraVisionState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; @@ -12,7 +13,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Burnsville BackDrop red") +@Autonomous(name = "Burnsville BackDrop red", preselectTeleOp = "Competition V1 TeleOp") public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine { @@ -22,7 +23,6 @@ public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine { @Override public void init() { super.init(); - robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.clawArm.setTargetPosition(0); robot.clawArm.setPower(0); robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -36,19 +36,18 @@ public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine { this.robot = new CompetitionRobotV1("Burnsville BackDrop red"); addTask(new DriveToCoordinatesTask(robot)); addTask(new OdometryLocalizerTask(robot)); -// addTask(new ClawArmControlTask(robot)); this.robot.setup(); addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-0")); -// addState(new CameraVisionState(robot)); + addState(new CameraVisionState(robot)); addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-1")); // drive to the left, center, or right spike mark -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0")); -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1")); -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0")); addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-02-0")); addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-5")); @@ -67,8 +66,8 @@ public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine { addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1")); -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0")); -// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0")); + addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0")); addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0")); addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1")); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java index 0024a00..c2c8994 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/CameraVisionState.java @@ -55,7 +55,7 @@ public class CameraVisionState extends CyberarmState { @Override public void exec() { robot.clawArmControl(); - if (System.currentTimeMillis() - initTime > 3000){ + if (System.currentTimeMillis() - initTime > 4000){ robot.objectPos = pipeline.objectPos(); setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java index d487da0..300d267 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -40,11 +40,15 @@ public class DriveToCoordinatesState extends CyberarmState { @Override public void start() { super.start(); - robot.hTarget = hTarget; - robot.yTarget = yTarget; - robot.xTarget = xTarget; - robot.yMaxPower = maxYPower; - robot.xMaxPower = maxXPower; + if (posSpecific) { + robot.hTarget = hTarget; + robot.yTarget = yTarget; + robot.xTarget = xTarget; + robot.yMaxPower = maxYPower; + robot.xMaxPower = maxXPower; + } else { + setHasFinished(true); + } Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java index 1476885..9520e78 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -257,16 +257,6 @@ public class CompetitionTeleOpState extends CyberarmState { } } - if (Objects.equals(armPos, "passive")) { - if (robot.lift.getCurrentPosition() >= 20) { - robot.chinUpServo.setPosition(chinUpServoDown); - robot.lift.setPower(-0.6); - } else { - robot.shoulder.setPosition(robot.shoulderPassive); - target = 570; - } - } - if (Objects.equals(armPos, "lift up")) { robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowDeposit); @@ -303,8 +293,6 @@ public class CompetitionTeleOpState extends CyberarmState { super.init(); pidController = new PIDController(p, i, d); robot.imu.resetYaw(); - - robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.clawArm.setTargetPosition(0); robot.clawArm.setPower(0); robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);