Skip to content

Create feeder subsystem #115

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: development
Choose a base branch
from
Open

Create feeder subsystem #115

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
07f0572
Add feeder subsystem for shooter
boomermath Sep 23, 2024
90c76d6
Refactor shooter into subclasses
boomermath Sep 24, 2024
2766a6e
Add intake back to handoff
boomermath Sep 24, 2024
13da421
Add shooter classes (oops)
boomermath Sep 24, 2024
3ac890c
Add shooter classes (oops)
boomermath Sep 24, 2024
58b8eb4
Merge remote-tracking branch 'origin/feeder-subsystem' into feeder-su…
boomermath Sep 24, 2024
d796b4c
Add back IntakeSubsystem
boomermath Sep 24, 2024
2a1517e
Add robot commands
boomermath Sep 25, 2024
a4479e1
Test pivot bump fire
boomermath Sep 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 6 additions & 15 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,9 @@
package frc.robot;

import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
import com.ctre.phoenix6.signals.GravityTypeValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
import edu.wpi.first.math.interpolation.InverseInterpolator;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.util.Units;
import frc.robot.util.ShooterStateData;

import java.io.IOException;
import java.util.Map;
Expand All @@ -22,7 +12,7 @@

public class Constants {

public class FieldConstants{
public static class FieldConstants {
public static final double fieldLength = Units.inchesToMeters(651.223);

public static final class Speaker {
Expand All @@ -39,6 +29,7 @@ public static final class Speaker {
public static Translation3d centerSpeakerOpening =
bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5);
public static final AprilTagFieldLayout aprilTags;

static {
try {
aprilTags = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile);
Expand All @@ -50,7 +41,7 @@ public static final class Speaker {

public static final class Shuttle {
public static Translation2d closeShuttleTargetCorner =
new Translation2d(0.0,7.0);
new Translation2d(0.0, 7.0);

public static Translation2d farShuttleTargetCorner =
new Translation2d(2.5, 7.0);
Expand All @@ -60,12 +51,12 @@ public static final class Shuttle {
}
}

public class CANCoderConstants{
public static class CANCoderConstants {
public static final int COUNTS_PER_REV = 4096;
public static final double COUNTS_PER_DEG = COUNTS_PER_REV / 360.0;
}

public class OIConstants{
public static class OIConstants {
public static final Map<Integer, ControllerType> CONTROLLERS = Map.of(
0, ControllerType.CUSTOM, 1, ControllerType.XBOX
);
Expand All @@ -79,7 +70,7 @@ public enum ControllerType {
}
}

public class TalonFXConstants{
public static class TalonFXConstants {
public static final boolean TALON_FUTURE_PROOF = true;
public static final String CANIVORE_NAME = "canivoreBus";
}
Expand Down
89 changes: 42 additions & 47 deletions src/main/java/frc/robot/RobotCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,33 +2,55 @@

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import frc.robot.subsystems.*;
import frc.robot.subsystems.ArmSubsystem;
import frc.robot.subsystems.ElevatorSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
import frc.robot.util.RobotDataPublisher;
import frc.robot.subsystems.shooter.*;
import frc.robot.util.AllianceFlipUtil;
import frc.robot.util.ShooterStateData;
import frc.robot.util.controllerUtils.MultiButton;

import java.util.function.Supplier;

public class RobotCommands {

private final IntakeSubsystem intake;
private final PivotSubsystem pivot;
private final ShooterSubsystem shooter;
private final CommandSwerveDrivetrain commandSwerveDrivetrain;
private final ArmSubsystem arm;
private final ElevatorSubsystem elevator;
private final FeederSubsystem feeder;
private final FlywheelSubsystem flywheel;

public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, ShooterSubsystem shooter, CommandSwerveDrivetrain commandSwerveDrivetrain, ArmSubsystem arm, ElevatorSubsystem elevator1) {
public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, CommandSwerveDrivetrain commandSwerveDrivetrain, ArmSubsystem arm, ElevatorSubsystem elevator, FeederSubsystem feeder, FlywheelSubsystem flywheel) {
this.intake = intake;
this.pivot = pivot;
this.shooter = shooter;
this.commandSwerveDrivetrain = commandSwerveDrivetrain;
this.arm = arm;
this.elevator = elevator1;
this.elevator = elevator;
this.feeder = feeder;
this.flywheel = flywheel;
}

private Command updateTarget(Supplier<ShooterTarget> shooterTarget) {
return pivot.adjustPivotPositionTo(() -> shooterTarget.get().angle)
.alongWith(flywheel.reachVelocity(() -> shooterTarget.get().rps));
}

public Command setTarget(double angle, double velocity) {
return pivot.adjustPivotPositionTo(angle)
.alongWith(flywheel.reachVelocity(velocity));
}

public Command setTargetAndShoot(double angle, double velocity) {
return setTarget(angle, velocity).andThen(feeder.feed(-1));
}


private Command createShooterTargetCommand(Pose2d target, InterpolatingTreeMap<Double, ShooterTarget> targetMap) {
Supplier<ShooterTarget> shooterTargetSupplier = () -> targetMap.get(commandSwerveDrivetrain.getState().Pose.relativeTo(target).getTranslation().getNorm());
return updateTarget(shooterTargetSupplier);
}

/**
Expand All @@ -38,57 +60,30 @@ public RobotCommands(IntakeSubsystem intake, PivotSubsystem pivot, ShooterSubsys
* @return {@code Command} instance
*/
public Command setShooterState() {
RobotDataPublisher<ShooterStateData> shooterStatePublisher = commandSwerveDrivetrain.observablePose().map(robotPose -> {
final Pose2d speakerPose = AllianceFlipUtil.apply(new Pose2d(0.0, 5.5, Rotation2d.fromDegrees(0)));
Pose2d relativeSpeaker = robotPose.relativeTo(speakerPose);
double distance = relativeSpeaker.getTranslation().getNorm();
return ShooterSubsystem.ShooterConstants.SHOOTER_MAP().get(distance);
});

return pivot.updatePivotPositionWith(shooterStatePublisher)
.alongWith(shooter.updateVelocityWith(shooterStatePublisher));
return createShooterTargetCommand(AllianceFlipUtil.apply(new Pose2d(0.0, 5.5, Rotation2d.fromDegrees(0))), ShooterValues.SHOOTER_MAP);
}

public Command setShuttleState() {
RobotDataPublisher<ShooterStateData> shuttleStatePublisher = commandSwerveDrivetrain.observablePose().map(robotPose -> {
final Pose2d shuttleTarget = AllianceFlipUtil.apply(new Pose2d(2.5, 7.0, Rotation2d.fromDegrees(0)));
Pose2d relativeShuttleTarget = robotPose.relativeTo(shuttleTarget);
double distance = relativeShuttleTarget.getTranslation().getNorm();
return ShooterSubsystem.ShooterConstants.SHUTTLE_MAP().get(distance);
});
return pivot.updatePivotPositionWith(shuttleStatePublisher).alongWith(shooter.updateVelocityWith(shuttleStatePublisher));
return createShooterTargetCommand(AllianceFlipUtil.apply(new Pose2d(0.0, 5.5, Rotation2d.fromDegrees(0))), ShooterValues.SHUTTLE_MAP);
}

public Command handoff() {
return pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen(
arm.moveUpAndStop(.5).until(() ->
arm.getEnc() >= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION).andThen(
shooter.spinFeederAndStop(-0.3).alongWith(intake.rollOut(-0.15))
).until(shooter::getBeamBreak)
);
return arm.moveUpAndStop(.5).until(() ->
arm.getEnc() >= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION)
.andThen(pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF))
.andThen(feeder.ingest(-0.3).alongWith(intake.rollOut(-0.15)));
}

public Command setAmp() {
return elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.AMP).andThen(pivot.moveDown(-0.25).unless(
() -> pivot.getEncoderAngle() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch())));
() -> pivot.getPosition() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch())));
}

public Command stowAmp() {
return pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen(elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)));
return pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF).andThen(elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)));
}


public Command bumpFire(){
return pivot.adjustPivotPositionTo(.53).andThen(
new StartEndCommand(() -> arm.moveUp(.7), arm::stop).until(() ->
arm.getEnc() <= ArmSubsystem.ArmConstants.ARM_HANDOFF_POSITION).andThen(
shooter.spinFeederAndStop(-0.3).alongWith(intake.rollOut(-.2))
).until(shooter::getBeamBreak)
).andThen(
shooter.setVelocityAndStop(100)).andThen(
intake.rollOut(-.1).withTimeout(0.2)).andThen(
Commands.waitSeconds(.45)).andThen(
shooter.spinFeederMaxAndStop().alongWith(intake.rollOut(-1).withTimeout(1)).andThen(
this.handoff().withTimeout(2)));
return pivot.adjustPivotPositionTo(0.53);
}
}
42 changes: 17 additions & 25 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -29,44 +28,39 @@
import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
import frc.robot.subsystems.drivetrain.Telemetry;
import frc.robot.subsystems.drivetrain.generated.TunerConstants;
import frc.robot.subsystems.shooter.FeederSubsystem;
import frc.robot.subsystems.shooter.FlywheelSubsystem;
import frc.robot.subsystems.shooter.PivotSubsystem;
import frc.robot.util.controllerUtils.ButtonHelper;
import frc.robot.util.controllerUtils.ControllerContainer;
import frc.robot.util.controllerUtils.MultiButton;


public class RobotContainer {

public final ShooterSubsystem shooter = new ShooterSubsystem();
public final PivotSubsystem pivot = new PivotSubsystem();
public final ElevatorSubsystem elevator = new ElevatorSubsystem();
public final IntakeSubsystem intake = new IntakeSubsystem();
public final ArmSubsystem arm = new ArmSubsystem();
public final CommandXboxController joystick = new CommandXboxController(1); // My joystick
public final VisionSubsystem vision = new VisionSubsystem();
public final FeederSubsystem feeder = new FeederSubsystem();
public final FlywheelSubsystem flywheel = new FlywheelSubsystem();
public final PivotSubsystem pivot = new PivotSubsystem();

final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(CommandSwerveDrivetrain.MAX_VELOCITY_METERS_PER_SECOND * 0.1)
.withRotationalDeadband(CommandSwerveDrivetrain.MaFxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); // I want field-centric

private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
public ControllerContainer controllerContainer = new ControllerContainer();
public SendableChooser<AutoConstants.AutoMode> autoChooser;
ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers());
private final ButtonHelper buttonHelper = new ButtonHelper(controllerContainer.getControllers());
public final CommandXboxController joystick = new CommandXboxController(1); // My joystick
private boolean autoNeedsRebuild = true;
private Command auto;

private final RobotCommands robotCommands = new RobotCommands(intake, pivot, shooter, drivetrain, arm, elevator);
private final RobotCommands robotCommands = new RobotCommands(intake, pivot, drivetrain, arm, elevator, feeder, flywheel);

public RobotContainer() {
NamedCommands.registerCommand("shootBump", Commands.sequence(
pivot.adjustPivotPositionTo(0.55),
shooter.shooterBumpFire(),
Commands.waitSeconds(.75), // is this waiting for a specific speed or something? should prob be replaced
shooter.spinFeederMaxAndStop().alongWith(intake.rollOut(-1).withTimeout(1))
));

var field = new Field2d();
SmartDashboard.putData("Field", field);

Expand All @@ -80,7 +74,6 @@ public RobotContainer() {
field.getObject("path").setPoses(poses)
);


if (Utils.isSimulation()) {
drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
}
Expand All @@ -103,19 +96,18 @@ public RobotContainer() {
private void configureBindings() {

buttonHelper.createButton(1, 0, robotCommands.setShuttleState().alongWith(new ShuttleAimCommand(drivetrain, () -> -joystick.getLeftY(), () -> -joystick.getLeftX())), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(8, 0, shooter.setVelocityAndStop(-70), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(7, 0, shooter.setVelocityAndStop(15), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(8, 0, flywheel.reachVelocity(-70), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(7, 0, flywheel.reachVelocity(15), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(5, 0, robotCommands.bumpFire(), MultiButton.RunCondition.WHEN_PRESSED);

buttonHelper.createButton(10, 0, robotCommands.handoff().withTimeout(2), MultiButton.RunCondition.WHEN_PRESSED);
buttonHelper.createButton(2, 0, intake.rollOut(-.65), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(4, 0, elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)).andThen(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)), MultiButton.RunCondition.WHEN_PRESSED);
buttonHelper.createButton(6, 0, shooter.spinFeederAndStop(-.1).alongWith(intake.rollIn(0.5)), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(4, 0, elevator.goDownAndStop(0.2).withTimeout(0.3).andThen(elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.DOWN)).andThen(pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)), MultiButton.RunCondition.WHEN_PRESSED);
buttonHelper.createButton(6, 0, feeder.feed(-.1).alongWith(intake.rollIn(0.5)), MultiButton.RunCondition.WHILE_HELD);
buttonHelper.createButton(9, 0, elevator.setPositionTo(ElevatorSubsystem.ElevatorConstants.ElevatorPositions.AMP).andThen(pivot.moveDown(-0.25).unless(
() -> pivot.getEncoderAngle() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))), MultiButton.RunCondition.WHEN_PRESSED);
() -> pivot.getPosition() < 0.4).withTimeout(0.6).andThen(pivot.adjustPivotPositionTo(0.03).unless(() -> !elevator.getMagSwitch()))), MultiButton.RunCondition.WHEN_PRESSED);
intake.intakeOccupiedTrigger.onTrue(vision.blinkLimelight().alongWith(successfulIntakeRumble()));
intake.intakeOccupiedTrigger.and(joystick.rightBumper().negate()).onTrue(robotCommands.handoff().withTimeout(2));
buttonHelper.createButton(11, 0, shooter.shooterTrap(), MultiButton.RunCondition.WHILE_HELD);

drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
drivetrain.applyRequest(
Expand Down Expand Up @@ -146,13 +138,13 @@ private void configureBindings() {
joystick.rightBumper().whileTrue(intake.rollIn(.7));

// Arm down
joystick.leftBumper().onTrue(arm.deployArm(0.5).alongWith(pivot.movePivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)));
joystick.leftBumper().onTrue(arm.deployArm(0.5).alongWith(pivot.adjustPivotPositionTo(PivotSubsystem.PivotConstants.PivotPosition.HANDOFF)));

// Amp Toggle
joystick.a().onTrue(robotCommands.setAmp());

// Shoot
joystick.rightTrigger().whileTrue(shooter.spinFeederMaxAndStop().alongWith(intake.rollOut(-1)));
joystick.rightTrigger().whileTrue(feeder.feed(-1).alongWith(intake.rollOut(-1)));
// Handoff
joystick.povUp().onTrue(robotCommands.handoff().withTimeout(2));
// Move elevator down
Expand Down Expand Up @@ -184,7 +176,7 @@ public void updateOdometryVision() {
}

public void buildAutoChooser() {
var namedCommands = new AutoNamedCommands(intake, shooter, pivot, arm, robotCommands);
var namedCommands = new AutoNamedCommands(intake, pivot, arm, feeder, robotCommands);
namedCommands.registerCommands();

autoChooser = new SendableChooser<>();
Expand Down
Loading