diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9f81897..d310af7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,6 +21,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; @@ -226,6 +227,40 @@ public static class Vision { public static final Matrix kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0))); + 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); + } } diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeed.java b/src/main/java/frc/robot/commands/ShooterSetSpeed.java new file mode 100644 index 0000000..c6fe41b --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterSetSpeed.java @@ -0,0 +1,25 @@ +// 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.InstantCommand; +import frc.robot.subsystems.ShooterSubsysem; + +public class ShooterSetSpeed extends InstantCommand { + private final ShooterSubsysem shooterSubsystem = ShooterSubsysem.getInstance(); + double speed; + + /** Creates a new ShooterSetSpeed. */ + public ShooterSetSpeed(double speed) { + this.addRequirements(shooterSubsystem); + this.speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.shooterSubsystem.setShooterSpeed(speed); + } +} diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java b/src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java new file mode 100644 index 0000000..63a0479 --- /dev/null +++ b/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); + // 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java new file mode 100644 index 0000000..0819ac4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java @@ -0,0 +1,94 @@ +// 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 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. */ + private 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(pose2d)); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} \ No newline at end of file