From b492205f3f02e805fe51efaad5c914be310fb17f Mon Sep 17 00:00:00 2001 From: MakeshiftProgrammingSix Date: Wed, 28 Jan 2026 19:35:07 -0500 Subject: [PATCH 1/2] shooter shoots --- .DataLogTool/datalogtool.json | 2 + src/main/java/frc/robot/RobotContainer.java | 14 ++- .../java/frc/robot/commands/ShootCommand.java | 42 +++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 115 ++++++++++++++++++ 5 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ShootCommand.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json index 6eeb3a4..bea6121 100644 --- a/.DataLogTool/datalogtool.json +++ b/.DataLogTool/datalogtool.json @@ -1,5 +1,7 @@ { "download": { + "deleteAfter": false, + "localDir": "C:\\MakeShift\\logs", "serverTeam": "4039" } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26886a7..ebd73ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,8 +18,10 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.commands.RobotCentricDriveCommand; +import frc.robot.commands.ShootCommand; import frc.robot.commands.TeleopDriveCommand; import frc.robot.commands.TurretAprilTagAimCommand; import frc.robot.commands.FeederCommand; @@ -58,6 +60,8 @@ public class RobotContainer { private final SpindexerSubsystem feederSubsystem = new SpindexerSubsystem(); private final FeederSubsystem turretFeederSubsystem = new FeederSubsystem(); + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final CommandXboxController driver = new CommandXboxController( OperatorConstants.kDriverControllerPort); @@ -76,6 +80,7 @@ public RobotContainer() { .ignoringDisable(true) ); SmartDashboard.putData(feederSubsystem); + SmartDashboard.putData(shooterSubsystem); hardwareMonitor.registerDevice(null, driver); @@ -120,11 +125,12 @@ private void configureBindings() { driver.leftTrigger().whileTrue(new IntakeCommand(intakeSubsystem, true)); driver.leftBumper().whileTrue(new IntakeCommand(intakeSubsystem, false)); - + driver.rightBumper().whileTrue(new SpindexerCommand(feederSubsystem, true).alongWith(new FeederCommand(turretFeederSubsystem, true))); + driver.rightTrigger().whileTrue(new ShootCommand(shooterSubsystem)); //driver.rightTrigger().whileTrue(new TurretAprilTagAimCommand(turretSubsystem, driveSubsystem)); //driver.rightTrigger().whileTrue(new OwlHeadTurretCommand(() -> driveSubsystem.getHeading(), turretSubsystem)); - driver.rightBumper().whileTrue(new AlignToTowerCommandGroup(driveSubsystem, visionSubsystem)); + //driver.rightBumper().whileTrue(new AlignToTowerCommandGroup(driveSubsystem, visionSubsystem)); operator.axisMagnitudeGreaterThan(XboxController.Axis.kRightX.value, 0.25) .or( @@ -142,8 +148,8 @@ private void configureBindings() { operator.leftTrigger().whileTrue(new SpindexerCommand(feederSubsystem, true)); operator.leftBumper().whileTrue(new SpindexerCommand(feederSubsystem, false)); - operator.rightTrigger().whileTrue(new FeederCommand(turretFeederSubsystem, true)); - operator.rightBumper().whileTrue(new FeederCommand(turretFeederSubsystem, false)); + operator.rightTrigger().whileTrue(new IntakeCommand(intakeSubsystem, true)); + operator.rightBumper().whileTrue(new FeederCommand(turretFeederSubsystem, true)); operator.a().whileTrue(new FeederCommand(turretFeederSubsystem, true).alongWith(new SpindexerCommand(feederSubsystem, true))); } diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java new file mode 100644 index 0000000..633f2e5 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ShooterSubsystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ShootCommand extends Command { + /** Creates a new ShootCommand. */ + + private final ShooterSubsystem shooterSubsystem; + public ShootCommand(ShooterSubsystem shooterSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.shooterSubsystem = shooterSubsystem; + addRequirements(shooterSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooterSubsystem.shoot(true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooterSubsystem.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 4da4340..e720c61 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -19,7 +19,7 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new Intake. */ public static final class IntakeContants { static int kIntakeMotorCanID = 21; - static double kIntakeSpeed = -1; + static double kIntakeSpeed = -0.6; } private final SparkFlex intakeMotor; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..aea6d4f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems; + +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.controls.Follower; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.FieldConstants; + +public class ShooterSubsystem extends SubsystemBase { + public final class ShooterConstants { + // How fast the turret rotates from one point to another. + public static final double KMotorVelocity = 20; + + // Device ids + public static final int kLeaderMotorID = 32; + public static final int kFollowerMotorID = 33; + + + // Pid values + public static final double kVShooter = 0.0; + public static final double kSShooter = 0.0; + public static final double kAShooter = 50; + public static final double kPShooter = 0.3; + public static final double kIShooter = 0.0; + public static final double kDShooter = 0.0; + + + + + } + + private TalonFX shooterLeaderMotor, shooterFollowerMotor; + + Follower follower; + + public ShooterSubsystem() { + shooterLeaderMotor = new TalonFX(ShooterConstants.kLeaderMotorID); + shooterFollowerMotor = new TalonFX(ShooterConstants.kFollowerMotorID); + + + shooterLeaderMotor.setNeutralMode(NeutralModeValue.Coast); + shooterFollowerMotor.setNeutralMode(NeutralModeValue.Coast); + + follower = new Follower(ShooterConstants.kLeaderMotorID, MotorAlignmentValue.Opposed); + + shooterFollowerMotor.setControl(follower); + MotorOutputConfigs mcfg = new MotorOutputConfigs(); + + mcfg.withInverted(InvertedValue.CounterClockwise_Positive); + mcfg.withNeutralMode(NeutralModeValue.Coast); + + TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration().withMotorOutput(mcfg); + + Slot0Configs slotConfigs = talonFXConfigs.Slot0; + + slotConfigs.kS = ShooterConstants.kSShooter; + slotConfigs.kV = ShooterConstants.kVShooter; + slotConfigs.kA = ShooterConstants.kAShooter; + slotConfigs.kP = ShooterConstants.kPShooter; + slotConfigs.kI = ShooterConstants.kIShooter; + slotConfigs.kD = ShooterConstants.kDShooter; + + shooterLeaderMotor.getConfigurator().apply(talonFXConfigs); + } + + + + public void shoot(Boolean forward) { + final VelocityVoltage request = new VelocityVoltage(ShooterConstants.KMotorVelocity).withSlot(0); + + if(forward) { + shooterLeaderMotor.setControl(request.withVelocity(ShooterConstants.KMotorVelocity)); + } else { + shooterLeaderMotor.setControl(request.withVelocity(-1 * ShooterConstants.KMotorVelocity)); + } + } + + public void stop() { + shooterLeaderMotor.stopMotor(); + } + + + + @Override + public void periodic() { + // For safety, stop the turret whenever the robot is disabled. + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addDoubleProperty("Shooter speed", () -> shooterLeaderMotor.getVelocity().getValueAsDouble(), null); + builder.addDoubleProperty("Shooter acceleration", () -> shooterLeaderMotor.getAcceleration().getValueAsDouble(), null); + } + +} From 6ff35f3699617aef74e7d8ac5db75f0313e82f37 Mon Sep 17 00:00:00 2001 From: MakeshiftProgrammingSix Date: Fri, 30 Jan 2026 11:28:38 -0500 Subject: [PATCH 2/2] Slower Spindexer --- src/main/java/frc/robot/subsystems/SpindexerSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/SpindexerSubsystem.java index 4782288..cb27b7f 100644 --- a/src/main/java/frc/robot/subsystems/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SpindexerSubsystem.java @@ -32,7 +32,7 @@ public class SpindexerSubsystem extends SubsystemBase { // Constants of the feeder subsystem public static final class SpindexerConstants { public static final int kSpindexerMotorId = 41; - public static final double kSpindexerSpeed = -2200; + public static final double kSpindexerSpeed = -1100; public static final double kSpindexerWheelP = 0.0004;//0.0002 public static final double kSpindexerWheelI = 0;