-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
67e26ba
commit baa9485
Showing
1 changed file
with
57 additions
and
41 deletions.
There are no files selected for viewing
98 changes: 57 additions & 41 deletions
98
src/main/java/org/ghrobotics/frc2024/ShootingPosition.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,45 +1,61 @@ | ||
| // package org.ghrobotics.frc2024; | ||
| package org.ghrobotics.frc2024; | ||
|
|
||
| import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
|
|
||
|
|
||
| public class ShootingPosition { | ||
|
|
||
|
|
||
| public double distanceToSpeaker(Pose2d robotPose, Pose2d speakerPose) { | ||
| return Math.sqrt(Math.pow(speakerPose.getTranslation().getX() - robotPose.getTranslation().getX(), 2) + Math.pow(speakerPose.getTranslation().getY() - robotPose.getTranslation().getY(), 2)); | ||
| } | ||
|
|
||
| public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap(); | ||
|
|
||
| /** | ||
| * 1st param is distance | ||
| * 2nd param is armAngle | ||
| */ | ||
| static { | ||
| SHOOTER_DISTANCE_ANGLE.put(0.5, 10.0); // Random Numbers for placeholder | ||
| SHOOTER_DISTANCE_ANGLE.put(0.75, 20.0); | ||
| SHOOTER_DISTANCE_ANGLE.put(1.0, 25.0); | ||
| SHOOTER_DISTANCE_ANGLE.put(1.5, 34.0); | ||
| SHOOTER_DISTANCE_ANGLE.put(2.0, 40.124); | ||
|
|
||
| } | ||
|
|
||
|
|
||
| // import edu.wpi.first.math.geometry.Pose2d; | ||
| // import edu.wpi.first.math.geometry.Rotation2d; | ||
|
|
||
| // public class ShootingPosition { | ||
|
|
||
| public double speakerAngle() { | ||
| double angle = ShootingPosition.SHOOTER_DISTANCE_ANGLE.get(distanceToSpeaker(null, null)); | ||
|
|
||
| // For Safety purposes | ||
| // Set the correct max & min arm angles | ||
| if (angle >= 50 || angle <= 10) { | ||
| angle = 0.0; | ||
| } | ||
|
|
||
| return angle; | ||
| } | ||
|
|
||
| // public double armAngleToSpeaker() { | ||
| // double angle; | ||
|
|
||
| // angle = regressionFormula(distanceToSpeaker(null, null)) | ||
|
|
||
| // return angle; | ||
| // } | ||
|
|
||
| public class Constants { | ||
| // Red Subwoofer Pose 2d | ||
| public static Pose2d kRedSubwooferPose = new Pose2d(16.5, 5.57, new Rotation2d(0)); | ||
|
|
||
| // public double distanceToSpeaker(Pose2d robotPose, Pose2d speakerPose) { | ||
| // return Math.sqrt(Math.pow(speakerPose.getTranslation().getX() - robotPose.getTranslation().getX(), 2) + Math.pow(speakerPose.getTranslation().getY() - robotPose.getTranslation().getY(), 2)); | ||
| // } | ||
|
|
||
| // /** | ||
| // * Returns expected angle based on distance to speaker | ||
| // * @param x Distance to the speaker | ||
| // * @return Arm Angle to speaker | ||
| // */ | ||
| // public double regressionFormula(double x) { | ||
| // double y = (10.0286 * Math.log((9.48057 * x) - 8.57204)) + 8.226559; | ||
|
|
||
| // if (y > 50) { | ||
| // y = 2; | ||
| // } else if (y < 0) { | ||
| // y = 2; | ||
| // } | ||
|
|
||
| // return y; | ||
| // } | ||
|
|
||
| // // public double armAngleToSpeaker() { | ||
| // // double angle; | ||
|
|
||
| // // angle = regressionFormula(distanceToSpeaker(null, null)) | ||
|
|
||
| // // return angle; | ||
| // // } | ||
|
|
||
| // public class Constants { | ||
| // // Red Subwoofer Pose 2d | ||
| // public static Pose2d kRedSubwooferPose = new Pose2d(16.5, 5.57, new Rotation2d(0)); | ||
|
|
||
| // // Blue Subwoofer Pose 2d | ||
| // public static Pose2d kBlueSubwooferPose = new Pose2d(0.05, 5.57, new Rotation2d(0)); | ||
| // } | ||
| // } | ||
| // Blue Subwoofer Pose 2d | ||
| public static Pose2d kBlueSubwooferPose = new Pose2d(0.05, 5.57, new Rotation2d(0)); | ||
| } | ||
| } |