From 08cfacf2ea2172ebdcb40f349fcf914e160fe28a Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Fri, 29 Dec 2023 22:35:24 -0600 Subject: [PATCH] Finished camera state, fixed memory leak problem. fixed my odometry code so it actually drives the robot again (had to remove max power) need to fix arm controls in autonomous. --- .../Competition2BlueBackStage.java | 58 +++++++ .../CompetitionBlueClose.java | 1 - .../CompetitionRedClose.java | 1 - .../CompetitionStates/CameraVisionState.java | 55 ++++-- .../CompetitionStates/ClawArmState.java | 40 +++-- .../CompetitionStates/DistanceCheckState.java | 69 -------- .../DriveToCoordinatesState.java | 62 ++++--- .../Common/CompetitionRobotV1.java | 65 ++++--- .../TeleOp/States/CompetitionTeleOpState.java | 159 +++++++++++------- 9 files changed, 288 insertions(+), 222 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java new file mode 100644 index 0000000..ee80472 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java @@ -0,0 +1,58 @@ +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.Common.CompetitionRobotV1; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "BURNSVILLE blue backdrop") + +public class Competition2BlueBackStage extends CyberarmEngine { + + CompetitionRobotV1 robot; + + + @Override + public void init() { + super.init(); + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.imu.resetYaw(); + robot.leftClaw.setPosition(0.25); + robot.rightClaw.setPosition(0.6); + } + + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Burnsville backdrop blue"); + this.robot.setup(); + addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "0-00-0")); + addState(new ClawArmState(robot,"burnsville backdrop blue", "0-01-0")); + addState(new CameraVisionState(robot)); + addState(new ClawArmState(robot,"burnsville backdrop blue", "0-02-0")); + + addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-03-0")); + + + + + + + + + + + + + + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java index 734b352..b90e733 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBlueClose.java @@ -5,7 +5,6 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; -import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java index 8784259..b96570b 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionRedClose.java @@ -5,7 +5,6 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState; -import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState; import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; 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 1af6af9..0024a00 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 @@ -17,6 +17,8 @@ public class CameraVisionState extends CyberarmState { CompetitionRobotV1 robot; ExamplePipeline pipeline; + private long initTime; + private boolean finish = false; public CameraVisionState(CompetitionRobotV1 robot) { this.robot = robot; @@ -26,6 +28,7 @@ public class CameraVisionState extends CyberarmState { public void init() { super.init(); + int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); robot.webcam1 = OpenCvCameraFactory.getInstance().createWebcam(robot.webCamName, cameraMonitorViewId); robot.webcam1.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { @@ -42,11 +45,25 @@ public class CameraVisionState extends CyberarmState { } + @Override + public void start() { + super.start(); + initTime = System.currentTimeMillis(); + + } + @Override public void exec() { + robot.clawArmControl(); + if (System.currentTimeMillis() - initTime > 3000){ + robot.objectPos = pipeline.objectPos(); + setHasFinished(true); + } + // odometry driving ALWAYS -// robot.DriveToCoordinates(); -// robot.OdometryLocalizer(); + robot.DriveToCoordinates(); + robot.OdometryLocalizer(); + } @@ -58,10 +75,12 @@ public class CameraVisionState extends CyberarmState { engine.telemetry.addData("middle side pixel amount", pipeline.middleavgfin); engine.telemetry.addData("right side pixel amount", pipeline.rightavgfin); engine.telemetry.addData("object pos", pipeline.objectPos()); + + } class ExamplePipeline extends OpenCvPipeline { - Mat YCbCr = new Mat(); + Mat HSV = new Mat(); Mat leftCrop; Mat middleCrop; Mat rightCrop; @@ -72,7 +91,7 @@ public class CameraVisionState extends CyberarmState { Scalar rectColor = new Scalar(255.0, 0.0, 0.0); public Mat processFrame(Mat input) { - Imgproc.cvtColor(input, YCbCr, Imgproc.COLOR_RGB2YCrCb); + Imgproc.cvtColor(input, HSV, Imgproc.COLOR_RGB2HSV); Rect leftRect = new Rect(1, 1, 212, 359); Rect rightRect = new Rect(213, 1, 212, 359); @@ -83,13 +102,13 @@ public class CameraVisionState extends CyberarmState { Imgproc.rectangle(output, rightRect, rectColor, 2); Imgproc.rectangle(output, middleRect, rectColor, 2); - leftCrop = YCbCr.submat(leftRect); - rightCrop = YCbCr.submat(rightRect); - middleCrop = YCbCr.submat(middleRect); + leftCrop = HSV.submat(leftRect); + rightCrop = HSV.submat(rightRect); + middleCrop = HSV.submat(middleRect); - Core.extractChannel(leftCrop, leftCrop, 2); - Core.extractChannel(middleCrop, middleCrop, 2); - Core.extractChannel(rightCrop, rightCrop, 2); + Core.extractChannel(leftCrop, leftCrop, 1); + Core.extractChannel(middleCrop, middleCrop, 1); + Core.extractChannel(rightCrop, rightCrop, 1); Scalar leftavg = Core.mean(leftCrop); Scalar middleavg = Core.mean(middleCrop); @@ -99,21 +118,29 @@ public class CameraVisionState extends CyberarmState { rightavgfin = rightavg.val[0]; middleavgfin = middleavg.val[0]; + HSV.release(); + leftCrop.release(); + middleCrop.release(); + rightCrop.release(); + return (output); + } - public String objectPos() { + public int objectPos() { if (leftavgfin > rightavgfin && leftavgfin > middleavgfin) { - return "LEFT"; + return 1; } else if (rightavgfin > leftavgfin && rightavgfin > middleavgfin) { - return "RIGHT"; + return 2; } else { - return "MIDDLE"; + return 3; } } } + + } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java index 5838612..ee09469 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.CompetitionStates; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; @@ -12,7 +13,6 @@ public class ClawArmState extends CyberarmState { CompetitionRobotV1 robot; public String armPos; - public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value(); @@ -25,40 +25,38 @@ public class ClawArmState extends CyberarmState { robot.OdometryLocalizer(); // driving arm to pos - if (armPos.equals("collect")){ - if (robot.lift.getCurrentPosition() >= 1){ + if (armPos == "collect") { + if (robot.lift.getCurrentPosition() >= 20) { robot.lift.setPower(-0.6); } else { robot.lift.setPower(0); robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowCollect); - robot.target = 10; + robot.target = 30; } } - if (armPos.equals("passive")){ - if (robot.lift.getCurrentPosition() >= 1){ - robot.lift.setPower(-0.6); - } else { - robot.lift.setPower(0); - robot.shoulder.setPosition(robot.shoulderPassive); - robot.elbow.setPosition(robot.elbowPassive); - robot.target = 850; - } - } - if (armPos.equals("deposit")){ + if (armPos == "deposit") { robot.shoulder.setPosition(robot.shoulderDeposit); robot.elbow.setPosition(robot.elbowDeposit); - robot.target = 370; + robot.target = 400; + } - if (armPos.equals("hover")) { - robot.shoulder.setPosition(robot.shoulderCollect); - robot.elbow.setPosition(robot.elbowCollect); - robot.target = 150; + if (armPos == "hover") { + if (robot.lift.getCurrentPosition() >= 20) { + robot.lift.setPower(-0.6); + } else { + robot.shoulder.setPosition(0.38); + robot.target = 200; + } + + } + if (armPos.equals("search")) { + robot.shoulder.setPosition(0.15); + robot.target = 570; } - robot.armPos = armPos; robot.clawArmControl(); if (robot.power < 0.1) { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java deleted file mode 100644 index 9ce755a..0000000 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DistanceCheckState.java +++ /dev/null @@ -1,69 +0,0 @@ -package org.timecrafters.CenterStage.Autonomous.CompetitionStates; - -import com.acmerobotics.dashboard.config.Config; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.timecrafters.CenterStage.Common.CompetitionRobotV1; - -import dev.cyberarm.engine.V2.CyberarmState; - -@Config -public class DistanceCheckState extends CyberarmState { - - CompetitionRobotV1 robot; - public long lastCheckedTIme = System.currentTimeMillis(); - public String objectPosCheck; - - public DistanceCheckState(CompetitionRobotV1 robot, String groupName, String actionName) { - this.robot = robot; - this.objectPosCheck = robot.configuration.variable(groupName, actionName, "objectPosCheck").value(); - } - - @Override - public void exec() { - // odometry driving ALWAYS - robot.DriveToCoordinates(); - robot.OdometryLocalizer(); - - if (robot.armPos.equals("right") || robot.armPos.equals("middle") || robot.armPos.equals("left")){ - setHasFinished(true); - } else { - if (Math.abs(robot.backLeftPower) < 0.15 && - Math.abs(robot.backRightPower) < 0.15 && - Math.abs(robot.frontLeftPower) < 0.15 && - Math.abs(robot.frontRightPower) < 0.15) { - if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 1000) { - robot.objectPos = objectPosCheck; - setHasFinished(true); - } else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000 && System.currentTimeMillis() - lastCheckedTIme > 1000) { - if (robot.loopCheck == 1 && robot.objectPos != objectPosCheck){ - robot.objectPos = "left"; - setHasFinished(true); - } - robot.loopCheck += 1; - setHasFinished(true); - } - } - } - } - - - @Override - public void telemetry() { - engine.telemetry.addData("x pos", robot.positionX); - engine.telemetry.addData("y pos", robot.positionY); - engine.telemetry.addData("h pos odo", Math.toDegrees(robot.positionH)); - engine.telemetry.addData("aux encoder", robot.currentAuxPosition); - engine.telemetry.addData("left encoder", robot.currentLeftPosition); - engine.telemetry.addData("right encoder", robot.currentRightPosition); - engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); - engine.telemetry.addData("front right power", robot.frontRightPower); - engine.telemetry.addData("front left power", robot.frontLeftPower); - engine.telemetry.addData("back right power", robot.backRightPower); - engine.telemetry.addData("back left power", robot.backLeftPower); - engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition()); - engine.telemetry.addData("distance ", robot.customObject.getDistance(DistanceUnit.MM)); - engine.telemetry.addData("object location ", robot.objectPos); - } -} 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 25fae43..d01490f 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,48 +12,64 @@ import dev.cyberarm.engine.V2.CyberarmState; public class DriveToCoordinatesState extends CyberarmState { CompetitionRobotV1 robot; - public double xTarget = 0; - public double yTarget = 0; - public double hTarget = 0; + public double xTarget; + public double yTarget; + public double hTarget; public boolean posAchieved = false; public boolean armDrive; - public String objectPos; -// public boolean posSpecific; + public int objectPos; + public boolean posSpecific; + public double maxXPower; + public double maxYPower; public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); + this.maxXPower = robot.configuration.variable(groupName, actionName, "maxXPower").value(); + this.maxYPower = robot.configuration.variable(groupName, actionName, "maxYPower").value(); this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value(); this.objectPos = robot.configuration.variable(groupName, actionName, "objectPos").value(); -// this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value(); + this.posSpecific = robot.configuration.variable(groupName, actionName, "posSpecific").value(); } @Override public void exec() { - robot.hTarget = hTarget; - robot.yTarget = yTarget; - robot.xTarget = xTarget; - robot.DriveToCoordinates(); - robot.OdometryLocalizer(); + if (!posSpecific) { + // enter loop + } else { + if (objectPos != robot.objectPos) { + // enter the ending loop + setHasFinished(true); + } + } + if (posAchieved) { + setHasFinished(true); + } + if (armDrive) { robot.clawArmControl(); } - if (posAchieved){ - setHasFinished(true); - } else { -// if (objectPos.equals(robot.objectPos) || objectPos.equals("everything") ) { - if (Math.abs(robot.backLeftPower) < 0.15 && - Math.abs(robot.backRightPower) < 0.15 && - Math.abs(robot.frontLeftPower) < 0.15 && - Math.abs(robot.frontRightPower) < 0.15) { - posAchieved = true; -// } - } - } + robot.yMaxPower = maxYPower; + robot.xMaxPower = maxXPower; + + robot.hTarget = hTarget; + robot.yTarget = yTarget; + robot.xTarget = xTarget; + + robot.DriveToCoordinates(); + robot.OdometryLocalizer(); + + + if (Math.abs(robot.backLeftPower) < 0.15 && + Math.abs(robot.backRightPower) < 0.15 && + Math.abs(robot.frontLeftPower) < 0.15 && + Math.abs(robot.frontRightPower) < 0.15) { + posAchieved = true; } + } 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 ff2559b..1bba8fb 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -30,26 +31,24 @@ public class CompetitionRobotV1 extends Robot { private CyberarmEngine engine; public TimeCraftersConfiguration configuration; - public String objectPos; + public int objectPos; // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: public double power; - public OpenCvWebcam webcam1 = null; public WebcamName webCamName; public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp; - public DcMotor odometerR, odometerL, odometerA; public IMU imu; public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo; public DistanceSensor customObject; + public TouchSensor touchLeftArm, touchRightArm; public int loopCheck = 0; // ----------------------------------------------------------------------------------------------------------------- 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 Yp = 0.03, Yi = 0, Yd = 0.0013; - private double drivePower = 1; + public static double Xp = -0.035, Xi = 0, Xd = 0; + public static double Yp = 0.035, Yi = 0, Yd = 0.0013; public double rx; public double xMultiplier = 1; public double yMultiplier = 1; @@ -75,7 +74,6 @@ public class CompetitionRobotV1 extends Robot { public double headingIntegralSum = 0; public double xIntegralSum = 0; public double yIntegralSum = 0; - public double targetHeading; public final double cm_per_tick = (2 * Math.PI * R) / N; private double headingLastError = 0; private double xLastError = 0; @@ -83,6 +81,7 @@ public class CompetitionRobotV1 extends Robot { ElapsedTime headingTimer = new ElapsedTime(); ElapsedTime xTimer = new ElapsedTime(); ElapsedTime yTimer = new ElapsedTime(); + public double frontLeftPower; public double backLeftPower; public double frontRightPower; @@ -95,15 +94,14 @@ public class CompetitionRobotV1 extends Robot { //-------------------------------------------------------------------------------------------------------------- arm sequence variables: PIDController pidController; - public String armPos; public int target; - public static double p = 0.0015, i = 0, d = 0, f = 0; - public static double shoulderCollect = 0.27; - public static double shoulderDeposit = 0.32; - public static double shoulderPassive = 0.8; - public static double elbowCollect = 0.02; - public static double elbowDeposit = 0; - public static double elbowPassive = 0; + public double p = 0.007, i = 0, d = 0.0001, f = 0; + public double shoulderCollect = 0.38; + public double shoulderDeposit = 0.36; + public double shoulderPassive = 0.8; + public double elbowCollect = 0.02; + public double elbowDeposit = 0; + public double elbowPassive = 0; private HardwareMap hardwareMap; @@ -145,7 +143,7 @@ public class CompetitionRobotV1 extends Robot { backRight.setDirection(DcMotorSimple.Direction.REVERSE); backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); - clawArm.setDirection(DcMotorSimple.Direction.REVERSE); + clawArm.setDirection(DcMotorSimple.Direction.FORWARD); lift.setDirection(DcMotorSimple.Direction.FORWARD); chinUp.setDirection(DcMotorSimple.Direction.FORWARD); @@ -170,7 +168,7 @@ public class CompetitionRobotV1 extends Robot { chinUp.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); clawArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - //IMU + // ----------------------------------------------------------------------------------------------------------------------------- IMU IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( RevHubOrientationOnRobot.LogoFacingDirection.LEFT, @@ -184,7 +182,10 @@ public class CompetitionRobotV1 extends Robot { pidController = new PIDController(p, i, d); + // ------------------------------------------------------------------------------------------------------------------------- SENSOR: customObject = engine.hardwareMap.get(Rev2mDistanceSensor.class, "customObject"); + touchLeftArm = engine.hardwareMap.get(TouchSensor.class, "left touch"); + touchRightArm = engine.hardwareMap.get(TouchSensor.class, "right touch"); //--------------------------------------------------------------------------------------------------------------------------- SERVO: shoulder = hardwareMap.servo.get("shoulder"); @@ -196,15 +197,13 @@ public class CompetitionRobotV1 extends Robot { elbow.setDirection(Servo.Direction.REVERSE); -// shoulder.setPosition(shoulderPassive); -// elbow.setPosition(elbowPassive); - } // -------------------------------------------------------------------------------------------------------------------------- Functions: + public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer: // update positions @@ -283,25 +282,25 @@ public class CompetitionRobotV1 extends Robot { } public void DriveToCoordinates () { - // determine the velocities needed for each direction // this uses PID to adjust needed Power for robot to move to target + +// if (yMaxPower < XPIDControl(xTarget, positionX)){ +// pidX = yMaxPower; +// } else { +// pidX = YPIDControl(yTarget, positionY); +// } double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); - if (xMaxPower > XPIDControl(xTarget, positionX)){ - pidX = xMaxPower; - } else { - pidX = YPIDControl(yTarget, positionY); - } +// if (xMaxPower < YPIDControl(yTarget, positionY)){ +// pidY = xMaxPower; +// } else { +// pidY = XPIDControl(xTarget, positionX); +// } - if (yMaxPower > YPIDControl(yTarget, positionY)){ - pidY = yMaxPower; - } else { pidY = XPIDControl(xTarget, positionX); - } - - pidY = XPIDControl(xTarget, positionX); + pidX = YPIDControl(yTarget, positionY); double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); @@ -335,4 +334,4 @@ public class CompetitionRobotV1 extends Robot { clawArm.setPower(power); } -} \ No newline at end of file +} 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 abfbbc8..97e2744 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 @@ -18,7 +18,7 @@ public class CompetitionTeleOpState extends CyberarmState { private CompetitionRobotV1 robot; // ------------------------------------------------------------------------------------------------------------robot claw arm variables: private PIDController pidController; - public static double p = 0.0015, i = 0, d = 0, f = 0; + public static double p = 0.007, i = 0, d = 0.0001, f = 0; public static int target = 0; // ------------------------------------------------------------------------------------------------------------- Heading lock variables: @@ -28,6 +28,7 @@ public class CompetitionTeleOpState extends CyberarmState { public double backDropLock = Math.toRadians(90); public double power; + public double armPower; private double currentHeading; private boolean headingLock = false; @@ -52,6 +53,8 @@ public class CompetitionTeleOpState extends CyberarmState { private boolean lbsVar1; private double drivePower = 1; public double rx; + public double minPower = 0; + // ------------------------------------------------------------------------------------------------------------------- Slider Variables: private int maxExtension = 2000; @@ -59,10 +62,10 @@ public class CompetitionTeleOpState extends CyberarmState { public boolean depositMode = false; // ---------------------------------------------------------------------------------------------------------------Arm control Variables: - public String armPos = "passive"; + public String armPos = "collect"; // chin up servo - public static double chinUpServoUp = 1; - public static double chinUpServoDown = 0.25; + public static double chinUpServoUp = 0.58; + public static double chinUpServoDown = 1; @@ -107,7 +110,7 @@ public class CompetitionTeleOpState extends CyberarmState { if (headingLock){ robot.rx = HeadingPIDControl(targetHeading, currentHeading); } else { - robot.rx = engine.gamepad1.right_stick_x; + robot.rx = engine.gamepad1.right_stick_x / 2; } @@ -121,9 +124,24 @@ public class CompetitionTeleOpState extends CyberarmState { } lbsVar1 = lbs1; - double y = engine.gamepad1.left_stick_y; - double x = -engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + + +// // Improve control? +// if (y < 0) { +// y = -Math.sqrt(-y); +// } else { +// y = Math.sqrt(y); +// } +// +// if (x < 0) { +// x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing; +// } else { +// x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing; +// } + + double x = engine.gamepad1.left_stick_x; + double y = -engine.gamepad1.left_stick_y; double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); // angle math to make things field oriented @@ -139,7 +157,7 @@ public class CompetitionTeleOpState extends CyberarmState { // setting each power determined previously from the math above // as well as multiplying it by a drive power that can be changed. - robot.backLeft.setPower(backLeftPower * drivePower); + robot.backLeft.setPower(backLeftPower * drivePower + minPower); robot.backRight.setPower(-backRightPower * drivePower); robot.frontLeft.setPower(frontLeftPower * drivePower); robot.frontRight.setPower(frontRightPower * drivePower); @@ -199,32 +217,33 @@ public class CompetitionTeleOpState extends CyberarmState { rbsVar2 = rbs2; } - public void ArmPosControl(){ + public void ArmPosControl() { - if (engine.gamepad2.a){ + if (engine.gamepad2.a) { armPos = "collect"; depositMode = false; - } else if (engine.gamepad2.x){ - armPos = "passive"; - depositMode = false; - } else if (engine.gamepad2.y){ +// } else if (engine.gamepad2.x) { +// armPos = "passive"; +// depositMode = false; + } else if (engine.gamepad2.y) { armPos = "deposit"; depositMode = true; - } - else if (engine.gamepad2.b){ + } else if (engine.gamepad2.b) { armPos = "hover"; depositMode = true; - } else if (engine.gamepad2.dpad_left){ + } else if (engine.gamepad2.dpad_left) { armPos = "lift up"; depositMode = true; - } else if (engine.gamepad2.dpad_right){ + } else if (engine.gamepad2.dpad_right) { armPos = "lift down"; depositMode = false; + } else if (engine.gamepad2.back) { + armPos = "reset"; } - if (armPos == "collect"){ - if (robot.lift.getCurrentPosition() >= 1){ + if (armPos == "collect") { + if (robot.lift.getCurrentPosition() >= 20) { robot.chinUpServo.setPosition(chinUpServoDown); robot.lift.setPower(-0.6); } else { @@ -232,57 +251,71 @@ public class CompetitionTeleOpState extends CyberarmState { robot.shoulder.setPosition(robot.shoulderCollect); robot.elbow.setPosition(robot.elbowCollect); robot.chinUpServo.setPosition(chinUpServoDown); - target = 10; + target = 30; } } - if (armPos == "passive"){ - if (robot.lift.getCurrentPosition() >= 1){ - robot.lift.setPower(-0.6); - robot.chinUpServo.setPosition(chinUpServoDown); - } else { - robot.lift.setPower(0); - robot.chinUpServo.setPosition(chinUpServoDown); - robot.shoulder.setPosition(robot.shoulderPassive); - robot.elbow.setPosition(robot.elbowPassive); - target = 850; - } - } - if (armPos == "deposit"){ - robot.shoulder.setPosition(robot.shoulderDeposit); - robot.elbow.setPosition(robot.elbowDeposit); - target = 370; - robot.chinUpServo.setPosition(chinUpServoDown); +// if (armPos == "passive") { +// if (robot.lift.getCurrentPosition() >= 20) { +// robot.lift.setPower(-0.6); +// robot.chinUpServo.setPosition(chinUpServoDown); +// } else { +// robot.lift.setPower(0); +// robot.chinUpServo.setPosition(chinUpServoDown); +// robot.shoulder.setPosition(robot.shoulderPassive); +// robot.elbow.setPosition(robot.elbowPassive); +// target = 850; +// } +// } + if (armPos == "deposit") { + robot.shoulder.setPosition(robot.shoulderDeposit); + robot.elbow.setPosition(robot.elbowDeposit); + target = 400; + robot.chinUpServo.setPosition(chinUpServoDown); } if (armPos == "hover") { - robot.shoulder.setPosition(robot.shoulderCollect); - robot.elbow.setPosition(robot.elbowCollect); - target = 120; - - } - if (armPos == "lift up") { - robot.shoulder.setPosition(robot.shoulderDeposit); - robot.elbow.setPosition(robot.elbowDeposit); - target = 120; - robot.chinUpServo.setPosition(chinUpServoUp); + if (robot.lift.getCurrentPosition() >= 20) { + robot.chinUpServo.setPosition(chinUpServoDown); + robot.lift.setPower(-0.6); + } else { + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); + target = 200; } - if (armPos == "lift down"){ - if (robot.lift.getCurrentPosition() >= 1){ + } + if (armPos == "lift up") { + robot.shoulder.setPosition(robot.shoulderDeposit); + robot.elbow.setPosition(robot.elbowDeposit); + target = 120; + robot.chinUpServo.setPosition(chinUpServoUp); + } + + if (armPos == "lift down") { + if (robot.lift.getCurrentPosition() >= 1) { robot.lift.setPower(-0.6); robot.chinUpServo.setPosition(chinUpServoDown); } else { robot.lift.setPower(0); robot.chinUpServo.setPosition(chinUpServoDown); - robot.shoulder.setPosition(robot.shoulderPassive); - robot.elbow.setPosition(robot.elbowPassive); + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); target = 850; } } + if (armPos == "reset") { + robot.shoulder.setPosition(robot.shoulderPassive); + if (robot.touchLeftArm.isPressed() || robot.touchRightArm.isPressed()) { + robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + armPos = "collect"; + } } + } + @Override public void init() { @@ -294,10 +327,11 @@ public class CompetitionTeleOpState extends CyberarmState { @Override public void exec() { + if (engine.gamepad2.dpad_up) { - robot.chinUp.setPower(1); - } else if (engine.gamepad2.dpad_down){ robot.chinUp.setPower(-1); + } else if (engine.gamepad2.dpad_down){ + robot.chinUp.setPower(1); } else { robot.chinUp.setPower(0); } @@ -336,14 +370,17 @@ public class CompetitionTeleOpState extends CyberarmState { // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift: pidController.setPID(p, i, d); - int armPos = robot.clawArm.getCurrentPosition(); - double pid = pidController.calculate(armPos, target); -// double ff = Math.cos(Math.toRadians(target / ticksInDegree)) * f; + int armCurrentPos = robot.clawArm.getCurrentPosition(); + double pid = pidController.calculate(armCurrentPos, target); - double power = pid; -// double power = pid + ff; + if (armPos == "reset"){ + armPower = -0.2; + } else if (armPos != "reset"){ + armPower = pid; - robot.clawArm.setPower(power); + } + + robot.clawArm.setPower(armPower); // ------------------------------------------------------------------------------------------------------------------- Lift Control: SliderTeleOp(); @@ -369,6 +406,8 @@ public class CompetitionTeleOpState extends CyberarmState { engine.telemetry.addData("Kd", Kd); engine.telemetry.addData("arm pos", armPos); engine.telemetry.addData("arm pos ticks", robot.clawArm.getCurrentPosition()); + engine.telemetry.addData("left touch", robot.touchLeftArm.isPressed()); + engine.telemetry.addData("right touch", robot.touchRightArm.isPressed()); } }