Shooter

src/main/java/frc/robot/Constants.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import frc.Utils.interpolation.InterpolationMap;
 import frc.Utils.swerve.COTSFalconSwerveConstants;
 import frc.Utils.swerve.SwerveModuleConstants;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -197,4 +198,38 @@ public static class Vision {
 
     public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0)));
   }
+
+    public static class ShooterConstants {
+        //m_shooterMotor ID
+        public static final int kMotorShooterID = 20;
+
+        //Motion Magic Values
+        public static final int MotionMagicCruiseVelocity = 5;
+        public static final int MotionMagicAcceleration = 10;
+        public static final int MotionMagicJerk = 50;
+
+        public static final double PeakForwardVoltage = 11.5;
+        public static final double PeakReverseVoltage = -11.5;
+
+        public static final int SensorToMechanismRatio = 50;
+
+        public static final int MaxError = 13;
+
+        //PID values
+        public static final int kP = 24;
+        public static final double kD = 0.1;
+        public static final double kS = 0.12;
+        public static final double kV = 0.25;
+
+        //Interpolation Map
+        public static final InterpolationMap ShooterInterpolation = new InterpolationMap()
+        .put(1,9)
+        .put(1.2, 9.2)
+        .put(1.4, 9.4)
+        .put(1.6, 9.6)
+        .put(1.8, 9.8)
+        .put(2, 10)
+        .put(2.1, 10.2);
+
+    }
 }

src/main/java/frc/robot/commands/ShooterSetSpeed.java

@@ -0,0 +1,39 @@
+// 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.ShooterSubsysem;
+
+public class ShooterSetSpeed extends Command {
+  private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance();
+  double speed;
+  /** Creates a new ShooterSetSpeed. */
+  public ShooterSetSpeed(double speed) {
+    this.addRequirements(shooterSubsystem);
+    this.speed = speed;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.shooterSubsystem.setShooterSpeed(speed);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.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.ShooterSubsysem;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class ShooterSetSpeedInterpolation extends Command {
+  private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance();
+  private final SwerveSubsystem swerve = SwerveSubsystem.getInstance();
+
+  /** Creates a new ShooterSetSpeedInterpolation. */
+  public ShooterSetSpeedInterpolation() {
+    this.addRequirements(shooterSubsystem, swerve);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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() {
+    this.shooterSubsystem.setShooterSpeed(shooterSubsystem.InterpolationValue(swerve.getPose()));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/ShooterSubsysem.java

@@ -0,0 +1,95 @@
+// 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.subsystems;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import static frc.robot.Constants.ShooterConstants.*;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.Utils.interpolation.InterpolateUtil;
+import frc.Utils.vision.Vision;
+
+public class ShooterSubsysem extends SubsystemBase {
+  private TalonFX m_shooterMotor;
+  private final SwerveSubsystem swerve = SwerveSubsystem.getInstance();
+  private final Vision vision = Vision.getInstance();
+
+  private final MotionMagicVelocityVoltage motionMagic = new MotionMagicVelocityVoltage(0);
+
+  private static ShooterSubsysem instance;
+
+  public static ShooterSubsysem getInstance(){
+    if (instance == null) {
+      instance = new ShooterSubsysem();
+    }
+    return instance;
+  }
+
+  /** Creates a new ShooterSubsysem. */
+  public ShooterSubsysem() {
+    // giving values to the motors
+    this.m_shooterMotor = new TalonFX(kMotorShooterID);
+
+    // declaring Configs
+    TalonFXConfiguration configs = new TalonFXConfiguration();
+    MotionMagicConfigs shooterMM =  new MotionMagicConfigs();
+
+    //giving motion magic values
+    shooterMM.MotionMagicCruiseVelocity = MotionMagicCruiseVelocity;
+    shooterMM.MotionMagicAcceleration = MotionMagicAcceleration;
+    shooterMM.MotionMagicJerk = MotionMagicJerk;
+    configs.MotionMagic = shooterMM;
+
+    //giving PID values
+    configs.Slot0.kP =kP;
+    configs.Slot0.kD = kD;
+    configs.Slot0.kS = kS;
+    configs.Slot0.kV = kV;
+
+    //max voltage for m_shooterMotor
+    configs.Voltage.PeakForwardVoltage = PeakForwardVoltage;
+    configs.Voltage.PeakReverseVoltage = PeakReverseVoltage; 
+
+    configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio;
+
+    //Checking if m_shooterMotor apply configs
+    StatusCode status = StatusCode.StatusCodeNotInitialized;
+    for (int i = 0; i<5; ++i){
+      status = m_shooterMotor.getConfigurator().apply(configs);
+      if (status.isOK())
+        break;
+    }
+    if (!status.isOK()){
+      System.out.println("Shooter could not apply configs, error code " + status.toString());
+    }
+    }
+
+    //set (active) shooter motor speed
+    public void setShooterSpeed(double speed){
+      this.m_shooterMotor.setControl(motionMagic.withVelocity(speed));
+    }
+
+    public double getShooterSpeed(){
+        return m_shooterMotor.getVelocity().getValue();
+    }
+
+    public boolean checkIfShooterAtSpeed(){
+      return (Math.abs(m_shooterMotor.getClosedLoopError().getValue()) < MaxError);
+    }
+
+    public double InterpolationValue(Pose2d pose2d){
+      return InterpolateUtil.interpolate(ShooterInterpolation, vision.DistanceFromTarget(swerve.getPose()));
+   }    
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}