From c5d5ca7b5974f399f687a083019b95ba1fbda359 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Wed, 3 Jan 2024 23:00:04 -0600 Subject: [PATCH] fixed heading lock, started to fix the max power function from the drive to coordinates state. plan to finish tomorrow and re tune the PID for driving because the robot has gotten changed mechanically, so it drives too fast --- .../DriveToCoordinatesState.java | 8 ++- .../Common/CompetitionRobotV1.java | 54 ++++++++++++------- .../TeleOp/States/CompetitionTeleOpState.java | 47 ++++++---------- 3 files changed, 56 insertions(+), 53 deletions(-) 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 8ea1294..987a19d 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 @@ -48,11 +48,11 @@ public class DriveToCoordinatesState extends CyberarmState { } else { if (objectPos != robot.objectPos) { // enter the ending loop - setHasFinished(true); +// setHasFinished(true); } } if (posAchieved) { - setHasFinished(true); +// setHasFinished(true); } if (armDrive) { @@ -93,6 +93,10 @@ public class DriveToCoordinatesState extends CyberarmState { 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("input y pidPower", robot.pidX); + engine.telemetry.addData("input x pidPower", robot.pidY); + engine.telemetry.addData("raw x pid", robot.XPIDControl(robot.xTarget, robot.positionX)); + engine.telemetry.addData("raw y pid", robot.YPIDControl(robot.yTarget, robot.positionY)); engine.telemetry.addData("global object position", robot.objectPos); engine.telemetry.addData("local object position", objectPos); 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 f96a217..bb36316 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -36,7 +36,6 @@ public class CompetitionRobotV1 extends Robot { public int objectPos; // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup: - public double power; public OpenCvWebcam webcam1 = null; public WebcamName webCamName; public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp; @@ -45,13 +44,11 @@ public class CompetitionRobotV1 extends Robot { 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.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; public double positionX = 1000; @@ -93,10 +90,12 @@ public class CompetitionRobotV1 extends Robot { public double xMaxPower = 1; public double pidX; public double pidY; + public double rawPidX; + public double rawPidY; //-------------------------------------------------------------------------------------------------------------- arm sequence variables: PIDController pidController; - + public double power; public String armPos; public int target; @@ -106,7 +105,6 @@ public class CompetitionRobotV1 extends Robot { public double shoulderPassive = 0.8; public double elbowCollect = 0.02; public double elbowDeposit = 0; - public double elbowPassive = 0; private HardwareMap hardwareMap; @@ -288,26 +286,42 @@ public class CompetitionRobotV1 extends Robot { } public void DriveToCoordinates () { - // determine the velocities needed for each direction + // determine the powers 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); -// } +// rawPidY = XPIDControl(xTarget, positionX); +// rawPidX = YPIDControl(yTarget, positionY); + + if (Math.abs(yTarget - positionY) > 5) { + if (Math.abs(XPIDControl(xTarget, positionX)) >= yMaxPower) { + if (XPIDControl(xTarget, positionX) < 0) { + pidY = yMaxPower * -1; + } else { + pidY = yMaxPower; + } + } else { + pidY = rawPidX; + } + } else { + pidY = rawPidX; + } + double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); -// if (xMaxPower < YPIDControl(yTarget, positionY)){ -// pidY = xMaxPower; -// } else { -// pidY = XPIDControl(xTarget, positionX); -// } - - pidY = XPIDControl(xTarget, positionX); - pidX = YPIDControl(yTarget, positionY); - + if (Math.abs(xTarget - positionX) > 5) { + if (Math.abs(YPIDControl(yTarget, positionY)) >= xMaxPower) { + if (YPIDControl(yTarget, positionY) < 0) { + pidX = xMaxPower * -1; + } else { + pidX = xMaxPower; + } + } else { + pidX = rawPidY; + } + } else { + pidX = rawPidY; + } double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); 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 b3969a9..212440f 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 @@ -20,8 +20,8 @@ public class CompetitionTeleOpState extends CyberarmState { private CompetitionRobotV1 robot; // ------------------------------------------------------------------------------------------------------------robot claw arm variables: private PIDController pidController; - public static double p = 0.007, i = 0, d = 0.0001, f = 0; - public static int target = 0; + public double p = 0.007, i = 0, d = 0.0001, f = 0; + public int target = 0; // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; @@ -29,7 +29,6 @@ public class CompetitionTeleOpState extends CyberarmState { public double collectLock = Math.toRadians(-90); public double backDropLock = Math.toRadians(90); - public double power; public double armPower; private double currentHeading; private boolean headingLock = false; @@ -107,12 +106,6 @@ public class CompetitionTeleOpState extends CyberarmState { public void DriveTrainTeleOp () { - if (headingLock){ - rx = HeadingPIDControl(targetHeading, currentHeading); - } else { - rx = engine.gamepad1.right_stick_x / 2; - } - boolean lbs1 = engine.gamepad1.left_stick_button; if (lbs1 && !lbsVar1) { @@ -239,18 +232,6 @@ public class CompetitionTeleOpState extends CyberarmState { } } -// 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 (Objects.equals(armPos, "deposit")) { robot.shoulder.setPosition(robot.shoulderDeposit); robot.elbow.setPosition(robot.elbowDeposit); @@ -306,12 +287,12 @@ public class CompetitionTeleOpState extends CyberarmState { super.init(); pidController = new PIDController(p, i, d); - } @Override public void exec() { + if (engine.gamepad2.dpad_up) { robot.chinUp.setPower(-1); } else if (engine.gamepad2.dpad_down){ @@ -327,24 +308,28 @@ public class CompetitionTeleOpState extends CyberarmState { currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); // drivetrain - DriveTrainTeleOp(); if (engine.gamepad1.b){ headingLock = true; targetHeading = backDropLock; - } - if (engine.gamepad1.x){ + } else if (engine.gamepad1.x){ headingLock = true; targetHeading = collectLock; - } - if (engine.gamepad1.a){ + } else if (engine.gamepad1.a){ headingLock = true; targetHeading = currentHeading; - } - if (engine.gamepad1.right_stick_x != 0){ + } else if (engine.gamepad1.right_stick_x != 0){ headingLock = false; } + if (headingLock){ + rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + rx = engine.gamepad1.right_stick_x / 2; + } + + DriveTrainTeleOp(); + // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift: pidController.setPID(p, i, d); int armCurrentPos = robot.clawArm.getCurrentPosition(); @@ -375,8 +360,8 @@ public class CompetitionTeleOpState extends CyberarmState { engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)); engine.telemetry.addData("imu", -robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)); - engine.telemetry.addData("pid power", power); - engine.telemetry.addData("rx power", robot.rx); + engine.telemetry.addData("pid power", HeadingPIDControl(targetHeading, currentHeading)); + engine.telemetry.addData("rx power", rx); engine.telemetry.addData("heading Lock?", headingLock); engine.telemetry.addData("Kp", Kp); engine.telemetry.addData("Ki", Ki);