diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java new file mode 100644 index 0000000..e82cce1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java @@ -0,0 +1,92 @@ +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 BackDrop red", preselectTeleOp = "Competition V1 TeleOp") + +public class StateBackDropRed 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", "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")); + + // pause + addState(new ClawArmState(robot,"State BackDrop red", "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")); + + // pause + addState(new ClawArmState(robot,"State BackDrop red", "6-00-1")); + + //open right close left + addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0")); + // bring arm up + addState(new ClawArmState(robot,"State BackDrop red", "8-00-0")); + + + + + + + + + } + +} 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 c2c8994..7b716d7 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 @@ -54,7 +54,6 @@ public class CameraVisionState extends CyberarmState { @Override public void exec() { - robot.clawArmControl(); if (System.currentTimeMillis() - initTime > 4000){ robot.objectPos = pipeline.objectPos(); setHasFinished(true); @@ -84,6 +83,7 @@ public class CameraVisionState extends CyberarmState { Mat leftCrop; Mat middleCrop; Mat rightCrop; + Mat rotatedMat = new Mat(); double leftavgfin; double middleavgfin; double rightavgfin; @@ -91,13 +91,14 @@ public class CameraVisionState extends CyberarmState { Scalar rectColor = new Scalar(255.0, 0.0, 0.0); public Mat processFrame(Mat input) { - Imgproc.cvtColor(input, HSV, Imgproc.COLOR_RGB2HSV); + Core.rotate(input, rotatedMat,Core.ROTATE_180); + Imgproc.cvtColor(rotatedMat, HSV, Imgproc.COLOR_RGB2HSV); Rect leftRect = new Rect(1, 1, 212, 359); Rect rightRect = new Rect(213, 1, 212, 359); Rect middleRect = new Rect(426, 1, 212, 359); - input.copyTo(output); + rotatedMat.copyTo(output); Imgproc.rectangle(output, leftRect, rectColor, 2); Imgproc.rectangle(output, rightRect, rectColor, 2); Imgproc.rectangle(output, middleRect, rectColor, 2); 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 5a9c504..38cf137 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 @@ -12,13 +12,9 @@ import dev.cyberarm.engine.V2.CyberarmState; public class DriveToCoordinatesState extends CyberarmState { CompetitionRobotV1 robot; - public static double xTarget; - public static double yTarget; - public static double hTarget; - public static double maxVelocityX; - public static double maxVelocityY; - public double maxVelocityH; - public boolean posAchieved = false; + public double xTarget; + public double yTarget; + public double hTarget; public boolean armDrive; public int objectPos; public boolean posSpecific; @@ -43,7 +39,7 @@ public class DriveToCoordinatesState extends CyberarmState { @Override public void start() { super.start(); - if (posSpecific) { + if (posSpecific && objectPos == robot.objectPos) { robot.hTarget = hTarget; robot.yTarget = yTarget; robot.xTarget = xTarget; @@ -60,16 +56,16 @@ public class DriveToCoordinatesState extends CyberarmState { @Override public void exec() { - if (robot.xVelocity > maxVelocityX){ - maxVelocityX = robot.xVelocity; - } - if (robot.yVelocity > maxVelocityY){ - maxVelocityY = robot.yVelocity; - } +// robot.yMaxPower = maxYPower; +// robot.xMaxPower = maxXPower; +// robot.hTarget = hTarget; +// robot.yTarget = yTarget; +// robot.xTarget = xTarget; + if (posSpecific) { if (objectPos != robot.objectPos) { // enter the ending loop -// setHasFinished(true); + setHasFinished(true); } else { if (armDrive) { @@ -78,7 +74,7 @@ public class DriveToCoordinatesState extends CyberarmState { if (Math.abs(robot.positionX - robot.xTarget) < 5 && Math.abs(robot.positionY - robot.yTarget) < 5) { -// setHasFinished(true); + setHasFinished(true); } } } else { @@ -88,15 +84,13 @@ public class DriveToCoordinatesState extends CyberarmState { if (Math.abs(robot.positionX - robot.xTarget) < 5 && Math.abs(robot.positionY - robot.yTarget) < 5) { -// setHasFinished(true); + setHasFinished(true); } } } @Override public void telemetry() { - engine.telemetry.addData("x velocity max", maxVelocityX); - engine.telemetry.addData("y velocity max", maxVelocityY); engine.telemetry.addData("x pos", robot.positionX); engine.telemetry.addData("y pos", robot.positionY); engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java index 5a4c0fe..bf5ce25 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -51,10 +51,8 @@ public class CompetitionRobotV1 extends Robot { // ----------------------------------------------------------------------------------------------------------------- odometry variables: public static double Hp = 0.8, Hi = 0, Hd = 0; - public static double Xp = -0.03, Xi = 0, Xd = 0; - public static double xvp = -0, xvi = 0, xvd = 0; - public static double Yp = 0.03, Yi = 0, Yd = 0; - public static double yvp = 0.03, yvi = 0, yvd = 0; + public static double Xp = 0.03, Xi = 0, Xd = 0; + public static double Yp = -0.03, Yi = 0, Yd = 0; public double Dnl1; public double Dnr2; @@ -76,7 +74,7 @@ public class CompetitionRobotV1 extends Robot { public int oldLeftPosition = 0; public int oldAuxPosition = 0; public final static double L = 20.9; // distance between left and right encoder in cm - final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm + final static double B = 12.6; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm public final static double R = 3; // wheel radius in cm final static double N = 8192; // encoder ticks per revolution (REV encoder) @@ -118,14 +116,14 @@ public class CompetitionRobotV1 extends Robot { //-------------------------------------------------------------------------------------------------------------- arm sequence variables: PIDController pidController; public double power; - public String armPos; + public String armPos = "hover"; public long armTime; public int target; public double p = 0.007, i = 0, d = 0.0001, f = 0; public double shoulderCollect = 0.86; - public double shoulderDeposit = 0.86; + public double shoulderDeposit = 1; public double shoulderPassive = 1; public double elbowCollect = 0.02; public double elbowDeposit = 0; @@ -393,7 +391,7 @@ public class CompetitionRobotV1 extends Robot { if (Objects.equals(armPos, "deposit")) { shoulder.setPosition(shoulderDeposit); elbow.setPosition(elbowDeposit); - target = 400; + target = 300; } @@ -407,8 +405,8 @@ public class CompetitionRobotV1 extends Robot { } if (armPos.equals("search")) { - shoulder.setPosition(0.48); - if (armTime > 400){ + shoulder.setPosition(0.55); + if (armTime > 450){ target = 570; } 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 a324562..dd496aa 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 @@ -34,8 +34,8 @@ public class CompetitionTeleOpState extends CyberarmState { // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; private double targetHeading; - public double collectLock = Math.toRadians(90); - public double backDropLock = Math.toRadians(-90); + public double collectLock = Math.toRadians(-90); + public double backDropLock = Math.toRadians(90); public double boost; public double armPower; private double currentHeading;