generated from Frc5572/Java-Template
-
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.
Merge remote-tracking branch 'origin/main' into fused-cancoders
- Loading branch information
Showing
30 changed files
with
1,157 additions
and
107 deletions.
There are no files selected for viewing
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
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
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 |
|---|---|---|
| @@ -0,0 +1,70 @@ | ||
| { | ||
| "name": "Pumbaa", | ||
| "sourceUrl": "", | ||
| "rotations": [ | ||
| { | ||
| "axis": "x", | ||
| "degrees": 90.0 | ||
| } | ||
| ], | ||
| "position": [ | ||
| 0.0, | ||
| 0.0, | ||
| 0.0 | ||
| ], | ||
| "cameras": [], | ||
| "components": [ | ||
| { | ||
| "zeroedRotations": [ | ||
| { | ||
| "axis": "x", | ||
| "degrees": 90.0 | ||
| } | ||
| ], | ||
| "zeroedPosition": [ | ||
| 0.0, | ||
| 0.0, | ||
| 0.0 | ||
| ] | ||
| }, | ||
| { | ||
| "zeroedRotations": [ | ||
| { | ||
| "axis": "x", | ||
| "degrees": 90.0 | ||
| } | ||
| ], | ||
| "zeroedPosition": [ | ||
| 0.0, | ||
| 0.0, | ||
| 0.0 | ||
| ] | ||
| }, | ||
| { | ||
| "zeroedRotations": [ | ||
| { | ||
| "axis": "x", | ||
| "degrees": 90.0 | ||
| } | ||
| ], | ||
| "zeroedPosition": [ | ||
| 0.0, | ||
| 0.0, | ||
| 0.0 | ||
| ] | ||
| }, | ||
| { | ||
| "zeroedRotations": [ | ||
| { | ||
| "axis": "x", | ||
| "degrees": 90.0 | ||
| } | ||
| ], | ||
| "zeroedPosition": [ | ||
| 0.0, | ||
| 0.0, | ||
| 0.0 | ||
| ] | ||
| } | ||
| ] | ||
| } |
Git LFS file not shown
Git LFS file not shown
Git LFS file not shown
Git LFS file not shown
Git LFS file not shown
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 |
|---|---|---|
| @@ -0,0 +1,144 @@ | ||
| package frc.lib.sim; | ||
|
|
||
| import java.util.ArrayList; | ||
| import java.util.Arrays; | ||
| import java.util.HashSet; | ||
| import java.util.List; | ||
| import java.util.Set; | ||
| import java.util.stream.Collectors; | ||
| import java.util.stream.Stream; | ||
| import org.littletonrobotics.junction.LoggedRobot; | ||
| import org.littletonrobotics.junction.Logger; | ||
| import edu.wpi.first.math.geometry.Pose3d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
| import edu.wpi.first.math.geometry.Translation3d; | ||
| import edu.wpi.first.math.util.Units; | ||
| import frc.lib.util.FieldConstants; | ||
|
|
||
| /** | ||
| * Simulated Arena | ||
| */ | ||
| public class SimulatedArena { | ||
|
|
||
| /** | ||
| * Shooting Note | ||
| */ | ||
| private static class ShotNote { | ||
| public Pose3d pose; | ||
| ArrayList<Pose3d> trajectory = new ArrayList<>(); | ||
| double t = 0.0; | ||
|
|
||
| /** | ||
| * Shooting Note | ||
| * | ||
| * @param startPose Starting Pose | ||
| * @param yaw Yaw | ||
| * @param pitch Pitch | ||
| * @param velocity Velocity | ||
| */ | ||
| public ShotNote(Pose3d startPose, Rotation2d yaw, Rotation2d pitch, double velocity) { | ||
| this.pose = startPose; | ||
| double vz = pitch.getSin() * velocity; | ||
| double vx = yaw.getCos() * velocity; | ||
| double vy = yaw.getSin() * velocity; | ||
| double prevZ = startPose.getZ(); | ||
| for (double t = 0.0; prevZ > 0.0; t += LoggedRobot.defaultPeriodSecs) { | ||
| double x = startPose.getX() + vx * t; | ||
| double y = startPose.getY() + vy * t; | ||
| double z = startPose.getZ() + vz * t - 9.81 * t * t; | ||
| trajectory.add(new Pose3d(x, y, z, new Rotation3d())); | ||
| prevZ = z; | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Update Viz | ||
| */ | ||
| public void update(double dt) { | ||
| int index = (int) Math.floor(t / LoggedRobot.defaultPeriodSecs); | ||
| if (index >= trajectory.size()) { | ||
| pose = trajectory.get(trajectory.size() - 1); | ||
| pose = new Pose3d(pose.getX(), pose.getY(), 0.0, new Rotation3d()); | ||
| } else { | ||
| pose = trajectory.get(index); | ||
| } | ||
| t += dt; | ||
| } | ||
| } | ||
|
|
||
| private static final double NOTE_HEIGHT = Units.inchesToMeters(1.7); | ||
| private Set<SimulatedPumbaa> robots = new HashSet<>(); | ||
| private int id = 0; | ||
| private static final Rotation3d NO_ROT = new Rotation3d(); | ||
| private List<Pose3d> notes = Stream | ||
| .concat( | ||
| Stream.concat(Arrays.stream(FieldConstants.StagingLocations.centerlineTranslations), | ||
| Arrays.stream(FieldConstants.StagingLocations.spikeTranslations)), | ||
| Arrays.stream(FieldConstants.StagingLocations.spikeTranslations) | ||
| .map((p) -> FieldConstants.allianceFlip(p))) | ||
| .map((pos) -> new Pose3d(new Translation3d(pos.getX(), pos.getY(), NOTE_HEIGHT), NO_ROT)) | ||
| .collect(Collectors.toList()); | ||
| public List<ShotNote> shotNotes = new ArrayList<>(); | ||
|
|
||
| /** | ||
| * Create a new simulated robot | ||
| * | ||
| * @return Simulated Robot | ||
| */ | ||
| public SimulatedPumbaa newPumbaa() { | ||
| SimulatedPumbaa dt = new SimulatedPumbaa(this.id++); | ||
| robots.add(dt); | ||
| return dt; | ||
| } | ||
|
|
||
| /** | ||
| * Update Viz | ||
| * | ||
| * @param dt Change in time | ||
| */ | ||
| public void update(double dt) { | ||
| robots: for (SimulatedPumbaa robot : this.robots) { | ||
| Logger.recordOutput("Viz/Robot" + robot.id, robot.getPose()); | ||
| robot.advanceNote(dt, this); | ||
| if (robot.couldIntake()) { | ||
| System.out.println("checking"); | ||
| for (int i = 0; i < notes.size(); i++) { | ||
| double distance = notes.get(i).toPose2d().getTranslation() | ||
| .minus(robot.getPose().getTranslation()).getNorm(); | ||
| if (distance < 0.6) { | ||
| notes.remove(i); | ||
| robot.intakeOneNote(); | ||
| continue robots; | ||
| } | ||
| } | ||
| } | ||
| } | ||
| for (int i = 0; i < shotNotes.size(); i++) { | ||
| shotNotes.get(i).update(dt); | ||
| if (shotNotes.get(i).pose.getZ() < NOTE_HEIGHT) { | ||
| notes.add(new Pose3d(shotNotes.get(i).pose.getX(), shotNotes.get(i).pose.getY(), | ||
| NOTE_HEIGHT, NO_ROT)); | ||
| shotNotes.remove(i); | ||
| i--; | ||
| continue; | ||
| } | ||
| } | ||
| Logger.recordOutput("Viz/Notes", this.notes.toArray(Pose3d[]::new)); | ||
| Logger.recordOutput("Viz/ShotNotes", | ||
| this.shotNotes.stream().map((shotNote) -> shotNote.pose).toArray(Pose3d[]::new)); | ||
| } | ||
|
|
||
| /** | ||
| * Shoot note | ||
| * | ||
| * @param pose Shooting Pose | ||
| * @param yaw Shooting Yaw | ||
| * @param pitch Shooting Pitch | ||
| * @param velocity Shooting Velocity | ||
| */ | ||
| void shootNote(Pose3d pose, Rotation2d yaw, Rotation2d pitch, double velocity) { | ||
| this.shotNotes.add(new ShotNote(pose, yaw, pitch, velocity)); | ||
| } | ||
|
|
||
| } |
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 |
|---|---|---|
| @@ -0,0 +1,124 @@ | ||
| package frc.lib.sim; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Pose3d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
| import edu.wpi.first.math.geometry.Translation3d; | ||
| import edu.wpi.first.math.util.Units; | ||
|
|
||
| /** | ||
| * Simulated Pumbaa | ||
| */ | ||
| public class SimulatedPumbaa { | ||
|
|
||
| public final int id; | ||
|
|
||
| /** | ||
| * Simulated Pumbaa | ||
| */ | ||
| SimulatedPumbaa(int id) { | ||
| this.id = id; | ||
| } | ||
|
|
||
| private static final double SHOOTER_DIV = 100.0; | ||
| private static final double SHOOTER_FRONT = 0.271162; | ||
| private Pose2d pose = new Pose2d(); | ||
| private double intake = 0.0; | ||
| private double indexer = 0.0; | ||
| private double notePosition = 0.9; | ||
| private boolean hasNote = true; | ||
| private double shooterSpeed = 0.0; | ||
| private double height = 24.0; | ||
| private Rotation2d wristAngle = new Rotation2d(); | ||
|
|
||
| public double notePosition() { | ||
| return this.notePosition; | ||
| } | ||
|
|
||
| public void setShooterSpeed(double speed) { | ||
| this.shooterSpeed = speed; | ||
| } | ||
|
|
||
| public void setElevatorWrist(double height, Rotation2d wristAngle) { | ||
| this.height = height; | ||
| this.wristAngle = wristAngle; | ||
| } | ||
|
|
||
| public Pose2d getPose() { | ||
| return pose; | ||
| } | ||
|
|
||
| public void setPose(Pose2d pose) { | ||
| this.pose = pose; | ||
| } | ||
|
|
||
| public boolean couldIntake() { | ||
| return intake > 0.2; | ||
| } | ||
|
|
||
| public void setIntake(double value) { | ||
| this.intake = value; | ||
| } | ||
|
|
||
| public void setIndexer(double value) { | ||
| this.indexer = value; | ||
| } | ||
|
|
||
| public void intakeOneNote() { | ||
| this.notePosition = 0.0; | ||
| this.hasNote = true; | ||
| } | ||
|
|
||
| /** | ||
| * Get Shot Pose | ||
| * | ||
| * @return Pose shooting from | ||
| */ | ||
| public Pose3d getShootFrom() { | ||
| Translation3d t = new Translation3d( | ||
| this.getPose().getX() + SHOOTER_FRONT * this.getPose().getRotation().getCos(), | ||
| this.getPose().getY() + SHOOTER_FRONT * this.getPose().getRotation().getSin(), | ||
| Units.inchesToMeters(height)); | ||
| Rotation3d r = | ||
| new Rotation3d(0.0, wristAngle.getRadians(), this.getPose().getRotation().getRadians()); | ||
| return new Pose3d(t, r); | ||
| } | ||
|
|
||
| /** | ||
| * Advance Note | ||
| * | ||
| * @param dt Change in time | ||
| * @param arena Simulated Arena | ||
| */ | ||
| public void advanceNote(double dt, SimulatedArena arena) { | ||
| Rotation2d yaw = this.getPose().getRotation(); | ||
| Rotation2d pitch = this.wristAngle; | ||
| if (hasNote) { | ||
| if (notePosition < 0.85) { | ||
| notePosition += intake * dt; | ||
| } else if (indexer < -0.2) { | ||
| this.hasNote = false; | ||
| arena.shootNote(new Pose3d(this.getPose()), yaw, Rotation2d.fromDegrees(0.0), 0.0); | ||
| } else if (notePosition > 1.0) { | ||
| this.hasNote = false; | ||
| arena.shootNote(this.getShootFrom(), yaw, pitch, this.shooterSpeed / SHOOTER_DIV); | ||
| } else { | ||
| notePosition += indexer * dt; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| public boolean hasNote() { | ||
| return this.hasNote; | ||
| } | ||
|
|
||
| public boolean lowerBeamBreak() { | ||
| return hasNote && notePosition < 0.8; | ||
| } | ||
|
|
||
| public boolean upperBeamBreak() { | ||
| return hasNote && notePosition > 0.9 && notePosition < 1.0; | ||
| } | ||
|
|
||
| } |
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
Oops, something went wrong.