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/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java index 9045baa..b2de7a9 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -17,6 +17,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; import org.openftc.easyopencv.OpenCvCameraRotation; @@ -51,7 +52,9 @@ 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.03, 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 double xMultiplier = 1; public double yMultiplier = 1; public double positionX = 1000; @@ -60,6 +63,8 @@ public class CompetitionRobotV1 extends Robot { public double xTarget = 1000; public double yTarget = 1000; public double hTarget = 0; + public double targetVelocityX = 0; + public double targetVelocityY = 0; public int currentRightPosition = 0; public int currentLeftPosition = 0; @@ -75,14 +80,20 @@ public class CompetitionRobotV1 extends Robot { // heading math variables for pid with imu public double headingIntegralSum = 0; public double xIntegralSum = 0; + public double xVeloIntegralSum = 0; public double yIntegralSum = 0; + public double yVeloIntegralSum = 0; public final double cm_per_tick = (2 * Math.PI * R) / N; private double headingLastError = 0; private double xLastError = 0; + private double xVeloLastError = 0; private double yLastError = 0; + private double yVeloLastError = 0; ElapsedTime headingTimer = new ElapsedTime(); ElapsedTime xTimer = new ElapsedTime(); ElapsedTime yTimer = new ElapsedTime(); + ElapsedTime xVeloTimer = new ElapsedTime(); + ElapsedTime yVeloTimer = new ElapsedTime(); public double frontLeftPower; public double backLeftPower; @@ -96,6 +107,11 @@ public class CompetitionRobotV1 extends Robot { public double rawPidX; public double rawPidY; + public double xVelocity; + public double yVelocity; + public double deltaTime; + + //-------------------------------------------------------------------------------------------------------------- arm sequence variables: PIDController pidController; public double power; @@ -211,6 +227,17 @@ public class CompetitionRobotV1 extends Robot { // -------------------------------------------------------------------------------------------------------------------------- Functions: + public void velocityChecker(){ + long startTime = System.currentTimeMillis(); + double oldXpos = positionX; + double oldYpos = positionX; + + xVelocity = (oldXpos - positionX) / deltaTime; + yVelocity = (oldYpos - positionY) / deltaTime; + + deltaTime = startTime - System.currentTimeMillis(); + + } public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer: // update positions @@ -266,6 +293,27 @@ public class CompetitionRobotV1 extends Robot { double output = (error * Hp) + (derivative * Hd) + (headingIntegralSum * Hi); return output; } + public double XVeloPIDControl ( double reference, double current){ + double error = (reference - current); + xVeloIntegralSum += error * xVeloTimer.seconds(); + double derivative = (error - xVeloLastError) / xVeloTimer.seconds(); + + xTimer.reset(); + + double output = (error * xvp) + (derivative * xvd) + (xIntegralSum * xvi); + return output; + } + + public double YVeloPIDControl ( double reference, double current){ + double error = (reference - current); + yVeloIntegralSum += error * yVeloTimer.seconds(); + double derivative = (error - yVeloLastError) / xTimer.seconds(); + + xTimer.reset(); + + double output = (error * yvp) + (derivative * yvd) + (yVeloIntegralSum * yvi); + return output; + } public double XPIDControl ( double reference, double current){ double error = (reference - current); @@ -322,6 +370,8 @@ public class CompetitionRobotV1 extends Robot { public void DriveToCoordinates () { // determine the powers needed for each direction // this uses PID to adjust needed Power for robot to move to target + XVeloPIDControl(targetVelocityX, xVelocity); + XVeloPIDControl(targetVelocityY, yVelocity); double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); 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);