diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0a46c450..9393070c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -154,10 +154,8 @@ private void configureDriverBindings() { // score speaker no align driver.getRightMenuButton() .onTrue(new ShooterPodiumShot()) - .whileTrue(new ConveyorToShooter() - .andThen(new WaitCommand(Settings.Conveyor.AT_FEEDER_WAIT_DELAY.get())) - .andThen(new ShooterWaitForTarget() - .withTimeout(1.5)) + .onTrue(new ShooterWaitForTarget() + .withTimeout(1.5) .andThen(new ConveyorShoot())) .onFalse(new ConveyorStop()) .onFalse(new IntakeStop()); @@ -254,10 +252,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 d5d1eb71..bea4538e 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 db470254..278f942d 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/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java index ec444ac6..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,60 +7,132 @@ 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 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(); + private Rotation2d getTargetAngle() { + return odometry.getPose().getTranslation().minus(getTargetPose()).getAngle(); + } + + private double getDistanceToTarget() { + 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 double getDistanceToTarget() { - return getTargetPose().getDistance(odometry.getPose().getTranslation()); + 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)); - if (odometry.getPose().getX() < Field.FERRY_SHOT_THRESHOLD_X) { - intake.acquire(); - conveyor.toShooter(); - shooter.setTargetSpeeds(Settings.Shooter.FERRY); - } else { intake.stop(); conveyor.stop(); + } else { + 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/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 ****/ 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 { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 6ef451b8..135c6ddd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -159,10 +159,12 @@ 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); + double FERRY_ALIGN_THRESHOLD_DEG = 5.0; + double ANGLE_DERIV_RC = 0.05; double REDUCED_FF_DIST = 0.75; } @@ -376,12 +378,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/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; } 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 480efe18..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.IR_SENSOR); - 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("Conveyor/Feeder Voltage", feederMotor.getBusVoltage() * feederMotor.getAppliedOutput()); - SmartDashboard.putNumber("Conveyor/Feeder Current", feederMotor.getOutputCurrent()); - - SmartDashboard.putBoolean("Conveyor/Note At Shooter", isNoteAtShooter()); } } 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() { 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..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 @@ -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; @@ -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 @@ -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());