From 1290e2e2bfea774025cab8a5dfc268580e80b776 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 16:37:43 -0400 Subject: [PATCH 1/8] Revert "Add back feeder IR sensor (#89)" This reverts commit 36157b79fd2ac083beaa7adeffe5939536c3064a. --- .../com/stuypulse/robot/RobotContainer.java | 5 --- .../commands/auton/ADE/ThreePieceADE.java | 4 +- .../auton/CBADE/BlayFivePieceCBAE.java | 4 +- .../commands/auton/CBADE/FivePieceCBAE.java | 7 +--- .../auton/CBADE/FivePiecePodiumCBAE.java | 10 ++--- .../CBADE/FivePiecePodiumForwardCBAE.java | 7 +--- .../auton/FollowPathAlignAndShoot.java | 3 -- .../FollowPathTrackingAlignAndShoot.java | 3 -- .../conveyor/ConveyorShootRoutine.java | 1 - .../commands/conveyor/ConveyorToShooter.java | 42 ------------------- .../stuypulse/robot/constants/Settings.java | 7 +--- .../robot/subsystems/conveyor/Conveyor.java | 4 -- .../subsystems/conveyor/ConveyorImpl.java | 34 +-------------- .../subsystems/conveyor/ConveyorSim.java | 18 +------- .../robot/subsystems/shooter/Shooter.java | 12 +++++- .../robot/subsystems/shooter/ShooterImpl.java | 30 ++++++++++++- .../robot/subsystems/shooter/ShooterSim.java | 5 +++ 17 files changed, 57 insertions(+), 139 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToShooter.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 664143b4..022b0787 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -152,7 +152,6 @@ private void configureDriverBindings() { // score speaker no align driver.getRightMenuButton() .onTrue(new ShooterPodiumShot()) - .onTrue(new ConveyorToShooter()) .onTrue(new ShooterWaitForTarget() .withTimeout(1.5) .andThen(new ConveyorShoot())) @@ -251,10 +250,6 @@ private void configureOperatorBindings() { .onFalse(new ConveyorStop()) .onFalse(new IntakeStop()) .onFalse(new AmperStop()); - operator.getRightBumper() - .onTrue(new ConveyorToShooter()) - .onFalse(new ConveyorStop()) - .onFalse(new IntakeStop()); operator.getTopButton() .onTrue(new AmperScore()) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADE/ThreePieceADE.java b/src/main/java/com/stuypulse/robot/commands/auton/ADE/ThreePieceADE.java index 390ee599..3d33592b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADE/ThreePieceADE.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADE/ThreePieceADE.java @@ -5,7 +5,6 @@ import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; @@ -33,8 +32,7 @@ public ThreePieceADE(PathPlannerPath... paths) { // intake A new FollowPathAndIntake(paths[0]), // shoot A - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), // intake D diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java index b875131a..a63de216 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; @@ -32,8 +31,7 @@ public BlayFivePieceCBAE(PathPlannerPath... paths) { new ConveyorShootRoutine(), new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAE.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAE.java index c69af8cc..d6447ea2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAE.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAE.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; @@ -35,13 +34,11 @@ public FivePieceCBAE(PathPlannerPath... paths) { new FollowPathAlignAndShoot(paths[1], new SwerveDriveToShoot()), new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[3]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[4]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumCBAE.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumCBAE.java index fd2be7fc..cea80801 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumCBAE.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumCBAE.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; @@ -32,18 +31,15 @@ public FivePiecePodiumCBAE(PathPlannerPath... paths) { new ConveyorShootRoutine(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[3]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumForwardCBAE.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumForwardCBAE.java index 7bd07cda..134f2b27 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumForwardCBAE.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePiecePodiumForwardCBAE.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; @@ -31,13 +30,11 @@ public FivePiecePodiumForwardCBAE(PathPlannerPath... paths) { new ConveyorShootRoutine(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot() - .alongWith(new ConveyorToShooter()), + new SwerveDriveToShoot(), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java index 58a5db8a..d7a07d2e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.commands.conveyor.ConveyorShoot; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; import com.stuypulse.robot.commands.conveyor.ConveyorStop; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; @@ -28,8 +27,6 @@ public double getPathTime(PathPlannerPath path) { public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand) { addCommands( new ParallelCommandGroup( - new ConveyorToShooter() - .withTimeout(3.0), SwerveDrive.getInstance().followPathCommand(path), new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE) .andThen(new ShooterPodiumShot()) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingAlignAndShoot.java index ca01432a..ea3d7a36 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingAlignAndShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingAlignAndShoot.java @@ -3,7 +3,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.conveyor.ConveyorShoot; import com.stuypulse.robot.commands.conveyor.ConveyorStop; -import com.stuypulse.robot.commands.conveyor.ConveyorToShooter; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; @@ -27,8 +26,6 @@ public double getPathTime(PathPlannerPath path) { public FollowPathTrackingAlignAndShoot(PathPlannerPath path, Command alignCommand) { addCommands( new ParallelCommandGroup( - new ConveyorToShooter() - .withTimeout(3.0), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path), new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE) .andThen(new ShooterPodiumShot()) diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java index e8f4762d..62ec75fe 100644 --- a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java @@ -17,7 +17,6 @@ public ConveyorShootRoutine() { public ConveyorShootRoutine(double delay) { addCommands( new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), - new ConveyorToShooter(), new ConveyorShoot(), new WaitCommand(delay), new ConveyorStop(), diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToShooter.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToShooter.java deleted file mode 100644 index 1e0549c5..00000000 --- a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToShooter.java +++ /dev/null @@ -1,42 +0,0 @@ -/************************ 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.subsystems.conveyor.Conveyor; -import com.stuypulse.robot.subsystems.intake.Intake; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ConveyorToShooter extends Command { - - private final Conveyor conveyor; - private final Intake intake; - - public ConveyorToShooter() { - conveyor = Conveyor.getInstance(); - intake = Intake.getInstance(); - - addRequirements(conveyor, intake); - } - - @Override - public void execute() { - conveyor.toShooter(); - intake.acquire(); - } - - @Override - public void end(boolean interrupted) { - conveyor.stop(); - intake.stop(); - } - - @Override - public boolean isFinished() { - return conveyor.isNoteAtShooter(); - } -} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 19765984..d6e50442 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -376,12 +376,9 @@ public interface Detection { public interface Conveyor { SmartNumber GANDALF_SHOOTER_SPEED = new SmartNumber("Conveyor/Gandalf Shooter Speed", 1.0); - SmartNumber GANDALF_AMP_SPEED = new SmartNumber("Conveyor/Gandalf Amp Speed", 0.75); - SmartNumber FEEDER_SHOOTER_SPEED = new SmartNumber("Conveyor/Feeder Shooter Speed", 1.0); - SmartNumber FEEDER_AMP_SPEED = new SmartNumber("Conveyor/Feeder Amp Speed", 1.0); + double GANDALF_AMP_SPEED = 0.75; - - SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.0); //TODO: Tune this value + SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.0); SmartNumber RECALL_DEBOUNCE = new SmartNumber("Conveyor/Recall Delay", 1.0); SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.35); diff --git a/src/main/java/com/stuypulse/robot/subsystems/conveyor/Conveyor.java b/src/main/java/com/stuypulse/robot/subsystems/conveyor/Conveyor.java index edbdfacd..13594678 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/conveyor/Conveyor.java +++ b/src/main/java/com/stuypulse/robot/subsystems/conveyor/Conveyor.java @@ -41,10 +41,6 @@ public static Conveyor getInstance() { public abstract double getGandalfSpeed(); - public abstract double getFeederSpeed(); - - public abstract boolean isNoteAtShooter(); - public abstract void toShooter(); public abstract void toAmp(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorImpl.java index 8d3375b1..2b60b5fc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorImpl.java @@ -9,42 +9,28 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Motors.StatusFrame; import com.stuypulse.robot.util.FilteredRelativeEncoder; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; - import com.revrobotics.CANSparkFlex; import com.revrobotics.RelativeEncoder; public class ConveyorImpl extends Conveyor { private final CANSparkFlex gandalfMotor; - private final CANSparkFlex feederMotor; private final RelativeEncoder gandalfEncoder; - - private final DigitalInput shooterIR; - private final BStream isAtShooter; protected ConveyorImpl() { gandalfMotor = new CANSparkFlex(Ports.Conveyor.GANDALF, MotorType.kBrushless); - feederMotor = new CANSparkFlex(Ports.Conveyor.FEEDER, MotorType.kBrushless); gandalfEncoder = new FilteredRelativeEncoder(gandalfMotor); - - shooterIR = new DigitalInput(Ports.Shooter.INTAKE_IR); - isAtShooter = BStream.create(shooterIR).not(). - filtered(new BDebounce.Rising(Settings.Conveyor.DEBOUNCE_TIME.get())); - gandalfEncoder.setVelocityConversionFactor(1.0 / 2.0); Motors.disableStatusFrames(gandalfMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY); @@ -57,32 +43,19 @@ public double getGandalfSpeed() { return gandalfMotor.get(); } - @Override - public double getFeederSpeed() { - return feederMotor.get(); - } - - @Override - public boolean isNoteAtShooter() { - return isAtShooter.get(); - } - @Override public void toShooter() { gandalfMotor.set(+Settings.Conveyor.GANDALF_SHOOTER_SPEED.get()); - feederMotor.set(+Settings.Conveyor.FEEDER_SHOOTER_SPEED.get()); } @Override public void toAmp() { - gandalfMotor.set(-Settings.Conveyor.GANDALF_AMP_SPEED.get()); - feederMotor.set(+Settings.Conveyor.FEEDER_AMP_SPEED.get()); + gandalfMotor.set(-Settings.Conveyor.GANDALF_AMP_SPEED); } @Override public void stop() { gandalfMotor.stopMotor(); - feederMotor.stopMotor(); } @Override @@ -102,10 +75,5 @@ public void periodic() { SmartDashboard.putNumber("Conveyor/Gandalf RPM", gandalfEncoder.getVelocity()); SmartDashboard.putNumber("Conveyor/Gandalf Linear Velocity", gandalfEncoder.getVelocity() * Units.inchesToMeters(1.0) * Math.PI); - - SmartDashboard.putNumber("Shooter/Feeder Voltage", feederMotor.getBusVoltage() * feederMotor.getAppliedOutput()); - SmartDashboard.putNumber("Shooter/Feeder Current", feederMotor.getOutputCurrent()); - - } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorSim.java b/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorSim.java index aa38d681..9caaea97 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/conveyor/ConveyorSim.java @@ -14,11 +14,9 @@ public class ConveyorSim extends Conveyor { private double gandalfMotorSpeed; - private double feederMotorSpeed; public ConveyorSim() { gandalfMotorSpeed = 0; - feederMotorSpeed = 0; } @Override @@ -26,32 +24,19 @@ public double getGandalfSpeed() { return gandalfMotorSpeed; } - @Override - public double getFeederSpeed() { - return feederMotorSpeed; - } - - @Override - public boolean isNoteAtShooter() { - return false; - } - @Override public void toShooter() { gandalfMotorSpeed = +Settings.Conveyor.GANDALF_SHOOTER_SPEED.get(); - feederMotorSpeed = +Settings.Conveyor.FEEDER_SHOOTER_SPEED.get(); } @Override public void toAmp() { - gandalfMotorSpeed = -Settings.Conveyor.GANDALF_AMP_SPEED.get(); - feederMotorSpeed = +Settings.Conveyor.FEEDER_AMP_SPEED.get(); + gandalfMotorSpeed = -Settings.Conveyor.GANDALF_AMP_SPEED; } @Override public void stop() { gandalfMotorSpeed = 0; - feederMotorSpeed = 0; } @Override @@ -59,7 +44,6 @@ public void periodic() { super.periodic(); SmartDashboard.putNumber("Conveyor/Gandalf Motor Speed", gandalfMotorSpeed); - SmartDashboard.putNumber("Conveyor/Feeder Motor Speed", feederMotorSpeed); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 632eab3b..b197e577 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -32,10 +32,12 @@ public static Shooter getInstance() { private final SmartNumber leftTargetRPM; private final SmartNumber rightTargetRPM; + private final SmartNumber feederTargetRPM; public Shooter() { leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0); rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0); + feederTargetRPM = new SmartNumber("Shooter/Feeder Target RPM", 0); } public final double getLeftTargetRPM() { @@ -46,9 +48,14 @@ public final double getRightTargetRPM() { return rightTargetRPM.get(); } + public final double getFeederTargetRPM() { + return feederTargetRPM.get(); + } + public final void setTargetSpeeds(ShooterSpeeds speeds) { this.leftTargetRPM.set(speeds.getLeftRPM()); this.rightTargetRPM.set(speeds.getRightRPM()); + this.feederTargetRPM.set(speeds.getFeederRPM()); } public final void stop() { @@ -56,7 +63,8 @@ public final void stop() { } public final boolean atTargetSpeeds() { - return Math.abs(getLeftShooterRPM() - getLeftTargetRPM()) < Settings.Shooter.AT_RPM_EPSILON + return Math.abs(getFeederRPM() - getFeederTargetRPM()) < Settings.Shooter.AT_RPM_EPSILON + && Math.abs(getLeftShooterRPM() - getLeftTargetRPM()) < Settings.Shooter.AT_RPM_EPSILON && Math.abs(getRightShooterRPM() - getRightTargetRPM()) < Settings.Shooter.AT_RPM_EPSILON; } @@ -68,5 +76,7 @@ public final double getAverageShooterRPM() { return (getLeftShooterRPM() + getRightShooterRPM()) / 2.0; } + public abstract double getFeederRPM(); + public abstract boolean noteShot(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 201ddc6b..c14497c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -16,6 +16,7 @@ import com.stuypulse.robot.constants.Motors.StatusFrame; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Feeder; import com.stuypulse.robot.constants.Settings.Shooter.Feedforward; import com.stuypulse.robot.constants.Settings.Shooter.PID; import com.stuypulse.robot.subsystems.odometry.Odometry; @@ -25,6 +26,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkFlex; import com.revrobotics.RelativeEncoder; @@ -32,38 +34,48 @@ public class ShooterImpl extends Shooter { private final CANSparkFlex leftMotor; private final CANSparkFlex rightMotor; + private final CANSparkMax feederMotor; private final RelativeEncoder leftEncoder; private final RelativeEncoder rightEncoder; + private final RelativeEncoder feederEncoder; private final Controller leftController; private final Controller rightController; + private final Controller feederController; private final IStream rpmChange; protected ShooterImpl() { leftMotor = new CANSparkFlex(Ports.Shooter.LEFT_MOTOR, MotorType.kBrushless); rightMotor = new CANSparkFlex(Ports.Shooter.RIGHT_MOTOR, MotorType.kBrushless); + feederMotor = new CANSparkMax(Ports.Conveyor.FEEDER, MotorType.kBrushless); leftEncoder = new FilteredRelativeEncoder(leftMotor); rightEncoder = new FilteredRelativeEncoder(rightMotor); + feederEncoder = new FilteredRelativeEncoder(feederMotor); leftEncoder.setVelocityConversionFactor(1.0); rightEncoder.setVelocityConversionFactor(1.0); + feederEncoder.setVelocityConversionFactor(Feeder.GEARING); leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() .add(new PIDController(PID.kP, PID.kI, PID.kD)); rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() .add(new PIDController(PID.kP, PID.kI, PID.kD)); - + feederController = new MotorFeedforward(Feeder.Feedforward.kS, Feeder.Feedforward.kV, Feeder.Feedforward.kA).velocity() + .add(new PIDController(Feeder.PID.kP, Feeder.PID.kI, Feeder.PID.kD)); + rpmChange = IStream.create(this::getAverageShooterRPM) .filtered(new HighPassFilter(Settings.Shooter.RPM_CHANGE_RC)); Motors.disableStatusFrames(leftMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY); Motors.disableStatusFrames(rightMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY); + Motors.disableStatusFrames(feederMotor, StatusFrame.ANALOG_SENSOR, StatusFrame.ALTERNATE_ENCODER, StatusFrame.ABS_ENCODER_POSIITION, StatusFrame.ABS_ENCODER_VELOCITY); Motors.Shooter.LEFT_SHOOTER.configure(leftMotor); Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); + Motors.Conveyor.SHOOTER_FEEDER_MOTOR.configure(feederMotor); } @Override @@ -75,6 +87,11 @@ public double getLeftShooterRPM() { public double getRightShooterRPM() { return rightEncoder.getVelocity(); } + + @Override + public double getFeederRPM() { + return feederEncoder.getVelocity(); + } @Override public boolean noteShot() { @@ -87,26 +104,35 @@ public void periodic() { leftController.update(getLeftTargetRPM(), getLeftShooterRPM()); rightController.update(getRightTargetRPM(), getRightShooterRPM()); + feederController.update(getFeederTargetRPM(), getFeederRPM()); - if (getLeftTargetRPM() == 0 && getRightTargetRPM() == 0) { + if (getLeftTargetRPM() == 0 && getRightTargetRPM() == 0 && getFeederTargetRPM() == 0) { leftMotor.stopMotor(); rightMotor.stopMotor(); + feederMotor.stopMotor(); } else { leftMotor.setVoltage(leftController.getOutput()); rightMotor.setVoltage(rightController.getOutput()); + feederMotor.setVoltage(feederController.getOutput()); } SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM()); SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM()); + SmartDashboard.putNumber("Shooter/Feeder RPM", getFeederRPM()); + + SmartDashboard.putNumber("Shooter/Feeder Linear Velocity", getFeederRPM() * Units.inchesToMeters(1.0) * Math.PI); SmartDashboard.putNumber("Shooter/Right Error", rightController.getError()); SmartDashboard.putNumber("Shooter/Left Error", leftController.getError()); + SmartDashboard.putNumber("Shooter/Feeder Error", feederController.getError()); SmartDashboard.putNumber("Shooter/Left Voltage", leftMotor.getBusVoltage() * leftMotor.getAppliedOutput()); SmartDashboard.putNumber("Shooter/Right Voltage", rightMotor.getBusVoltage() * rightMotor.getAppliedOutput()); + SmartDashboard.putNumber("Shooter/Feeder Voltage", feederMotor.getBusVoltage() * feederMotor.getAppliedOutput()); SmartDashboard.putNumber("Shooter/Left Current", leftMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Right Current", rightMotor.getOutputCurrent()); + SmartDashboard.putNumber("Shooter/Feeder Current", feederMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/RPM Change", rpmChange.get()); SmartDashboard.putBoolean("Shooter/Note Shot", noteShot()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java index 6ff3a083..caa27041 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java @@ -48,6 +48,11 @@ public double getLeftShooterRPM() { public double getRightShooterRPM() { return rightWheel.getAngularVelocityRPM(); } + + @Override + public double getFeederRPM() { + return 0; + } @Override public boolean noteShot() { From 0756950431fe8f1246ed07d5bf6a099f0562e3b4 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 17:29:22 -0400 Subject: [PATCH 2/8] Tune for voltage --- .../swerve/modules/KrakenSwerveModule.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index f68042ab..61fe3041 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -67,10 +67,10 @@ public KrakenSwerveModule( // PIDF values Slot0Configs slot0 = new Slot0Configs(); - slot0.kS = 0.25; - slot0.kV = 0.12; - slot0.kA = 0.01; - slot0.kP = 0.11; + slot0.kS = 0.14304; + slot0.kV = 0.10884; + slot0.kA = 0.023145; + slot0.kP = 0.07; slot0.kI = 0; slot0.kD = 0; @@ -158,19 +158,19 @@ private double convertDriveVel(double speedMetersPerSecond) { public void periodic() { super.periodic(); - VelocityTorqueCurrentFOC driveOutput = new VelocityTorqueCurrentFOC(convertDriveVel(getTargetState().speedMetersPerSecond)); + VelocityVoltage driveOutput = new VelocityVoltage(convertDriveVel(getTargetState().speedMetersPerSecond)); pivotController.update(Angle.fromRotation2d(getTargetState().angle), Angle.fromRotation2d(getAngle())); if (Math.abs(getTargetState().speedMetersPerSecond) < Settings.Swerve.MODULE_VELOCITY_DEADBAND) { - driveMotor.setControl(new VelocityTorqueCurrentFOC(0)); + driveMotor.setControl(new VelocityVoltage(0)); pivotMotor.setVoltage(0); } else { driveMotor.setControl(driveOutput); pivotMotor.setVoltage(pivotController.getOutput()); } - SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Current", driveMotor.getTorqueCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Current", driveMotor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Position", getPosition()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Velocity", getVelocity()); SmartDashboard.putNumber("Swerve/Modules/" + getId() + "/Drive Voltage", driveMotor.getMotorVoltage().getValueAsDouble()); From 8242cf731f776beb956d78df1de1242e77099af8 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 17:34:50 -0400 Subject: [PATCH 3/8] Apply 65amp current limit --- .../robot/subsystems/swerve/modules/KrakenSwerveModule.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java index 61fe3041..49e5361a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/modules/KrakenSwerveModule.java @@ -101,8 +101,8 @@ public KrakenSwerveModule( driveConfig.Feedback.SensorToMechanismRatio = 1.0; // 1:1 sensor to mechanism ratio // Current limits - // driveConfig.CurrentLimits.StatorCurrentLimit = 65; // 65A stator current limit - // driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Enable stator current limiting + driveConfig.CurrentLimits.StatorCurrentLimit = 65; // 65A stator current limit + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Enable stator current limiting // driveConfig.CurrentLimits.SupplyCurrentLimit = 40; // 40A supply current limit // driveConfig.CurrentLimits.SupplyCurrentThreshold = 40; // 40A supply current threshold From 583eb996361be1fe448b0374de9b0ceaad8b22b9 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 17:52:44 -0400 Subject: [PATCH 4/8] Remove voltage zero at bottom lift --- .../java/com/stuypulse/robot/subsystems/amper/AmperImpl.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java index 84b29e3e..164bca11 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java @@ -166,10 +166,6 @@ public void periodic() { voltage = 0; } - if (getTargetHeight() == getMinHeight() && voltage > 0) { - voltage = 0; - } - if (getTargetHeight() == Settings.Amper.Lift.TRAP_SCORE_HEIGHT && voltage < 0.75) { voltage = 0.75; } From 0325872bc84415e034e7a63ac91e85212595d76f Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 18:13:26 -0400 Subject: [PATCH 5/8] Invert feeder motor --- src/main/java/com/stuypulse/robot/constants/Motors.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index fc01ea89..d2dd1743 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -61,7 +61,7 @@ public interface Shooter { public interface Conveyor { CANSparkConfig GANDALF_MOTOR = new CANSparkConfig(true, IdleMode.kBrake,500, 0.25); - CANSparkConfig SHOOTER_FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 500, 0.1); + CANSparkConfig SHOOTER_FEEDER_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.1); } public interface Climber { From 1ce07e88c67c7c8dac6bddfe48c8b74a5eddac46 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 18:13:50 -0400 Subject: [PATCH 6/8] Use normal turn kP for assist kP --- 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 86e34961..55b4bebf 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -159,7 +159,7 @@ public interface Assist { double BUZZ_INTENSITY = 1; - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 2.0); + SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0); SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); From 1502f072c00512eb1113b0789cdb7e68f3458eb0 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 25 Mar 2024 19:02:01 -0400 Subject: [PATCH 7/8] Ferry shot changes --- .../commands/swerve/SwerveDriveAutoFerry.java | 35 ++++++++++++++++--- .../swerve/SwerveDriveDriveAligned.java | 4 +-- .../com/stuypulse/robot/constants/Field.java | 3 +- 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java index ec444ac6..489fce96 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned { @@ -36,25 +37,49 @@ public SwerveDriveAutoFerry(Gamepad driver) { // returns pose of close amp corner private Translation2d getTargetPose() { return Robot.isBlue() - ? new Translation2d(0, Field.WIDTH) - : new Translation2d(0, 0); + ? new Translation2d(0, Field.WIDTH - 0.5) + : new Translation2d(0, 0.5); } @Override public Rotation2d getTargetAngle() { - return getTargetPose().minus(odometry.getPose().getTranslation()).getAngle(); + return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle(); } @Override public double getDistanceToTarget() { - return getTargetPose().getDistance(odometry.getPose().getTranslation()); + return odometry.getPose().getTranslation().getDistance(getTargetPose()); + } + + private boolean shouldMoveBack() { + Translation2d robot = odometry.getPose().getTranslation(); + + return robot.getX() < Field.FERRY_SHOT_MIN_FAR_X + && robot.getY() < Field.WIDTH / 2.0 + 1; + } + + private boolean canShoot() { + Translation2d robot = odometry.getPose().getTranslation(); + + if (shouldMoveBack()) + return false; + + if (robot.getX() > Field.FERRY_SHOT_MAX_X) + return false; + + return true; } @Override public void execute() { super.execute(); - if (odometry.getPose().getX() < Field.FERRY_SHOT_THRESHOLD_X) { + if (shouldMoveBack()) { + swerve.setFieldRelativeSpeeds(new ChassisSpeeds(1, 0, 0)); + + intake.stop(); + conveyor.stop(); + } else if (canShoot() && Math.abs(controller.getError().toDegrees()) < 5.0) { intake.acquire(); conveyor.toShooter(); shooter.setTargetSpeeds(Settings.Shooter.FERRY); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java index 22076ac8..865c324d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java @@ -21,11 +21,11 @@ public abstract class SwerveDriveDriveAligned extends Command { - private final SwerveDrive swerve; + protected final SwerveDrive swerve; private final Odometry odometry; private final VStream drive; - private final AngleController controller; + public final AngleController controller; private final IStream angleVelocity; public SwerveDriveDriveAligned(Gamepad driver) { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 831bad4f..a69c1b10 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -272,7 +272,8 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria /*** FERRYING ***/ - double FERRY_SHOT_THRESHOLD_X = 9.0; + double FERRY_SHOT_MAX_X = 9.0; + double FERRY_SHOT_MIN_FAR_X = 8.25; /**** EMPTY FIELD POSES ****/ From 0c56659df56f868c063dafe477eae52e60489fcf Mon Sep 17 00:00:00 2001 From: BenG49 Date: Tue, 26 Mar 2024 11:26:26 -0400 Subject: [PATCH 8/8] Reorganize ferry command --- .../commands/swerve/SwerveDriveAutoFerry.java | 79 +++++++++++++++---- .../swerve/SwerveDriveDriveAligned.java | 4 +- .../stuypulse/robot/constants/Settings.java | 2 + 3 files changed, 67 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java index 489fce96..2eaa7a47 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java @@ -7,31 +7,70 @@ 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.input.Gamepad; - +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import com.stuypulse.stuylib.util.AngleVelocity; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +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.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; -public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned { +public class SwerveDriveAutoFerry extends Command { + private final SwerveDrive swerve; private final Odometry odometry; private final Shooter shooter; private final Conveyor conveyor; private final Intake intake; + + private final VStream drive; - public SwerveDriveAutoFerry(Gamepad driver) { - super(driver); + private final AngleController controller; + private final IStream angleVelocity; + public SwerveDriveAutoFerry(Gamepad driver) { + swerve = SwerveDrive.getInstance(); shooter = Shooter.getInstance(); odometry = Odometry.getInstance(); conveyor = Conveyor.getInstance(); intake = Intake.getInstance(); - addRequirements(shooter, odometry); + drive = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), + new VLowPassFilter(Drive.RC.get())); + + 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 @@ -41,13 +80,11 @@ private Translation2d getTargetPose() { : new Translation2d(0, 0.5); } - @Override - public Rotation2d getTargetAngle() { + private Rotation2d getTargetAngle() { return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle(); } - @Override - public double getDistanceToTarget() { + private double getDistanceToTarget() { return odometry.getPose().getTranslation().getDistance(getTargetPose()); } @@ -70,22 +107,32 @@ private boolean canShoot() { return true; } + @Override + public void initialize() { + shooter.setTargetSpeeds(Settings.Shooter.FERRY); + } + @Override public void execute() { - super.execute(); + double omega = angleVelocity.get() + controller.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(odometry.getPose().getRotation())); if (shouldMoveBack()) { swerve.setFieldRelativeSpeeds(new ChassisSpeeds(1, 0, 0)); intake.stop(); conveyor.stop(); - } else if (canShoot() && Math.abs(controller.getError().toDegrees()) < 5.0) { - intake.acquire(); - conveyor.toShooter(); - shooter.setTargetSpeeds(Settings.Shooter.FERRY); } else { - intake.stop(); - conveyor.stop(); + if (canShoot() && Math.abs(controller.getError().toDegrees()) < Assist.FERRY_ALIGN_THRESHOLD_DEG) { + intake.acquire(); + conveyor.toShooter(); + } else { + intake.stop(); + conveyor.stop(); + } + + swerve.drive(drive.get(), omega); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java index 865c324d..22076ac8 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java @@ -21,11 +21,11 @@ public abstract class SwerveDriveDriveAligned extends Command { - protected final SwerveDrive swerve; + private final SwerveDrive swerve; private final Odometry odometry; private final VStream drive; - public final AngleController controller; + private final AngleController controller; private final IStream angleVelocity; public SwerveDriveDriveAligned(Gamepad driver) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 55b4bebf..135c6ddd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -163,6 +163,8 @@ public interface Assist { SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); + double FERRY_ALIGN_THRESHOLD_DEG = 5.0; + double ANGLE_DERIV_RC = 0.05; double REDUCED_FF_DIST = 0.75; }