From b30c7545449b979896713f2ad7fb05366f00c5b7 Mon Sep 17 00:00:00 2001
From: Ian Shi <Ianshi1026@gmail.com>
Date: Mon, 17 Jun 2024 22:53:11 -0400
Subject: [PATCH] ferry from anywhere on field
Co-authored-by: k4limul <k4limul@users.noreply.github.com>
---
.../com/stuypulse/robot/RobotContainer.java | 21 ++--
.../SwerveDriveFerryFromCurrentPosition.java | 115 ++++++++++++++++++
.../commands/swerve/SwerveDriveToFerry.java | 2 +-
3 files changed, 128 insertions(+), 10 deletions(-)
create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFerryFromCurrentPosition.java
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index 2b97ce31..6dadfccd 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -212,16 +212,19 @@ private void configureDriverBindings() {
// .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
// .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))));
+ // driver.getTopButton()
+ // .onTrue(new ShooterSetRPM(Settings.Shooter.FERRY))
+ // .whileTrue(new SwerveDriveToFerry()
+ // .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
+ // .andThen(new ShooterWaitForTarget()
+ // .withTimeout(0.5)))
+ // .andThen(new ConveyorShoot()))
+ // .onFalse(new ConveyorStop())
+ // .onFalse(new IntakeStop())
+ // .onFalse(new ShooterSetRPM(Settings.Shooter.PODIUM_SHOT));
+
driver.getTopButton()
- .onTrue(new ShooterSetRPM(Settings.Shooter.FERRY))
- .whileTrue(new SwerveDriveToFerry()
- .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
- .andThen(new ShooterWaitForTarget()
- .withTimeout(0.5)))
- .andThen(new ConveyorShoot()))
- .onFalse(new ConveyorStop())
- .onFalse(new IntakeStop())
- .onFalse(new ShooterSetRPM(Settings.Shooter.PODIUM_SHOT));
+ .whileTrue(new SwerveDriveFerryFromCurrentPosition());
// climb
driver.getRightButton()
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFerryFromCurrentPosition.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFerryFromCurrentPosition.java
new file mode 100644
index 00000000..4546a6d4
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFerryFromCurrentPosition.java
@@ -0,0 +1,115 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.subsystems.conveyor.Conveyor;
+import com.stuypulse.robot.subsystems.intake.Intake;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.control.angle.AngleController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.math.Angle;
+import com.stuypulse.stuylib.math.Vector2D;
+import com.stuypulse.stuylib.streams.numbers.IStream;
+import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
+import com.stuypulse.stuylib.util.AngleVelocity;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Swerve.Assist;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+
+public class SwerveDriveFerryFromCurrentPosition extends Command {
+
+ private final SwerveDrive swerve;
+ private final Odometry odometry;
+ private final Shooter shooter;
+ private final Conveyor conveyor;
+ private final Intake intake;
+
+ private final AngleController controller;
+ private final IStream angleVelocity;
+
+ public SwerveDriveFerryFromCurrentPosition() {
+ swerve = SwerveDrive.getInstance();
+ shooter = Shooter.getInstance();
+ odometry = Odometry.getInstance();
+ conveyor = Conveyor.getInstance();
+ intake = Intake.getInstance();
+
+ controller = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD)
+ .setOutputFilter(x -> -x);
+
+ AngleVelocity derivative = new AngleVelocity();
+
+ angleVelocity = IStream.create(() -> derivative.get(Angle.fromRotation2d(getTargetAngle())))
+ .filtered(new LowPassFilter(Assist.ANGLE_DERIV_RC))
+ // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST
+ // so that angular velocity doesn't oscillate
+ .filtered(x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST))
+ .filtered(x -> -x);
+
+ addRequirements(swerve, shooter, odometry, conveyor, intake);
+ }
+
+ // returns pose of close amp corner
+ private Translation2d getTargetPose() {
+
+ Translation2d pose = Robot.isBlue()
+ ? new Translation2d(0, Field.WIDTH - 1.5)
+ : new Translation2d(0, 1.5);
+
+ return pose;
+
+ // Translation2d offset = odometry.getRobotVelocity()
+ // .times(NOTE_TRAVEL_TIME);
+
+ // return pose.minus(offset);
+ }
+
+ private Rotation2d getTargetAngle() {
+ return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle();
+ }
+
+ private double getDistanceToTarget() {
+ return odometry.getPose().getTranslation().getDistance(getTargetPose());
+ }
+
+ private double getAngleTolerance() {
+ double distance = getDistanceToTarget();
+ final double SHOT_LANDING_TOLERANCE = 1.0;
+
+ return Math.toDegrees(Math.atan(SHOT_LANDING_TOLERANCE / distance));
+ }
+
+ @Override
+ public void initialize() {
+ swerve.stop();
+ shooter.setTargetSpeeds(Settings.Shooter.FERRY);
+ }
+
+ @Override
+ public void execute() {
+ double omega = angleVelocity.get() + controller.update(
+ Angle.fromRotation2d(getTargetAngle()),
+ Angle.fromRotation2d(odometry.getPose().getRotation()));
+
+ SmartDashboard.putNumber("Ferry/Angle Tolerance", getAngleTolerance());
+
+ intake.acquire();
+ conveyor.toShooter();
+
+ swerve.drive(new Vector2D(new Translation2d(0, 0)), omega);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ intake.stop();
+ conveyor.stop();
+ shooter.setTargetSpeeds(Settings.Shooter.PODIUM_SHOT);
+ }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java
index 25dc55f2..0bb04b57 100644
--- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java
+++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java
@@ -33,7 +33,7 @@ public SwerveDriveToFerry() {
// use robot's y, put in x and rotation
return new Pose2d(
- SLMath.clamp(robot.getX(), Field.FERRY_SHOT_MIN_X, Field.FERRY_SHOT_MAX_X),
+ Field.CONST_FERRY_X,
Robot.isBlue() ? Math.max(robot.getY(), Field.FERRY_CUTOFF) : Math.min(robot.getY(), Field.WIDTH - Field.FERRY_CUTOFF),
targetAngle
);