[Robot] Fadeaway that actually works?!

SouthwestRoboticsProgramming · Apr 12, 2024 · e24b3e0 · e24b3e0
1 parent 8796600
commit e24b3e0
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 11 deletions.
diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java
@@ -92,7 +92,8 @@ public final class NTData {
  public static final NTEntry<Double> SHOOTER_LOB_SHORT_HEIGHT_METERS = new NTDouble("Shooter/Lob/Short Lob Height (m)", 1.3).setPersistent();
  public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_BLUE = new NTDouble("Shooter/Lob/Drive Angle Correction Blue (deg)", -30).setPersistent();
  public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_RED = new NTDouble("Shooter/Lob/Drive Angle Correction Red (deg)", 0).setPersistent();
- public static final NTEntry<Double> SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25);
+ public static final NTEntry<Double> SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25).setPersistent();
+ public static final NTEntry<Double> GREEN_FN_COEFFICIENT = new NTDouble("Shooter/Green FN Coefficient", 1).setPersistent();
 
  private NTData() {
  throw new AssertionError();

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
@@ -43,7 +43,7 @@ public enum FlywheelControl {
 
  private AimCalculator.Aim targetAim; // Target aim is null if not currently aiming
  private AimCalculator aimCalculator;
- private final AimCalculator tableAimCalculator;
+ private final TableAimCalculator tableAimCalculator;
 
  private FlywheelControl flywheelControl;
 
@@ -98,9 +98,21 @@ public void periodic() {
  aim.pivotAngle());
  } else {
  double distToSpeaker = getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation());
- aim = aimCalculator.calculateAim(distToSpeaker);
+ Pose2d robotPose = drive.getEstimatedPose();
+ Translation2d robotPos = robotPose.getTranslation();
+ ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds();
+
+ Translation2d target = getSpeakerPosition();
+ Rotation2d angleToTarget = target.minus(robotPos).getAngle();
+
+ // Relative to the target
+ Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
+ aim = tableAimCalculator.calculateAim(distToSpeaker, robotVelocity.getX());
 
  if (RobotBase.isSimulation())
+ SimView.lobTrajectory.update(
+ aim.flywheelVelocity() / NTData.SHOOTER_LOB_POWER_COEFFICIENT.get(),
+ aim.pivotAngle());
  SimView.lobTrajectory.clear();
  }
  targetAim = aim;

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java
@@ -102,15 +102,20 @@ private double sample(TreeMap<Double, Double> map, double distanceToSpeaker) {
 
  @Override
  public Aim calculateAim(double distanceToSpeaker) {
- logDistance.set(distanceToSpeaker);
-
- distanceToSpeaker *= NTData.SHOOTER_DISTANCE_SCALE.get();
+ return calculateAim(distanceToSpeaker, 0.0);
+ }
 
- return new Aim(
- sample(flywheelVelocityMap, distanceToSpeaker),
- sample(pivotAngleMap, distanceToSpeaker),
- distanceToSpeaker
- );
+ public Aim calculateAim(double distanceToSpeaker, double velocityTowardsSpeaker) {
+ logDistance.set(distanceToSpeaker);
+ System.out.println(velocityTowardsSpeaker);
+
+ distanceToSpeaker *= NTData.SHOOTER_DISTANCE_SCALE.get();
+
+ return new Aim(
+ sample(flywheelVelocityMap, distanceToSpeaker),
+ sample(pivotAngleMap, distanceToSpeaker) + Math.toRadians(NTData.GREEN_FN_COEFFICIENT.get()) * velocityTowardsSpeaker,
+ distanceToSpeaker
+ );
  }
 
  public double getSnapDistance(double currentDist, boolean wantMoveCloser) {