From 3327860e5dd9a8335ee80fe6d045ab418d5a230a Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 6 Jan 2024 00:59:34 -0600 Subject: [PATCH] fixed max power function, tuned odometry a bit, added a wait arguement in the arm state so we can have delays and pauses when needed, and wrote out all of blue audience side auto steps. --- .../Competition2BlueBackStage.java | 79 ++++++++++++++----- .../CompetitionStates/ClawArmState.java | 11 ++- .../DriveToCoordinatesState.java | 38 ++++----- .../Common/CompetitionRobotV1.java | 70 ++++++++-------- .../TeleOp/States/CompetitionTeleOpState.java | 31 ++++---- 5 files changed, 139 insertions(+), 90 deletions(-) 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 index 24ab34a..9d776b8 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java @@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "BURNSVILLE blue backdrop") +@Autonomous(name = "BURNSVILLE blue audience") public class Competition2BlueBackStage extends CyberarmEngine { @@ -30,34 +30,77 @@ public class Competition2BlueBackStage extends CyberarmEngine { @Override public void setup() { - this.robot = new CompetitionRobotV1("Burnsville backdrop blue"); + this.robot = new CompetitionRobotV1("burnsville audience 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")); - - addState(new ClawFingerState(robot,"burnsville backdrop blue", "0-04-0")); - - addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-05-0")); - addState(new DriveToCoordinatesState(robot,"burnsville backdrop blue", "3-06-0")); - - - - - + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0")); + addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0")); +// 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", "3-02-0")); +// 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")); + + // drive to search pos + addState(new ClawArmState(robot,"burnsville audience blue", "0-02-2")); + + // close right finger + addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3")); + + // drive back and away from the spike mark (x,y) (1050, 1000) H = 0 +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0")); + // drive to the middle truss (right version) (1130,980) H = -90 +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0")); + + // drive to hover mode to clear the middle truss + addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1")); + // drive under the middle truss across the field (right version) (1170,1080) H = -90 +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0")); + + // drive to deposit mode to place on back drop + addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1")); + + // drive to back drop +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0")); + + // open claw to deposit gold + addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1")); + + // drive back from backdrop +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0")); + + // close left claw + addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1")); + // drive to parking spot +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0")); +// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0")); + addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0")); + // arm to collect pos + addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1")); } 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 19be294..38df752 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 @@ -13,9 +13,18 @@ public class ClawArmState extends CyberarmState { CompetitionRobotV1 robot; public String armPos; + public long wait; + public long initTime; public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value(); + this.wait = robot.configuration.variable(groupName, actionName, "wait").value(); + } + + @Override + public void start() { + super.start(); + initTime = System.currentTimeMillis(); } @Override @@ -28,7 +37,7 @@ public class ClawArmState extends CyberarmState { robot.armPos = armPos; robot.clawArmControl(); - if (robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) { + if ((robot.clawArm.getCurrentPosition() > robot.target - 20 || robot.clawArm.getCurrentPosition() < robot.target + 20) && wait < System.currentTimeMillis() - initTime) { 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 987a19d..e31e917 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 @@ -21,7 +21,6 @@ public class DriveToCoordinatesState extends CyberarmState { public boolean posSpecific; public double maxXPower; public double maxYPower; - public long initTime; public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) { this.robot = robot; @@ -38,43 +37,38 @@ public class DriveToCoordinatesState extends CyberarmState { @Override public void start() { super.start(); - initTime = System.currentTimeMillis(); + robot.hTarget = hTarget; + robot.yTarget = yTarget; + robot.xTarget = xTarget; + robot.yMaxPower = maxYPower; + robot.xMaxPower = maxXPower; + } @Override public void exec() { - if (!posSpecific) { - // enter loop - } else { + if (posSpecific) { if (objectPos != robot.objectPos) { // enter the ending loop -// setHasFinished(true); + setHasFinished(true); } } - if (posAchieved) { -// setHasFinished(true); - } if (armDrive) { robot.clawArmControl(); } - robot.yMaxPower = maxYPower; - robot.xMaxPower = maxXPower; - - robot.hTarget = hTarget; - robot.yTarget = yTarget; - robot.xTarget = xTarget; - + robot.OdometryLocalizer(); + robot.XDrivePowerModifier(); + robot.YDrivePowerModifier(); robot.DriveToCoordinates(); robot.OdometryLocalizer(); - - if ((Math.abs(robot.backLeftPower) < 0.18 && - Math.abs(robot.backRightPower) < 0.18 && - Math.abs(robot.frontLeftPower) < 0.18 && - Math.abs(robot.frontRightPower) < 0.18) || (System.currentTimeMillis() - initTime > 5000)) { - posAchieved = true; + if ((Math.abs(robot.backLeftPower) < 0.15 && + Math.abs(robot.backRightPower) < 0.15 && + Math.abs(robot.frontLeftPower) < .15 && + Math.abs(robot.frontRightPower) < 0.15)){ + setHasFinished(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 bb36316..759a86c 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -47,8 +47,8 @@ public class CompetitionRobotV1 extends Robot { // ----------------------------------------------------------------------------------------------------------------- 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 static double Xp = -0.03, Xi = 0, Xd = 0; + public static double Yp = 0.03, Yi = 0, Yd = 0; public double xMultiplier = 1; public double yMultiplier = 1; public double positionX = 1000; @@ -175,9 +175,10 @@ public class CompetitionRobotV1 extends Robot { // ----------------------------------------------------------------------------------------------------------------------------- IMU IMU.Parameters parameters = new IMU.Parameters( new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, RevHubOrientationOnRobot.UsbFacingDirection.UP)); + imu.initialize(parameters); configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -285,44 +286,43 @@ public class CompetitionRobotV1 extends Robot { return output; } + public void YDrivePowerModifier () { + + rawPidY = XPIDControl(xTarget, positionX); + + if (Math.abs(rawPidY) > yMaxPower) { + if (rawPidY < 0) { + pidY = -yMaxPower; + } else { + pidY = yMaxPower; + } + } else { + pidY = rawPidY; + } + } + + public void XDrivePowerModifier () { + + rawPidX = YPIDControl(yTarget, positionY); + + if (Math.abs(rawPidX) > xMaxPower) { + if (rawPidX < 0) { + pidX = -xMaxPower; + } else { + pidX = xMaxPower; + } + } else { + pidX = rawPidX; + } + } + public void DriveToCoordinates () { // determine the powers needed for each direction // this uses PID to adjust needed Power for robot to move to target -// 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 (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); // field oriented math, (rotating the global field to the relative field) @@ -366,7 +366,7 @@ public class CompetitionRobotV1 extends Robot { lift.setPower(-0.6); } else { shoulder.setPosition(0.38); - target = 100; + target = 120; } } 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 212440f..4ee04c4 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 @@ -23,17 +23,18 @@ public class CompetitionTeleOpState extends CyberarmState { public double p = 0.007, i = 0, d = 0.0001, f = 0; public int target = 0; + // ------------------------------------------------------------------------------------------------------------- Heading lock variables: public double integralSum = 0; private double targetHeading; - public double collectLock = Math.toRadians(-90); - public double backDropLock = Math.toRadians(90); - + public double collectLock = Math.toRadians(90); + public double backDropLock = Math.toRadians(-90); + public double boost; public double armPower; private double currentHeading; private boolean headingLock = false; - public static double Kp = 1; + public static double Kp = 0.8; public static double Ki = 0; public static double Kd = 0; private double lastError = 0; @@ -53,7 +54,7 @@ public class CompetitionTeleOpState extends CyberarmState { //---------------------------------------------------------------------------------------------------------------- Drivetrain Variables: private boolean lbsVar1; private double drivePower = 1; - public double rx; + public double rx = engine.gamepad1.right_stick_x / 2; // ------------------------------------------------------------------------------------------------------------------- Slider Variables: private int maxExtension = 2000; @@ -117,8 +118,12 @@ public class CompetitionTeleOpState extends CyberarmState { } lbsVar1 = lbs1; - double x = engine.gamepad1.left_stick_x; - double y = -engine.gamepad1.left_stick_y; + if (engine.gamepad1.left_stick_x != 0 || engine.gamepad1.left_stick_y != 0){ + boost = engine.gamepad1.right_trigger + 1; + } + + double x = -((engine.gamepad1.left_stick_x * 0.5) * boost); + double y = ((engine.gamepad1.left_stick_y * 0.5) * boost); double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); // angle math to make things field oriented @@ -134,10 +139,10 @@ 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.backRight.setPower(-backRightPower * drivePower); - robot.frontLeft.setPower(frontLeftPower * drivePower); - robot.frontRight.setPower(frontRightPower * drivePower); + robot.backLeft.setPower(backLeftPower); + robot.backRight.setPower(-backRightPower); + robot.frontLeft.setPower(frontLeftPower); + robot.frontRight.setPower(frontRightPower); } public double angleWrap(double radians) { @@ -200,9 +205,6 @@ public class CompetitionTeleOpState extends CyberarmState { if (engine.gamepad2.a) { armPos = "collect"; depositMode = false; -// } else if (engine.gamepad2.x) { -// armPos = "passive"; -// depositMode = false; } else if (engine.gamepad2.y) { armPos = "deposit"; depositMode = true; @@ -286,6 +288,7 @@ public class CompetitionTeleOpState extends CyberarmState { public void init() { super.init(); pidController = new PIDController(p, i, d); + robot.imu.resetYaw(); }