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);