From 682b57e99ad923c07fda44d61204bfc76724b5d1 Mon Sep 17 00:00:00 2001 From: DrorMargalit Date: Thu, 18 Jan 2024 00:42:27 +0200 Subject: [PATCH 1/3] Shooter Subsystem, Command setSpeed and SetSpeedInterpolation --- src/main/java/frc/robot/Constants.java | 35 +++++++ .../frc/robot/commands/ShooterSetSpeed.java | 39 ++++++++ .../ShooterSetSpeedInterpolation.java | 42 ++++++++ .../frc/robot/subsystems/ShooterSubsysem.java | 95 +++++++++++++++++++ 4 files changed, 211 insertions(+) create mode 100644 src/main/java/frc/robot/commands/ShooterSetSpeed.java create mode 100644 src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsysem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7432df0..cdd7978 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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); + + } } 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..2b746ae --- /dev/null +++ b/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; + } +} 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..e1d6185 --- /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, 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; + } +} 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..13f4ce1 --- /dev/null +++ b/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 + } +} \ No newline at end of file From 0fc3f524805ab41d13fee63727363e594e5a7191 Mon Sep 17 00:00:00 2001 From: DrorMargalit Date: Thu, 18 Jan 2024 10:01:12 +0200 Subject: [PATCH 2/3] Fixing problems --- .../java/frc/robot/commands/ShooterSetSpeedInterpolation.java | 2 +- src/main/java/frc/robot/subsystems/ShooterSubsysem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java b/src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java index e1d6185..63a0479 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedInterpolation.java @@ -14,7 +14,7 @@ public class ShooterSetSpeedInterpolation extends Command { /** Creates a new ShooterSetSpeedInterpolation. */ public ShooterSetSpeedInterpolation() { - this.addRequirements(shooterSubsystem, swerve); + this.addRequirements(shooterSubsystem); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java index 13f4ce1..f785288 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java @@ -33,7 +33,7 @@ public static ShooterSubsysem getInstance(){ } /** Creates a new ShooterSubsysem. */ - public ShooterSubsysem() { + private ShooterSubsysem() { // giving values to the motors this.m_shooterMotor = new TalonFX(kMotorShooterID); From 4031718b2b69fca63e298a29cb9d605ea43f830b Mon Sep 17 00:00:00 2001 From: kully Date: Fri, 19 Jan 2024 22:13:12 +0200 Subject: [PATCH 3/3] use InstantCommand --- .../frc/robot/commands/ShooterSetSpeed.java | 20 ++----- .../frc/robot/subsystems/ShooterSubsysem.java | 53 +++++++++---------- 2 files changed, 29 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeed.java b/src/main/java/frc/robot/commands/ShooterSetSpeed.java index 2b746ae..c6fe41b 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeed.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeed.java @@ -4,17 +4,17 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.ShooterSubsysem; -public class ShooterSetSpeed extends Command { +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; - // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -22,18 +22,4 @@ public ShooterSetSpeed(double speed) { 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; - } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java index f785288..0819ac4 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsysem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsysem.java @@ -18,14 +18,13 @@ 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(){ + public static ShooterSubsysem getInstance() { if (instance == null) { instance = new ShooterSubsysem(); } @@ -39,54 +38,54 @@ private ShooterSubsysem() { // declaring Configs TalonFXConfiguration configs = new TalonFXConfiguration(); - MotionMagicConfigs shooterMM = new MotionMagicConfigs(); - - //giving motion magic values + 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; + // giving PID values + configs.Slot0.kP = kP; configs.Slot0.kD = kD; configs.Slot0.kS = kS; configs.Slot0.kV = kV; - //max voltage for m_shooterMotor + // max voltage for m_shooterMotor configs.Voltage.PeakForwardVoltage = PeakForwardVoltage; - configs.Voltage.PeakReverseVoltage = PeakReverseVoltage; + configs.Voltage.PeakReverseVoltage = PeakReverseVoltage; configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio; - //Checking if m_shooterMotor apply configs + // Checking if m_shooterMotor apply configs StatusCode status = StatusCode.StatusCodeNotInitialized; - for (int i = 0; i<5; ++i){ + for (int i = 0; i < 5; ++i) { status = m_shooterMotor.getConfigurator().apply(configs); if (status.isOK()) break; } - if (!status.isOK()){ + 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)); - } + // 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 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())); - } + 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() {