Add Shooter

.DataLogTool/datalogtool.json

@@ -1,5 +1,7 @@
 {
   "download": {
+    "deleteAfter": false,
+    "localDir": "C:\\MakeShift\\logs",
     "serverTeam": "4039"
   }
 }

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)));
 	}

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;
+  }
+}

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;

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);
+	}
+
+}

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;