From 3b868f10a095dc8a46e2684ce2eceb58272c5028 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Fri, 16 Feb 2024 15:16:11 -0600 Subject: [PATCH] added a blue and red audience side autonomous (it works and has been tested) --- .../CompetitionEngines/StateAudienceBlue.java | 73 +++++++++++++++++++ .../CompetitionEngines/StateAudienceRed.java | 70 ++++++++++++++++++ .../CompetitionEngines/StateBackDropBlue.java | 58 +++++++-------- .../TeleOp/States/CompetitionTeleOpState.java | 16 ++-- 4 files changed, 179 insertions(+), 38 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceBlue.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceRed.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceBlue.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceBlue.java new file mode 100644 index 0000000..cb7bdae --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceBlue.java @@ -0,0 +1,73 @@ +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; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "State Audience blue", preselectTeleOp = "Competition V1 TeleOp") + +public class StateAudienceBlue extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @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); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("State BackDrop red"); + addTask(new DriveToCoordinatesTask(robot)); + addTask(new OdometryLocalizerTask(robot)); + + this.robot.setup(); + + addState(new CameraVisionState(robot)); + + addState(new ClawArmState(robot,"State BackDrop red", "1-00-0")); + + // drive to the left, center, or right spike mark + addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2")); + addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0")); + + // bring arm to hover + addState(new ClawArmState(robot,"State BackDrop red", "3-00-0")); + + //open claw + addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0")); + + addState(new ClawArmState(robot,"State BackDrop red", "9-00-0")); + + + + + + + + + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceRed.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceRed.java new file mode 100644 index 0000000..ca80205 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateAudienceRed.java @@ -0,0 +1,70 @@ +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; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask; +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "State Audience red", preselectTeleOp = "Competition V1 TeleOp") + +public class StateAudienceRed extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @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); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("State BackDrop blue"); + addTask(new DriveToCoordinatesTask(robot)); + addTask(new OdometryLocalizerTask(robot)); + + this.robot.setup(); + + addState(new CameraVisionState(robot)); + + addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0")); + + // drive to the left, center, or right spike mark + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0")); + + // bring arm to hover + addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0")); + + //open claw + addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0")); + + addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0")); + + + + + + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java index c60640d..39f573c 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java @@ -13,7 +13,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp") +@Autonomous(name = "State BackDrop blue", preselectTeleOp = "Competition V1 TeleOp") public class StateBackDropBlue extends CyberarmEngine { @@ -34,7 +34,7 @@ public class StateBackDropBlue extends CyberarmEngine { @Override public void setup() { - this.robot = new CompetitionRobotV1("State BackDrop red"); + this.robot = new CompetitionRobotV1("State BackDrop blue"); addTask(new DriveToCoordinatesTask(robot)); addTask(new OdometryLocalizerTask(robot)); @@ -42,55 +42,55 @@ public class StateBackDropBlue extends CyberarmEngine { addState(new CameraVisionState(robot)); - addState(new ClawArmState(robot,"State BackDrop red", "1-00-0")); + addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0")); // drive to the left, center, or right spike mark - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0")); // bring arm to hover - addState(new ClawArmState(robot,"State BackDrop red", "3-00-0")); + addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0")); //open claw - addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0")); + addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0")); - addState(new ClawArmState(robot,"State BackDrop red", "5-00-0")); + addState(new ClawArmState(robot,"State BackDrop blue", "5-00-0")); // drive towards backboard - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-1")); // pause - addState(new ClawArmState(robot,"State BackDrop red", "6-00-0")); + addState(new ClawArmState(robot,"State BackDrop blue", "6-00-0")); // drive into board - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-2")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-2")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-2")); // pause - addState(new ClawArmState(robot,"State BackDrop red", "6-00-1")); + addState(new ClawArmState(robot,"State BackDrop blue", "6-00-1")); //open right close left - addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0")); + addState(new ClawFingerState(robot,"State BackDrop blue", "7-00-0")); // bring arm up - addState(new ClawArmState(robot,"State BackDrop red", "8-00-0")); + addState(new ClawArmState(robot,"State BackDrop blue", "8-00-0")); // drivw to park - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0")); - addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-01-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-02-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-0")); + addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-1")); - addState(new ClawArmState(robot,"State BackDrop red", "9-00-0")); + addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0")); 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 34ae39c..7d2c371 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 @@ -24,8 +24,8 @@ public class CompetitionTeleOpState extends CyberarmState { public int target = 0; // ---------------------------------------------------------------------------------------------------------------------shoot variables: - public static double releasePos = 0.95; - public static double holdPos = 0.55 ; + public static double releasePos = 0.45; + public static double holdPos = 0.95 ; public double maxVelocityX; @@ -71,7 +71,7 @@ public class CompetitionTeleOpState extends CyberarmState { // ---------------------------------------------------------------------------------------------------------------Arm control Variables: public String armPos = "collect"; // chin up servo - public static double chinUpServoUp = 0.7; + public static double chinUpServoUp = 0.55; public static double chinUpServoDown = 0; public long lastExecutedTime; @@ -299,9 +299,8 @@ public class CompetitionTeleOpState extends CyberarmState { } if (Objects.equals(armPos, "lift up")) { - robot.shoulder.setPosition(robot.shoulderCollect); - robot.elbow.setPosition(robot.elbowDeposit); - target = 120; + robot.shoulder.setPosition(0.2); + target = 850; robot.chinUpServo.setPosition(chinUpServoUp); } @@ -311,9 +310,8 @@ public class CompetitionTeleOpState extends CyberarmState { robot.chinUpServo.setPosition(chinUpServoDown); } else { robot.lift.setPower(0); - robot.chinUpServo.setPosition(chinUpServoDown); - robot.shoulder.setPosition(robot.shoulderCollect); - target = 120; + robot.shoulder.setPosition(0.2); + target = 850; } }