From 4450c7e48a0e566de73835283b7147d40ca6b7f1 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Mon, 5 Feb 2024 20:24:51 -0600 Subject: [PATCH] fully programmed teleOp and started odo tuning --- .../Common/CompetitionRobotV1.java | 18 +++-- .../TeleOp/States/CompetitionTeleOpState.java | 65 ++++++++++++++++--- 2 files changed, 69 insertions(+), 14 deletions(-) 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 fa15d84..cc15127 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -55,6 +55,9 @@ public class CompetitionRobotV1 extends Robot { 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 Dnl1; + public double Dnr2; public double xMultiplier = 1; public double yMultiplier = 1; public double positionX = 1000; @@ -121,8 +124,8 @@ public class CompetitionRobotV1 extends Robot { public int target; public double p = 0.007, i = 0, d = 0.0001, f = 0; - public double shoulderCollect = 1; - public double shoulderDeposit = 1; + public double shoulderCollect = 0.86; + public double shoulderDeposit = 0.86; public double shoulderPassive = 1; public double elbowCollect = 0.02; public double elbowDeposit = 0; @@ -167,7 +170,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.FORWARD); + clawArm.setDirection(DcMotorSimple.Direction.REVERSE); lift.setDirection(DcMotorSimple.Direction.FORWARD); chinUp.setDirection(DcMotorSimple.Direction.FORWARD); @@ -246,14 +249,17 @@ public class CompetitionRobotV1 extends Robot { oldLeftPosition = currentLeftPosition; oldAuxPosition = currentAuxPosition; - currentRightPosition = frontLeft.getCurrentPosition(); - currentLeftPosition = -backRight.getCurrentPosition(); - currentAuxPosition = backLeft.getCurrentPosition(); + currentRightPosition = frontRight.getCurrentPosition(); + currentLeftPosition = backLeft.getCurrentPosition(); + currentAuxPosition = -frontLeft.getCurrentPosition(); int dnl1 = currentLeftPosition - oldLeftPosition; int dnr2 = currentRightPosition - oldRightPosition; int dna3 = currentAuxPosition - oldAuxPosition; + Dnl1 = dnl1; + Dnr2 = dnr2; + // the robot has turned and moved a tiny bit between two measurements double dtheta = cm_per_tick * (dnr2 - dnl1) / L; double dx = cm_per_tick * (dnl1 + dnr2) / 2.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 9520e78..173c79d 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 @@ -70,6 +70,7 @@ public class CompetitionTeleOpState extends CyberarmState { // chin up servo public static double chinUpServoUp = 0.58; public static double chinUpServoDown = 1; + public long lastExecutedTime; @@ -201,30 +202,68 @@ public class CompetitionTeleOpState extends CyberarmState { } public void ArmPosControl() { - - - if (engine.gamepad2.a) { + if (engine.gamepad2.a && !engine.gamepad2.back) { armPos = "collect"; depositMode = false; - } else if (engine.gamepad2.y) { + } else if (engine.gamepad2.y && !engine.gamepad2.back) { armPos = "deposit"; depositMode = true; - } else if (engine.gamepad2.b) { + } else if (engine.gamepad2.b && !engine.gamepad2.start) { armPos = "hover"; depositMode = true; - } else if (engine.gamepad2.x) { + } else if (engine.gamepad2.x && !engine.gamepad2.start) { armPos = "passive"; depositMode = true; - } else if (engine.gamepad2.dpad_left) { + } else if (engine.gamepad2.dpad_left && !engine.gamepad2.start) { armPos = "lift up"; depositMode = true; - } else if (engine.gamepad2.dpad_right) { + } else if (engine.gamepad2.dpad_right && !engine.gamepad2.start) { armPos = "lift down"; depositMode = false; } else if (engine.gamepad2.back) { armPos = "reset"; + } else if (engine.gamepad2.right_bumper) { + armPos = "tuning up"; + } else if (engine.gamepad2.left_bumper) { + armPos = "tuning down"; } + if (Objects.equals(armPos, "tuning up")) { + if (robot.lift.getCurrentPosition() >= 20) { + robot.chinUpServo.setPosition(chinUpServoDown); + robot.lift.setPower(-0.6); + } else { + robot.lift.setPower(0); + if (System.currentTimeMillis() - lastExecutedTime > 200){ + robot.shoulderCollect = robot.shoulderCollect + 0.02; + lastExecutedTime = System.currentTimeMillis(); + } + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); + robot.chinUpServo.setPosition(chinUpServoDown); + target = 0; + armPos = "collect"; + + } + } + if (Objects.equals(armPos, "tuning down")) { + if (robot.lift.getCurrentPosition() >= 20) { + robot.chinUpServo.setPosition(chinUpServoDown); + robot.lift.setPower(-0.6); + } else { + robot.lift.setPower(0); + if (System.currentTimeMillis() - lastExecutedTime > 200) { + robot.shoulderCollect = robot.shoulderCollect - 0.02; + lastExecutedTime = System.currentTimeMillis(); + } + robot.shoulder.setPosition(robot.shoulderCollect); + robot.elbow.setPosition(robot.elbowCollect); + robot.chinUpServo.setPosition(chinUpServoDown); + target = 0; + armPos = "collect"; + + } + } if (Objects.equals(armPos, "collect")) { if (robot.lift.getCurrentPosition() >= 20) { robot.chinUpServo.setPosition(chinUpServoDown); @@ -296,11 +335,13 @@ public class CompetitionTeleOpState extends CyberarmState { robot.clawArm.setTargetPosition(0); robot.clawArm.setPower(0); robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lastExecutedTime = System.currentTimeMillis(); } @Override public void exec() { + robot.OdometryLocalizer(); if (engine.gamepad2.start && engine.gamepad2.x){ robot.shootServo.setPosition(releasePos); @@ -372,6 +413,14 @@ public class CompetitionTeleOpState extends CyberarmState { @Override public void telemetry () { + engine.telemetry.addData("Dnl1", robot.Dnl1); + engine.telemetry.addData("Dnr2", robot.Dnr2); + 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("Lift Encoder Pos", robot.lift.getCurrentPosition()); engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));