From 891b0c4992cb5e75cc9bd1fc7b70a8dd1d5b7042 Mon Sep 17 00:00:00 2001 From: Eddieman855 <69999780+Eddieman855@users.noreply.github.com> Date: Mon, 12 Feb 2024 19:24:33 -0800 Subject: [PATCH] inverse kinematics for handoff --- .../team3647/frc2024/auto/AutoCommands.java | 27 ++++--- .../frc2024/commands/ShooterCommands.java | 5 +- .../frc2024/commands/WristCommands.java | 4 + .../frc2024/constants/VisionConstants.java | 55 ++++++++++---- .../frc2024/robot/RobotContainer.java | 43 ++++------- .../frc2024/subsystems/Superstructure.java | 29 ++++---- .../frc2024/util/InverseKinematics.java | 74 +++++++++++++++++++ 7 files changed, 163 insertions(+), 74 deletions(-) create mode 100644 src/main/java/team3647/frc2024/util/InverseKinematics.java diff --git a/src/main/java/team3647/frc2024/auto/AutoCommands.java b/src/main/java/team3647/frc2024/auto/AutoCommands.java index ef48412..ecffdfb 100644 --- a/src/main/java/team3647/frc2024/auto/AutoCommands.java +++ b/src/main/java/team3647/frc2024/auto/AutoCommands.java @@ -73,28 +73,34 @@ public AutoCommands( public void registerCommands() {} public Command five_S1N1F1N2N3(Alliance color) { - return Commands.sequence( - Commands.waitSeconds(1), - followChoreoPathWithOverride(s1_to_n1_to_f1, color), - followChoreoPathWithOverride(f1_to_n2, color), - followChoreoPath(n2_to_n3, color)); + return Commands.parallel( + masterSuperstructureSequence(), + Commands.sequence( + followChoreoPathWithOverride(s1_to_n1_to_f1, color), + followChoreoPathWithOverride(f1_to_n2, color), + followChoreoPath(n2_to_n3, color))); } public Command four_S1N1N2N3(Alliance color) { return Commands.parallel( - Commands.sequence(scorePreload(), continuouslyIntakeForShoot(swerve::getOdoPose)), - superstructure.spinUp(), + masterSuperstructureSequence(), Commands.sequence( followChoreoPathWithOverride(s1_to_n1, color), followChoreoPathWithOverride(n1_to_n2, color), followChoreoPathWithOverride(n2_to_n3, color))); } + public Command masterSuperstructureSequence() { + return Commands.parallel( + Commands.sequence(scorePreload(), continuouslyIntakeForShoot(swerve::getOdoPose)), + superstructure.spinUp(), + superstructure.prep()); + } + public Command scorePreload() { return Commands.parallel( - superstructure.prep(), Commands.sequence(Commands.waitSeconds(0.5), superstructure.feed())) - .withTimeout(1.2); + .withTimeout(0.8); } public Pose2d getInitial(String path) { @@ -139,8 +145,7 @@ public Command continuouslyIntakeForShoot(Supplier drivePose) { < AutoConstants .kDrivetrainXShootingThreshold), Commands.waitSeconds(0.5), - superstructure.feed().withTimeout(0.5)), - superstructure.prep())) + superstructure.feed().withTimeout(0.5)))) .repeatedly(); } diff --git a/src/main/java/team3647/frc2024/commands/ShooterCommands.java b/src/main/java/team3647/frc2024/commands/ShooterCommands.java index 98674df..fe38cc3 100644 --- a/src/main/java/team3647/frc2024/commands/ShooterCommands.java +++ b/src/main/java/team3647/frc2024/commands/ShooterCommands.java @@ -1,5 +1,6 @@ package team3647.frc2024.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -19,7 +20,7 @@ public Command openLoop(DoubleSupplier bill) { return Commands.run( () -> { shooterRight.openLoop(bill.getAsDouble() * 0.8); - shooterLeft.openLoop(bill.getAsDouble()); + shooterLeft.openLoop(MathUtil.clamp(bill.getAsDouble() * 1.2, 0, 1)); }, shooterRight, shooterLeft); @@ -29,7 +30,7 @@ public Command setVelocity(DoubleSupplier bill) { return Commands.run( () -> { shooterRight.setVelocity(bill.getAsDouble() * 0.8); - shooterLeft.setVelocity(bill.getAsDouble()); + shooterLeft.setVelocity(bill.getAsDouble() * 1.2); }, shooterRight, shooterLeft); diff --git a/src/main/java/team3647/frc2024/commands/WristCommands.java b/src/main/java/team3647/frc2024/commands/WristCommands.java index b801524..4ca58b3 100644 --- a/src/main/java/team3647/frc2024/commands/WristCommands.java +++ b/src/main/java/team3647/frc2024/commands/WristCommands.java @@ -23,6 +23,10 @@ public Command setAngle(double angle) { return Commands.run(() -> wrist.setAngle(angle), wrist); } + public Command setAngle(DoubleSupplier angle) { + return Commands.run(() -> wrist.setAngle(angle.getAsDouble()), wrist); + } + public Command holdPositionAtCall() { return new Command() { double degreeAtStart = WristConstants.kInitialDegree; diff --git a/src/main/java/team3647/frc2024/constants/VisionConstants.java b/src/main/java/team3647/frc2024/constants/VisionConstants.java index 8d126ca..4336a00 100644 --- a/src/main/java/team3647/frc2024/constants/VisionConstants.java +++ b/src/main/java/team3647/frc2024/constants/VisionConstants.java @@ -6,23 +6,48 @@ import edu.wpi.first.math.util.Units; public class VisionConstants { - public static final String photonIP_1 = "10.36.47.14"; - public static final String photonIP_2 = "10.36.47.14"; - public static final String photonIP_3 = "10.36.47.14"; + public static final String photonIP_right = "10.36.47.14"; + public static final String photonIP_left = "10.36.47.15"; + public static final String photonIP_driver = "10.36.47.16"; - public static final String photonName_1_BackLeft = "ar doo cam"; - public static final String photonName_1_BackRight = "ar doo cam"; - public static final String photonName_1_Left = "ar doo cam"; - public static final String photonName_1_Right = "ar doo cam"; - public static final String photonName_1_Driver = "ar doo cam"; + public static final String backLeft = "back left"; + public static final String backRight = "back right"; + public static final String left = "left"; + public static final String right = "right"; + public static final String driver = "driver"; - public static final Transform3d robotToPhotonCam = + public static final Transform3d robotToBackLeft = new Transform3d( new Translation3d( - -Units.inchesToMeters(11), - Units.inchesToMeters(11), - Units.inchesToMeters(7.5)), - new Rotation3d(Math.PI, 0, 0) - .rotateBy(new Rotation3d(0, -Math.PI / 6, 0)) - .rotateBy(new Rotation3d(0, 0, Math.PI * 3 / 4))); + Units.inchesToMeters(-9.2193), + Units.inchesToMeters(8), + Units.inchesToMeters(8.7885)), + new Rotation3d(0, Math.PI / 180 * 28.12, 0) + .rotateBy(new Rotation3d(0, 0, Math.PI * 2 / 3))); + + public static final Transform3d robotToBackRight = + new Transform3d( + new Translation3d( + Units.inchesToMeters(-9.2193), + Units.inchesToMeters(-8), + Units.inchesToMeters(8.7885)), + new Rotation3d(0, Math.PI / 180 * 28.12, 0) + .rotateBy(new Rotation3d(0, 0, -Math.PI * 2 / 3))); + + public static final Transform3d robotToLeft = + new Transform3d( + new Translation3d( + Units.inchesToMeters(1.4481), + Units.inchesToMeters(11.0043), + Units.inchesToMeters(18.4375)), + new Rotation3d(0, -Math.PI / 9, 0).rotateBy(new Rotation3d(0, 0, Math.PI / 2))); + + public static final Transform3d robotToRight = + new Transform3d( + new Translation3d( + -Units.inchesToMeters(1.4481), + Units.inchesToMeters(-11.0043), + Units.inchesToMeters(18.4375)), + new Rotation3d(0, -Math.PI / 9, 0) + .rotateBy(new Rotation3d(0, 0, -Math.PI / 2))); } diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java index 8d295b5..0f317ef 100644 --- a/src/main/java/team3647/frc2024/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2024/robot/RobotContainer.java @@ -12,9 +12,6 @@ import team3647.frc2024.auto.AutonomousMode; import team3647.frc2024.commands.DrivetrainCommands; import team3647.frc2024.constants.FieldConstants; -// import team3647.frc2024.auto.AutoCommands; -// // import team3647.frc2024.auto.AutoCommands; -// import team3647.frc2024.auto.AutonomousMode; import team3647.frc2024.constants.GlobalConstants; import team3647.frc2024.constants.IntakeConstants; import team3647.frc2024.constants.KickerConstants; @@ -67,6 +64,7 @@ public RobotContainer() { autoCommands.registerCommands(); runningMode = autoCommands.blueFour_S1N1N2N3; pivot.setEncoder(PivotConstants.kInitialAngle); + wrist.setEncoder(WristConstants.kInitialDegree); swerve.setRobotPose(runningMode.getPathplannerPose2d()); } @@ -100,10 +98,7 @@ private void configureButtonBindings() { mainController .leftBumper .or(() -> superstructure.getPiece()) - .onFalse(superstructure.stowIntake()); - mainController - .leftBumper - .or(() -> superstructure.getPiece()) + .onFalse(superstructure.stowIntake()) .onFalse(superstructure.kickerCommands.kill()); mainController .leftBumper @@ -241,43 +236,31 @@ public Command getAutonomousCommand() { private final DrivetrainCommands drivetrainCommands = new DrivetrainCommands(swerve); - public final AprilTagPhotonVision ar_doo_cam = + public final AprilTagPhotonVision backLeft = new AprilTagPhotonVision( - VisionConstants.photonName_1_BackLeft, - VisionConstants.robotToPhotonCam, - swerve::getOdoPose); + VisionConstants.backLeft, VisionConstants.robotToBackLeft, swerve::getOdoPose); - public final AprilTagPhotonVision ar_doo_cam2 = + public final AprilTagPhotonVision backRight = new AprilTagPhotonVision( - VisionConstants.photonName_1_BackRight, - VisionConstants.robotToPhotonCam, + VisionConstants.backRight, + VisionConstants.robotToBackRight, swerve::getOdoPose); - public final AprilTagPhotonVision ar_doo_cam3 = + public final AprilTagPhotonVision left = new AprilTagPhotonVision( - VisionConstants.photonName_1_Left, - VisionConstants.robotToPhotonCam, - swerve::getOdoPose); + VisionConstants.left, VisionConstants.robotToLeft, swerve::getOdoPose); - public final AprilTagPhotonVision ar_doo_cam4 = + public final AprilTagPhotonVision right = new AprilTagPhotonVision( - VisionConstants.photonName_1_Right, - VisionConstants.robotToPhotonCam, - swerve::getOdoPose); + VisionConstants.right, VisionConstants.robotToRight, swerve::getOdoPose); private final VisionController visionController = new VisionController( - swerve::addVisionData, - swerve::shouldAddData, - ar_doo_cam, - ar_doo_cam2, - ar_doo_cam3, - ar_doo_cam4); + swerve::addVisionData, swerve::shouldAddData, backLeft, backRight, left, right); private final LEDs LEDs = new LEDs(LEDConstants.m_candle); - public final NeuralDetector detector = - new NeuralDetectorPhotonVision(VisionConstants.photonName_1_Driver); + public final NeuralDetector detector = new NeuralDetectorPhotonVision(VisionConstants.driver); public final TargetingUtil targetingUtil = new TargetingUtil( diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java index e5511dc..cec787e 100644 --- a/src/main/java/team3647/frc2024/subsystems/Superstructure.java +++ b/src/main/java/team3647/frc2024/subsystems/Superstructure.java @@ -10,8 +10,7 @@ import team3647.frc2024.commands.PivotCommands; import team3647.frc2024.commands.ShooterCommands; import team3647.frc2024.commands.WristCommands; -import team3647.frc2024.constants.PivotConstants; -import team3647.frc2024.constants.WristConstants; +import team3647.frc2024.util.InverseKinematics; public class Superstructure { @@ -29,12 +28,13 @@ public class Superstructure { private final DoubleSupplier pivotAngleSupplier; private final double pivotStowAngle = 40; - private final double pivotIntakeAngle = PivotConstants.kMinDegree; - private final double wristStowAngle = WristConstants.kMaxDegree; + private final double wristStowAngle = 60; private final double wristIntakeAngle = 0; private final double shootSpeed; private boolean hasPiece = false; + private InverseKinematics inverseKinematics; + public Superstructure( Intake intake, Kicker kicker, @@ -58,6 +58,8 @@ public Superstructure( shooterCommands = new ShooterCommands(shooterRight, shooterLeft); pivotCommands = new PivotCommands(pivot); wristCommands = new WristCommands(wrist); + + inverseKinematics = new InverseKinematics(pivotAngleSupplier); } public Command feed() { @@ -130,14 +132,9 @@ public Command intake() { public Command passToShooter() { return Commands.parallel( - intakeCommands.kill(), - wristCommands.setAngle(wristStowAngle), - pivotCommands.setAngle(() -> pivotIntakeAngle)) - .until( - () -> - wrist.angleReached(wristStowAngle, 1) - && pivot.angleReached(pivotIntakeAngle, 1)) - .andThen(shootThrough()); + intakeCommands.kill(), + wristCommands.setAngle(() -> inverseKinematics.getWristHandoffAngleByPivot()), + Commands.sequence(Commands.waitSeconds(0.5), shootThrough())); } public Command shootThrough() { @@ -147,10 +144,10 @@ public Command shootThrough() { } public Command stowIntake() { - return Commands.deadline( - pivotCommands - .setAngle(() -> pivotStowAngle) - .until(() -> pivot.angleReached(pivotStowAngle, 5)), + return Commands.parallel( + wristCommands + .setAngle(wristStowAngle) + .until(() -> wrist.angleReached(wristStowAngle, 5)), intakeCommands.kill()); } diff --git a/src/main/java/team3647/frc2024/util/InverseKinematics.java b/src/main/java/team3647/frc2024/util/InverseKinematics.java new file mode 100644 index 0000000..51df96c --- /dev/null +++ b/src/main/java/team3647/frc2024/util/InverseKinematics.java @@ -0,0 +1,74 @@ +package team3647.frc2024.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.function.DoubleSupplier; +import team3647.lib.GeomUtil; + +public class InverseKinematics { + + private final DoubleSupplier pivotAngle; + private final double wristAngleoffSet = 0; + private final double pivotLength = 0; + private final double pivotHeight = 0; + private final double pivotX = 0; + private final double wristHeight = 0; + private final double wristX = 0; + private final double wristLength = 0; + private final double wristRollersAngle = 0; + private final Pose2d wristOriginPos = new Pose2d(wristX, wristHeight, new Rotation2d()); + + public InverseKinematics(DoubleSupplier pivotAngle) { + this.pivotAngle = pivotAngle; + } + + // all poses are from the side pov with the intake at the back, positive x is forward and + // positive y is up, 0 rot is forward and positive is ccw + + public Pose2d getPivotReceivingPosition() { + return new Pose2d( + pivotX - pivotLength * Math.cos(pivotAngle.getAsDouble()), + pivotHeight - pivotLength * Math.sin(pivotAngle.getAsDouble()), + new Rotation2d()); + } + + public double getWristHandoffAngleByPivot() { + Pose2d pivotReceivingPose = getPivotReceivingPosition(); + double triangleBase = + GeomUtil.distance( + wristOriginPos, + pivotReceivingPose); // creating triangle with wrist length, wrist origin to + // pivot receoving, and note to pivot receiving as + // sides + double triangleSide1 = wristLength; + double quadraticA = 1; // law of cos + double quadraticB = -2 * triangleSide1 * Math.cos(wristRollersAngle); + double quadraticC = triangleSide1 * triangleSide1 - triangleBase * triangleBase; + double triangleSide2 = + (-quadraticB + Math.sqrt(quadraticB * quadraticB - 4 * quadraticA * quadraticC)) + / (2 * quadraticA); // pick greater solution + Pose2d intersection = + getCircleIntersection( + wristOriginPos, triangleSide1, pivotReceivingPose, triangleSide2); + double wristAngle = + Math.atan( + (intersection.getY() - wristOriginPos.getY()) + / (wristOriginPos.getX() - intersection.getX())); + if (wristAngle < 0) { // account for domain + wristAngle += Math.PI; + } + wristAngle += wristAngleoffSet; // difference between measured angle and angle to rollers; + return wristAngle; + } + + public Pose2d getCircleIntersection(Pose2d c1, double r1, Pose2d c2, double r2) { + double distance = GeomUtil.distance(c1, c2); + double d1 = (r1 * r1 - r2 * r2 + distance * distance) / (2 * distance); + double h = Math.sqrt(r1 * r1 - d1 * d1); + double x3 = c1.getX() + (d1 * (c2.getX() - c1.getX())) / distance; + double y3 = c1.getY() + (d1 * (c2.getY() - c1.getY())) / distance; + double x4 = x3 - (h * (c2.getY() - c1.getY())) / distance; // get the higher point + double y4 = y3 + (h * (c2.getX() - c1.getX())) / distance; + return new Pose2d(x4, y4, new Rotation2d()); + } +}