Skip to content

Commit

Permalink
shooting is now better. Still needs tuning
Browse files Browse the repository at this point in the history
  • Loading branch information
wilsonwatson committed Oct 6, 2024
1 parent e524bc9 commit 23abdb2
Show file tree
Hide file tree
Showing 3 changed files with 84 additions and 30 deletions.
44 changes: 32 additions & 12 deletions src/main/java/frc/lib/sim/SimulatedArena.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -19,16 +20,33 @@ public class SimulatedArena {

private static class ShotNote {
public Pose3d pose;
public Translation3d velocity;
ArrayList<Pose3d> 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;
}
}

Expand All @@ -37,8 +55,11 @@ public void update(double dt) {
private int id = 0;
private static final Rotation3d NO_ROT = new Rotation3d();
private List<Pose3d> 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<ShotNote> shotNotes = new ArrayList<>();
Expand All @@ -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();
Expand All @@ -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));
}

}
29 changes: 12 additions & 17 deletions src/main/java/frc/lib/sim/SimulatedPumbaa.java
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -65,32 +64,28 @@ public void intakeOneNote() {
this.hasNote = true;
}

public Pose3d getShootFrom() {

Check warning on line 67 in src/main/java/frc/lib/sim/SimulatedPumbaa.java

View workflow job for this annotation

GitHub Actions / Linting

[checkstyle] reported by reviewdog 🐶 Missing a Javadoc comment. Raw Output: /github/workspace/./src/main/java/frc/lib/sim/SimulatedPumbaa.java:67:5: warning: Missing a Javadoc comment. (com.puppycrawl.tools.checkstyle.checks.javadoc.MissingJavadocMethodCheck)
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) {

Check warning on line 77 in src/main/java/frc/lib/sim/SimulatedPumbaa.java

View workflow job for this annotation

GitHub Actions / Linting

[checkstyle] reported by reviewdog 🐶 Missing a Javadoc comment. Raw Output: /github/workspace/./src/main/java/frc/lib/sim/SimulatedPumbaa.java:77:5: warning: Missing a Javadoc comment. (com.puppycrawl.tools.checkstyle.checks.javadoc.MissingJavadocMethodCheck)
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;
}
Expand Down
41 changes: 40 additions & 1 deletion src/main/java/frc/lib/viz/PumbaaViz.java
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -57,7 +62,7 @@ public void setElevatorWrist(double height, Rotation2d wristAngle) {
}

public void setPose(Pose2d pose) {

this.robotPose = pose;
}

public static enum NoteLocation {
Expand All @@ -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<Pose3d> trajectory = new ArrayList<>();

public void update() {
if (this.sim != null) {
if (this.sim.hasNote()) {
Expand All @@ -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;
}
}

}

0 comments on commit 23abdb2

Please sign in to comment.