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 );