From 23abdb2f464bdee8bee58c6e174f5fe58c4d53f3 Mon Sep 17 00:00:00 2001 From: wilsonwatson Date: Sun, 6 Oct 2024 13:11:14 -0500 Subject: [PATCH] shooting is now better. Still needs tuning --- src/main/java/frc/lib/sim/SimulatedArena.java | 44 ++++++++++++++----- .../java/frc/lib/sim/SimulatedPumbaa.java | 29 +++++------- src/main/java/frc/lib/viz/PumbaaViz.java | 41 ++++++++++++++++- 3 files changed, 84 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/lib/sim/SimulatedArena.java b/src/main/java/frc/lib/sim/SimulatedArena.java index f39f7d6..8ca5386 100644 --- a/src/main/java/frc/lib/sim/SimulatedArena.java +++ b/src/main/java/frc/lib/sim/SimulatedArena.java @@ -7,10 +7,11 @@ 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.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.lib.util.FieldConstants; @@ -19,16 +20,33 @@ public class SimulatedArena { private static class ShotNote { public Pose3d pose; - public Translation3d velocity; + ArrayList trajectory = new ArrayList<>(); + double t = 0.0; - public ShotNote(Pose3d pose, Translation3d velocity) { - this.pose = pose; - this.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; + } } public void update(double dt) { - velocity = velocity.minus(new Translation3d(0, 0, 9.81 * dt)); - pose = pose.plus(new Transform3d(velocity.times(dt), NO_ROT)); + 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; } } @@ -37,8 +55,11 @@ public void update(double dt) { private int id = 0; private static final Rotation3d NO_ROT = new Rotation3d(); private List notes = Stream - .concat(Arrays.stream(FieldConstants.StagingLocations.centerlineTranslations), - Arrays.stream(FieldConstants.StagingLocations.spikeTranslations)) + .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 shotNotes = new ArrayList<>(); @@ -58,7 +79,6 @@ public void update(double dt) { for (int i = 0; i < notes.size(); i++) { double distance = notes.get(i).toPose2d().getTranslation() .minus(robot.getPose().getTranslation()).getNorm(); - System.out.println(i + ": " + distance); if (distance < 0.6) { notes.remove(i); robot.intakeOneNote(); @@ -82,8 +102,8 @@ public void update(double dt) { this.shotNotes.stream().map((shotNote) -> shotNote.pose).toArray(Pose3d[]::new)); } - void shootNote(Pose3d pose, Translation3d velocity) { - this.shotNotes.add(new ShotNote(pose, velocity)); + void shootNote(Pose3d pose, Rotation2d yaw, Rotation2d pitch, double velocity) { + this.shotNotes.add(new ShotNote(pose, yaw, pitch, velocity)); } } diff --git a/src/main/java/frc/lib/sim/SimulatedPumbaa.java b/src/main/java/frc/lib/sim/SimulatedPumbaa.java index 220bc3f..50c3e38 100644 --- a/src/main/java/frc/lib/sim/SimulatedPumbaa.java +++ b/src/main/java/frc/lib/sim/SimulatedPumbaa.java @@ -1,6 +1,5 @@ package frc.lib.sim; -import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -65,32 +64,28 @@ public void intakeOneNote() { this.hasNote = true; } + 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); + } + public void advanceNote(double dt, SimulatedArena arena) { Rotation2d yaw = this.getPose().getRotation(); Rotation2d pitch = this.wristAngle; - double x = yaw.getCos() * pitch.getCos(); - double y = yaw.getSin() * pitch.getCos(); - Translation3d shooterSpeed = new Translation3d(x, y, pitch.getSin()); - shooterSpeed = shooterSpeed.times(this.shooterSpeed / SHOOTER_DIV); - Logger.recordOutput("Viz/shooterSpeed", shooterSpeed); - Logger.recordOutput("Viz/shooterDir", yaw); if (hasNote) { if (notePosition < 0.85) { notePosition += intake * dt; } else if (indexer < -0.2) { this.hasNote = false; - arena.shootNote(new Pose3d(this.getPose()), new Translation3d()); + arena.shootNote(new Pose3d(this.getPose()), yaw, Rotation2d.fromDegrees(0.0), 0.0); } else if (notePosition > 1.0) { this.hasNote = false; - 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()); - Pose3d fromPose = new Pose3d(t, r); - Logger.recordOutput("Viz/fromPose", fromPose); - arena.shootNote(fromPose, shooterSpeed); + arena.shootNote(this.getShootFrom(), yaw, pitch, this.shooterSpeed / SHOOTER_DIV); } else { notePosition += indexer * dt; } diff --git a/src/main/java/frc/lib/viz/PumbaaViz.java b/src/main/java/frc/lib/viz/PumbaaViz.java index 137c67a..4cdf999 100644 --- a/src/main/java/frc/lib/viz/PumbaaViz.java +++ b/src/main/java/frc/lib/viz/PumbaaViz.java @@ -1,5 +1,7 @@ package frc.lib.viz; +import java.util.ArrayList; +import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -21,6 +23,8 @@ public class PumbaaViz { private NoteLocation noteLocation = NoteLocation.None; private final SimulatedPumbaa sim; private Rotation2d wristAngle; + private double height; + private Pose2d robotPose = new Pose2d(); private final Rotation3d NO_ROT = new Rotation3d(); private final Pose3d NO_NOTE_POSE = new Pose3d(0, 0, -100.0, NO_ROT); @@ -43,6 +47,7 @@ public PumbaaViz(String prefix, SimulatedPumbaa sim) { } public void setElevatorWrist(double height, Rotation2d wristAngle) { + this.height = height; height = Units.inchesToMeters(height - Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT); this.wristAngle = wristAngle; @@ -57,7 +62,7 @@ public void setElevatorWrist(double height, Rotation2d wristAngle) { } public void setPose(Pose2d pose) { - + this.robotPose = pose; } public static enum NoteLocation { @@ -80,6 +85,20 @@ public void setNoteLocation(NoteLocation location) { this.noteLocation = location; } + private static final double SHOOTER_FRONT = 0.271162; + + public Pose3d getShootFrom() { + Translation3d t = new Translation3d( + this.robotPose.getX() + SHOOTER_FRONT * this.robotPose.getRotation().getCos(), + this.robotPose.getY() + SHOOTER_FRONT * this.robotPose.getRotation().getSin(), + Units.inchesToMeters(height)); + Rotation3d r = + new Rotation3d(0.0, wristAngle.getRadians(), this.robotPose.getRotation().getRadians()); + return new Pose3d(t, r); + } + + ArrayList trajectory = new ArrayList<>(); + public void update() { if (this.sim != null) { if (this.sim.hasNote()) { @@ -105,8 +124,28 @@ public void update() { this.notePose = NO_NOTE_POSE; } } + recalculateTrajectory(); + Logger.recordOutput(prefix + "/trajectory", trajectory.toArray(Pose3d[]::new)); Logger.recordOutput(prefix + "/components", new Pose3d[] {shooterPose, elevatorBottomPose, elevatorTopPose, notePose}); } + private static final double SHOOT_SPEED = 33.0; + + void recalculateTrajectory() { + trajectory.clear(); + Pose3d startPose = getShootFrom(); + double vz = wristAngle.getSin() * SHOOT_SPEED; + double vx = robotPose.getRotation().getCos() * SHOOT_SPEED; + double vy = robotPose.getRotation().getSin() * SHOOT_SPEED; + 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; + } + } + }