From b59338f5d2084fc2c778d8ba93bc98cef40961e2 Mon Sep 17 00:00:00 2001 From: Udunen Date: Wed, 3 Apr 2024 14:21:40 -0500 Subject: [PATCH] pre load in push pt 2 --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/AutoAim.java | 65 ++++--------------- 2 files changed, 12 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16f204f..44fe587 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -68,7 +68,7 @@ public static class ArmConstants { public static final double armTravelPos = groundArmPos + 0.05; public static final double armLength = Units.inchesToMeters(24.727914); - public static final double encoderPitchDiameter = 62.23*1000; + public static final double encoderPitchDiameter = 62.23/1000; public static final double pivotHeight = Units.inchesToMeters(10.907); public static final double[][] armAngleArray = { // test for different angles from different x distances diff --git a/src/main/java/frc/robot/commands/AutoAim.java b/src/main/java/frc/robot/commands/AutoAim.java index e8d1557..575075c 100644 --- a/src/main/java/frc/robot/commands/AutoAim.java +++ b/src/main/java/frc/robot/commands/AutoAim.java @@ -45,15 +45,16 @@ public AutoAim(Arm a, Shooter s) { public Command aimSpeaker() { // if(LimelightHelpers.getFiducialID("limelight") == VisionConstants.speakerMidTag) { - double kP = 0.0001; - double xDistLLToShooter = ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) + double kP = 0.01; + double xDistLLToShooter = VisionConstants.limelightXDistToArmPivot + + ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) - ShooterConstants.shooterLength*Math.cos(RobotContainer.arm.getRadiansTravelled() - + ShooterConstants.differenceFromArm) - + VisionConstants.limelightXDistToArmPivot; - double yDistLLToShooter = ArmConstants.armLength*Math.sin(RobotContainer.arm.getRadiansTravelled()) + - ShooterConstants.differenceFromArm); + double yDistLLToShooter = -VisionConstants.limelightYDistToArmPivot + + ArmConstants.armLength*Math.sin(RobotContainer.arm.getRadiansTravelled()) - ShooterConstants.shooterLength*Math.sin(RobotContainer.arm.getRadiansTravelled() - + ShooterConstants.differenceFromArm) - - VisionConstants.limelightYDistToArmPivot; + - ShooterConstants.differenceFromArm); + double targetOffsetAngle_Vertical = LimelightHelpers.getTY("limelight"); double angleToGoalDegrees = VisionConstants.limelightDegrees + targetOffsetAngle_Vertical; @@ -87,51 +88,7 @@ public Command aimSpeaker() { // } } - public Command aimSpeakerAuto() { - if(LimelightHelpers.getFiducialID("limelight") == VisionConstants.speakerMidTag) { - double kP = 0.1; - double xDistLLToShooter = ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) - - ShooterConstants.shooterLength*Math.cos(RobotContainer.arm.getRadiansTravelled() - + ShooterConstants.differenceFromArm) - + VisionConstants.limelightXDistToArmPivot; - double yDistLLToShooter = ArmConstants.armLength*Math.sin(RobotContainer.arm.getRadiansTravelled()) - - ShooterConstants.shooterLength*Math.sin(RobotContainer.arm.getRadiansTravelled() - + ShooterConstants.differenceFromArm) - - VisionConstants.limelightYDistToArmPivot; - double targetOffsetAngle_Vertical = LimelightHelpers.getTY("limelight"); - - double angleToGoalDegrees = VisionConstants.limelightDegrees + targetOffsetAngle_Vertical; - double angleToGoalRadians = Units.degreesToRadians(angleToGoalDegrees); - - //calculate distance - double distanceFromLimelightToGoalMeters = (FieldConstants.speakerHeight - VisionConstants.limelightHeight) / Math.tan(angleToGoalRadians); - double xShooterToGoal = distanceFromLimelightToGoalMeters + xDistLLToShooter; - - double yShooterToGoal = FieldConstants.speakerHeight - ArmConstants.pivotHeight - yDistLLToShooter; - double error = stupid(xShooterToGoal, yShooterToGoal)[0] - RobotContainer.arm.getRadiansTravelled(); - double rpmNeeded = stupid(xShooterToGoal, yShooterToGoal)[1]; - - return new RunCommand( - () -> RobotContainer.arm.move(error*kP), - RobotContainer.arm - ).alongWith( - new RunCommand( - () -> RobotContainer.shooter.setShooterRPM(rpmNeeded), - RobotContainer.shooter) - ); - } else { - System.out.println(((DriverStation.getAlliance().get() == DriverStation.Alliance.Red) ? "Red" : "Blue") + " Alliance Mid Speaker AprilTag Not Found!"); - return new RunCommand( - () -> RobotContainer.arm.getDefaultCommand(), - RobotContainer.arm - ).alongWith( - new RunCommand( - () -> RobotContainer.shooter.getDefaultCommand(), - RobotContainer.shooter) - ); - } - } - + // public static double[] stupid(double xDist, double yDist) { // this code is very effective until around 30 feet which is further than podium so it doesnt matter lol // the reason its uneffective at until 30 is bc the note needs to follow the trajectory and enter the speaker @@ -148,10 +105,10 @@ public static double[] stupid(double xDist, double yDist) { //y = -5.401353307, x = -5.60307862645667 is up against subwoofer double a_y = -9.8; // gravity double v_y = 0; - double p_y = -xDist; // vertical distance from target, this will change bc the arm will raise the further it gets away + double p_y = -yDist; // vertical distance from target, this will change bc the arm will raise the further it gets away double a_x = -0; // pretend this doesn't exist because I don't feel like doing air resistance double v_x = 0; - double p_x = -yDist; // horizontal distance from target, this will change bc the robot moves + double p_x = -xDist; // horizontal distance from target, this will change bc the robot moves double shootingvelo_y = Math.sqrt(p_y * a_y * 2); double shootingvelo_x = p_x/(shootingvelo_y/a_y);