From a366ae1ccf87a0fc66c8ea1798d7d0f9216632bc Mon Sep 17 00:00:00 2001 From: colyic Date: Fri, 17 May 2024 08:07:53 -0400 Subject: [PATCH 1/6] Add SwerveDriveToFerry --- simgui.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 10 +- .../commands/swerve/SwerveDriveToFerry.java | 167 ++++++++++++++++++ .../commands/swerve/SwerveDriveToPose.java | 1 - .../stuypulse/robot/constants/Settings.java | 1 + 5 files changed, 178 insertions(+), 2 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java diff --git a/simgui.json b/simgui.json index a3ebfcfa..01d8bb0a 100644 --- a/simgui.json +++ b/simgui.json @@ -19,6 +19,7 @@ "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Scheduler": "Scheduler", "/SmartDashboard/Visualizers/Climber": "Mechanism2d", "/SmartDashboard/Visualizers/Intake": "Mechanism2d", "/SmartDashboard/Visualizers/Lift": "Mechanism2d" diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 67c36bb9..5007210b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -211,7 +211,15 @@ private void configureDriverBindings() { // .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))); driver.getTopButton() - .whileTrue(new SwerveDriveAutoFerry(driver)); + .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()); + // .whileTrue(new SwerveDriveAutoFerry(driver)); // climb driver.getRightButton() diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java new file mode 100644 index 00000000..3ae67081 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java @@ -0,0 +1,167 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Alignment.Shoot; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.Derivative; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveToFerry extends Command { + + private final SwerveDrive swerve; + private final Odometry odometry; + + private final Controller distanceController; + private final AnglePIDController angleController; + private final IStream velocityError; + + private final IStream distanceToSpeaker; + private final BStream isAligned; + + private final Number targetDistance; + + private double distanceTolerance; + private double angleTolerance; + private double velocityTolerance; + + public SwerveDriveToFerry() { + this(Alignment.FERRY_SHOT_DISTANCE); + } + + public SwerveDriveToFerry(Number targetDistance) { + this(targetDistance, Alignment.DEBOUNCE_TIME); + } + + public SwerveDriveToFerry(Number targetDistance, double debounce) { + this.targetDistance = targetDistance; + + swerve = SwerveDrive.getInstance(); + odometry = Odometry.getInstance(); + + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) + .setOutputFilter( + x -> -x, + x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) + ); + + angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); + + velocityError = IStream.create(distanceController::getError) + .filtered(new Derivative()) + .filtered(new LowPassFilter(0.05)) + .filtered(x -> Math.abs(x)); + + distanceToSpeaker = IStream.create(() -> getTranslationToPose().getNorm()) + .filtered(new LowPassFilter(0.05)); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Rising(debounce)); + + distanceTolerance = 0.05; + angleTolerance = Alignment.ANGLE_TOLERANCE.get(); + velocityTolerance = 0.1; + + addRequirements(swerve); + } + + private Translation2d getTargetPose() { + Translation2d pose = Robot.isBlue() + ? new Translation2d(0, Field.WIDTH - 1.5) + : new Translation2d(0, 1.5); + + return pose; + } + + private Rotation2d getTargetAngle() { + return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle(); + } + + private Translation2d getTranslationToPose() { + return getTargetPose().minus(odometry.getPose().getTranslation()); + } + + private double getTargetDistance() { + return SLMath.clamp(targetDistance.doubleValue(), 1, 5); + } + + public SwerveDriveToFerry withTolerance(double distanceTolerance, double angleTolerance) { + this.distanceTolerance = distanceTolerance; + this.angleTolerance = angleTolerance; + return this; + } + + public SwerveDriveToFerry withRotationConstants(double p, double i, double d) { + angleController.setPID(p, i, d); + return this; + } + + private boolean isAligned() { + return distanceController.isDone(distanceTolerance) + && angleController.isDoneDegrees(angleTolerance) + && velocityError.get() < velocityTolerance; + } + + @Override + public void initialize() { + SmartDashboard.putBoolean("AutonAlignment", true); + } + + @Override + public void execute() { + double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); + double rotation = angleController.update( + Angle.fromRotation2d(getTargetAngle()).add(Angle.k180deg), + Angle.fromRotation2d(odometry.getPose().getRotation())); + + Translation2d speeds = new Translation2d( + speed, + getTargetAngle()); + + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + swerve.setFieldRelativeSpeeds( + new ChassisSpeeds( + speeds.getX(), + speeds.getY(), + rotation)); + + SmartDashboard.putNumber("Alignment/Velocity Error", velocityError.get()); + } + + @Override + public boolean isFinished() { + return isAligned.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + SmartDashboard.putBoolean("AutonAlignment", false); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java index ca2db889..305b3169 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -13,7 +13,6 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.Derivative; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.pathplanner.lib.util.PIDConstants; import com.stuypulse.robot.constants.Field; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 70ab23a3..dfd34836 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -400,6 +400,7 @@ public interface Alignment { SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + SmartNumber FERRY_SHOT_DISTANCE = new SmartNumber("Shooter/Ferry Shot Distance", 7.0); SmartNumber PODIUM_SHOT_DISTANCE = new SmartNumber("Shooter/Podium Distance", 2.85); // 2.75 in lab double PODIUM_SHOT_MAX_ANGLE = 80; From f24a2a596290c2f2cd407e9580b94befe38a6496 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Fri, 17 May 2024 13:22:21 -0500 Subject: [PATCH 2/6] Make amp motion uninterruptable (very scuffed and maybe has unintended consequences) --- .../com/stuypulse/robot/RobotContainer.java | 6 +- .../robot/commands/AmpScoreRoutine.java | 4 +- .../conveyor/ConveyorToAmpUnsafe.java | 65 +++++++++++++++++++ 3 files changed, 72 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5007210b..ff2c43db 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -50,6 +50,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -148,8 +149,9 @@ private void configureDriverBindings() { // note to amper and align then score driver.getLeftBumper() - .whileTrue(new AmpScoreRoutine()) - .onFalse(new AmperStop()) + .onTrue(new ConveyorToAmpUnsafe()) // requirements: conveyor, intake + .whileTrue(new AmpScoreRoutine()) // requirements: swerve, amper + .onFalse(new ConditionalCommand(new AmperStop(), new InstantCommand(), () -> !Amper.getInstance().hasNote())) .onFalse(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT)); // score speaker no align diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 9ba14b32..627c25f6 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -20,6 +20,7 @@ import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Amper.Lift; import com.stuypulse.robot.constants.Settings.Amper.Score; +import com.stuypulse.robot.subsystems.amper.Amper; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.stuylib.math.Vector2D; @@ -31,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class AmpScoreRoutine extends SequentialCommandGroup { @@ -57,7 +59,7 @@ private static Pose2d getTargetPose(double distanceToWall) { public AmpScoreRoutine() { addCommands( new ParallelCommandGroup( - new ConveyorToAmp(), + new WaitUntilCommand(() -> Amper.getInstance().hasNote()), new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get())) .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE) .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN)) diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java new file mode 100644 index 00000000..0d3e9aa5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java @@ -0,0 +1,65 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.conveyor; + +import com.stuypulse.robot.commands.amper.AmperToHeight; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Amper.Lift; +import com.stuypulse.robot.subsystems.amper.Amper; +import com.stuypulse.robot.subsystems.conveyor.Conveyor; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ConveyorToAmpUnsafe extends Command { + public static Command withCheckLift() { + return AmperToHeight.untilBelow(Lift.MIN_HEIGHT, Lift.MAX_HEIGHT_ERROR) + .andThen(new ConveyorToAmp()); + } + + private final Conveyor conveyor; + private final Shooter shooter; + private final Intake intake; + private final Amper amper; + + public ConveyorToAmpUnsafe() { + conveyor = Conveyor.getInstance(); + shooter = Shooter.getInstance(); + intake = Intake.getInstance(); + amper = Amper.getInstance(); + + addRequirements(conveyor, intake); + } + + @Override + public void initialize() { + shooter.setTargetSpeeds(Settings.Shooter.HANDOFF); + } + + @Override + public void execute() { + if (shooter.atTargetSpeeds()) { + conveyor.toAmp(); + intake.acquire(); + amper.fromConveyor(); + } + } + + @Override + public boolean isFinished() { + return amper.hasNote(); + } + + @Override + public void end(boolean interrupted) { + conveyor.stop(); + // shooter.stop(); + intake.stop(); + amper.stopRoller(); + } +} From a72e3767aedfcb7a6d7362e92101ae81f3daa340 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Fri, 17 May 2024 13:45:05 -0500 Subject: [PATCH 3/6] Move amp score in slightly for lab --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index dfd34836..fbceec41 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -405,7 +405,7 @@ public interface Alignment { double PODIUM_SHOT_MAX_ANGLE = 80; SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5)); - SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5)); + SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.5)); // NOTE: remove "- 1.5" for battlecry SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(21.0)); SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0)); From d49b4346219a42489cfbbb90531957ed71dfaa94 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Fri, 17 May 2024 13:45:40 -0500 Subject: [PATCH 4/6] Fix constant x ferry command --- .../com/stuypulse/robot/RobotContainer.java | 3 +- .../commands/swerve/SwerveDriveToFerry.java | 162 ++---------------- .../com/stuypulse/robot/constants/Field.java | 2 + 3 files changed, 22 insertions(+), 145 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ff2c43db..ffb22f38 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -220,7 +220,8 @@ private void configureDriverBindings() { .withTimeout(0.5))) .andThen(new ConveyorShoot())) .onFalse(new ConveyorStop()) - .onFalse(new IntakeStop()); + .onFalse(new IntakeStop()) + .onFalse(new ShooterSetRPM(Settings.Shooter.PODIUM_SHOT)); // .whileTrue(new SwerveDriveAutoFerry(driver)); // climb 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 3ae67081..b956f178 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java @@ -6,162 +6,36 @@ package com.stuypulse.robot.commands.swerve; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings.Alignment; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.constants.Settings.Alignment.Shoot; import com.stuypulse.robot.subsystems.odometry.Odometry; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.math.SLMath; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; -import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.Derivative; -import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; -import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class SwerveDriveToFerry extends Command { - - private final SwerveDrive swerve; - private final Odometry odometry; - - private final Controller distanceController; - private final AnglePIDController angleController; - private final IStream velocityError; - - private final IStream distanceToSpeaker; - private final BStream isAligned; - private final Number targetDistance; - - private double distanceTolerance; - private double angleTolerance; - private double velocityTolerance; +public class SwerveDriveToFerry extends SwerveDriveToPose { public SwerveDriveToFerry() { - this(Alignment.FERRY_SHOT_DISTANCE); - } - - public SwerveDriveToFerry(Number targetDistance) { - this(targetDistance, Alignment.DEBOUNCE_TIME); - } - - public SwerveDriveToFerry(Number targetDistance, double debounce) { - this.targetDistance = targetDistance; - - swerve = SwerveDrive.getInstance(); - odometry = Odometry.getInstance(); - - distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) - .setOutputFilter( - x -> -x, - x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) - ); - - angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); - - velocityError = IStream.create(distanceController::getError) - .filtered(new Derivative()) - .filtered(new LowPassFilter(0.05)) - .filtered(x -> Math.abs(x)); - - distanceToSpeaker = IStream.create(() -> getTranslationToPose().getNorm()) - .filtered(new LowPassFilter(0.05)); - - isAligned = BStream.create(this::isAligned) - .filtered(new BDebounceRC.Rising(debounce)); - - distanceTolerance = 0.05; - angleTolerance = Alignment.ANGLE_TOLERANCE.get(); - velocityTolerance = 0.1; - - addRequirements(swerve); - } - - private Translation2d getTargetPose() { - Translation2d pose = Robot.isBlue() - ? new Translation2d(0, Field.WIDTH - 1.5) - : new Translation2d(0, 1.5); - - return pose; - } - - private Rotation2d getTargetAngle() { - return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle(); - } - - private Translation2d getTranslationToPose() { - return getTargetPose().minus(odometry.getPose().getTranslation()); - } - - private double getTargetDistance() { - return SLMath.clamp(targetDistance.doubleValue(), 1, 5); - } - - public SwerveDriveToFerry withTolerance(double distanceTolerance, double angleTolerance) { - this.distanceTolerance = distanceTolerance; - this.angleTolerance = angleTolerance; - return this; - } - - public SwerveDriveToFerry withRotationConstants(double p, double i, double d) { - angleController.setPID(p, i, d); - return this; - } + super(() -> { + Pose2d robot = Odometry.getInstance().getPose(); - private boolean isAligned() { - return distanceController.isDone(distanceTolerance) - && angleController.isDoneDegrees(angleTolerance) - && velocityError.get() < velocityTolerance; - } - - @Override - public void initialize() { - SmartDashboard.putBoolean("AutonAlignment", true); - } - - @Override - public void execute() { - double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); - double rotation = angleController.update( - Angle.fromRotation2d(getTargetAngle()).add(Angle.k180deg), - Angle.fromRotation2d(odometry.getPose().getRotation())); - - Translation2d speeds = new Translation2d( - speed, - getTargetAngle()); - - if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) - rotation = 0; - - swerve.setFieldRelativeSpeeds( - new ChassisSpeeds( - speeds.getX(), - speeds.getY(), - rotation)); + Translation2d target = Robot.isBlue() + ? new Translation2d(0, Field.WIDTH - 1.5) + : new Translation2d(0, 1.5); - SmartDashboard.putNumber("Alignment/Velocity Error", velocityError.get()); - } + Rotation2d targetAngle = new Translation2d(Field.CONST_FERRY_X, robot.getY()) + .minus(target) + .getAngle(); - @Override - public boolean isFinished() { - return isAligned.get(); - } + // use robot's y, put in x and rotation - @Override - public void end(boolean interrupted) { - swerve.stop(); - SmartDashboard.putBoolean("AutonAlignment", false); + return new Pose2d( + Field.CONST_FERRY_X, + robot.getY(), + targetAngle + ); + }); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 2f92da19..761c2df7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -282,6 +282,8 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria double FERRY_SHOT_MIN_X = 6.0; double FERRY_SHOT_MIN_FAR_X = 8.5; + double CONST_FERRY_X = (FERRY_SHOT_MAX_X + FERRY_SHOT_MIN_FAR_X) / 2.0; + /**** EMPTY FIELD POSES ****/ Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d()); From b5131f46e3c71bf5d80f657fece563e9e49cc425 Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Wed, 22 May 2024 15:04:27 -0400 Subject: [PATCH 5/6] Added AmpScoreThenStop to AmpScoreRoutine --- .../com/stuypulse/robot/RobotContainer.java | 2 +- .../robot/commands/AmpScoreRoutine.java | 4 +- .../commands/amper/AmperScoreThenStop.java | 37 +++++++++++++++++++ 3 files changed, 41 insertions(+), 2 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ffb22f38..3ccf238f 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -151,7 +151,7 @@ private void configureDriverBindings() { driver.getLeftBumper() .onTrue(new ConveyorToAmpUnsafe()) // requirements: conveyor, intake .whileTrue(new AmpScoreRoutine()) // requirements: swerve, amper - .onFalse(new ConditionalCommand(new AmperStop(), new InstantCommand(), () -> !Amper.getInstance().hasNote())) + // .onFalse(new ConditionalCommand(new AmperStop(), new InstantCommand(), () -> !Amper.getInstance().hasNote())) .onFalse(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT)); // score speaker no align diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 627c25f6..93a770dc 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -8,6 +8,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.commands.amper.AmperScore; +import com.stuypulse.robot.commands.amper.AmperScoreThenStop; import com.stuypulse.robot.commands.amper.AmperToHeight; import com.stuypulse.robot.commands.conveyor.ConveyorToAmp; import com.stuypulse.robot.commands.leds.LEDSet; @@ -77,7 +78,8 @@ public AmpScoreRoutine() { .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE)) ), - AmperScore.untilDone(), + // AmperScore.untilDone(), + AmperScoreThenStop.scoreThenStop(), new WaitCommand(0.25), diff --git a/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java new file mode 100644 index 00000000..1994752d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java @@ -0,0 +1,37 @@ + +package com.stuypulse.robot.commands.amper; + +import com.stuypulse.robot.subsystems.amper.Amper; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AmperScoreThenStop extends InstantCommand { + + private final Amper amper; + + public static Command scoreThenStop() { + return new AmperScoreThenStop(); + } + + public AmperScoreThenStop() { + amper = Amper.getInstance(); + addRequirements(amper); + } + + @Override + public void initialize() { + amper.amp(); + } + + @Override + public boolean isFinished() { + return !amper.hasNote(); + } + + @Override + public void end(boolean interrupted) { + amper.stopRoller(); + } + +} From a78354e2cf1afc42b66a9dd7f9f8c13408359b41 Mon Sep 17 00:00:00 2001 From: colyic Date: Fri, 31 May 2024 08:51:39 -0400 Subject: [PATCH 6/6] Add ferry cutoff --- src/main/java/com/stuypulse/robot/RobotContainer.java | 1 - src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java | 1 - .../com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java | 2 +- src/main/java/com/stuypulse/robot/constants/Field.java | 1 + 4 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 3ccf238f..9115cf99 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -222,7 +222,6 @@ private void configureDriverBindings() { .onFalse(new ConveyorStop()) .onFalse(new IntakeStop()) .onFalse(new ShooterSetRPM(Settings.Shooter.PODIUM_SHOT)); - // .whileTrue(new SwerveDriveAutoFerry(driver)); // climb driver.getRightButton() diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 93a770dc..9c7e097d 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -78,7 +78,6 @@ public AmpScoreRoutine() { .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE)) ), - // AmperScore.untilDone(), AmperScoreThenStop.scoreThenStop(), new WaitCommand(0.25), 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 b956f178..c035afbf 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() { return new Pose2d( Field.CONST_FERRY_X, - robot.getY(), + Robot.isBlue() ? Math.max(robot.getY(), Field.FERRY_CUTOFF) : Math.min(robot.getY(), Field.WIDTH - Field.FERRY_CUTOFF), targetAngle ); }); diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 761c2df7..b6677aab 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -283,6 +283,7 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria double FERRY_SHOT_MIN_FAR_X = 8.5; double CONST_FERRY_X = (FERRY_SHOT_MAX_X + FERRY_SHOT_MIN_FAR_X) / 2.0; + double FERRY_CUTOFF = 1.8; /**** EMPTY FIELD POSES ****/