From e808b77c4617e2a13b45f8282f9ec37d8c91df9b Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Thu, 28 Mar 2024 23:25:00 -0400 Subject: [PATCH] Added motion magic vel volt for flywheel --- src/main/java/frc/robot/RobotContainer.java | 7 +- .../robot/subsystems/ShooterSubsystem2.java | 35 +++--- .../robot/{subsystems => util}/ArmIO.java.txt | 0 src/main/java/frc/robot/util/FlywheelIO.java | 5 - .../java/frc/robot/util/FlywheelIO.java.txt | 100 ++++++++++++++++++ 5 files changed, 123 insertions(+), 24 deletions(-) rename src/main/java/frc/robot/{subsystems => util}/ArmIO.java.txt (100%) delete mode 100644 src/main/java/frc/robot/util/FlywheelIO.java create mode 100644 src/main/java/frc/robot/util/FlywheelIO.java.txt diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdfdd83..07023a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -208,7 +208,12 @@ private void configureBindings() { // TODO Testing "autonShootWhenReady" Remove later // m_driverController.rightTrigger().whileTrue(autonShootWhenReady); - m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75)); + + // TODO Test MotionMagicVelocityVoltage" only, then remove + m_driverController.leftTrigger().whileTrue(new InstantCommand(() -> shooter2.setFlywheelVelocityMM(shooter2.FLYWHEEL_VELOCITY))); + + // TODO Remove comment out after testing the MotionMagic Velocity Voltage + // m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75)); /* TODO These are the rrevious event commands and bindings */ // m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter)); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java index 9d3b92b..6d07c20 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -14,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.Constants; -@SuppressWarnings("unused") +// @SuppressWarnings("unused") public class ShooterSubsystem2 extends SubsystemBase{ @@ -34,15 +35,14 @@ public class ShooterSubsystem2 extends SubsystemBase{ // Class member variables private VelocityVoltage m_velocityVoltage = new VelocityVoltage(0); + // TODO Something to try + private MotionMagicVelocityVoltage mmVelocityVoltage = new MotionMagicVelocityVoltage(0); private double currentFlywheelVel = m_primaryMotor.getVelocity().getValue(); public ShooterSubsystem2() { /* Verbose? Absolutely. Effective? I hope so */ - - - m_primaryMotor.setControl(m_velocityVoltage); m_secondaryMotor.setControl(m_velocityVoltage); @@ -59,18 +59,10 @@ public ShooterSubsystem2() { | kd | output per unit of error derivative in velocity (output/(rps/s)) */ - /* Example provided by CTRE */ - /* - slot0Configs.kS = 0.05; // Add 0.05 V output to overcome static friction - slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output - slot0Configs.kP = 0.11; // An error of 1 rps results in 0.11 V output - slot0Configs.kI = 0; // no output for integrated error - slot0Configs.kD = 0; // no output for error derivative - */ + // https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java var flywheelConfigs0 = new Slot0Configs(); flywheelConfigs0 - // https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java .withKP(0.10) .withKI(0.00) .withKS(0.00) // Should low for a light flywheel? Maybe the pulley strength would impact it though? @@ -79,8 +71,8 @@ public ShooterSubsystem2() { var flywheelVelocityConfig = new VoltageConfigs(); flywheelVelocityConfig - .withPeakForwardVoltage(12) // FRC 2910 running 12 - .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away + .withPeakForwardVoltage(12) + .withPeakReverseVoltage(12); var flywheelCurrentConfigs = new CurrentLimitsConfigs(); flywheelCurrentConfigs @@ -106,11 +98,8 @@ public ShooterSubsystem2() { m_primaryMotor.setInverted(true); m_secondaryMotor.setInverted(true); - - // Alternate way to get the current flywheel velocity? } // end of constructor - public void setFlywheelVelocity(double velocity) { m_primaryMotor.setControl(m_velocityVoltage.withVelocity(velocity)); @@ -125,6 +114,16 @@ public void stopFlywheel() { m_primaryMotor.setControl(m_velocityVoltage.withVelocity(0)); m_secondaryMotor.setControl(m_velocityVoltage.withVelocity(0)); } + + /* A set of methods using MotionMagicVelocityVoltage */ + public void setFlywheelVelocityMM(double velocity) { + m_primaryMotor.setControl(mmVelocityVoltage.withVelocity(velocity)); + m_secondaryMotor.setControl(mmVelocityVoltage.withVelocity(velocity)); + } + public void setFlywheelToIdleMM() { + m_primaryMotor.setControl(mmVelocityVoltage.withVelocity(FLYWHEEL_IDLE_VELOCITY)); + m_secondaryMotor.setControl(mmVelocityVoltage.withVelocity(FLYWHEEL_IDLE_VELOCITY)); + } // See also `var currentFlywheelVel = m_primaryMotor.getVelocity().getValue()` in the Constructor diff --git a/src/main/java/frc/robot/subsystems/ArmIO.java.txt b/src/main/java/frc/robot/util/ArmIO.java.txt similarity index 100% rename from src/main/java/frc/robot/subsystems/ArmIO.java.txt rename to src/main/java/frc/robot/util/ArmIO.java.txt diff --git a/src/main/java/frc/robot/util/FlywheelIO.java b/src/main/java/frc/robot/util/FlywheelIO.java deleted file mode 100644 index a5d15b0..0000000 --- a/src/main/java/frc/robot/util/FlywheelIO.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.util; - -public class FlywheelIO { - -} diff --git a/src/main/java/frc/robot/util/FlywheelIO.java.txt b/src/main/java/frc/robot/util/FlywheelIO.java.txt new file mode 100644 index 0000000..62ba7ab --- /dev/null +++ b/src/main/java/frc/robot/util/FlywheelIO.java.txt @@ -0,0 +1,100 @@ +package frc.robot.util; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class FlywheelIO { + + // Declare variables for the motors to be controlled + private TalonFX m_primaryMotor = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_ID, "rio"); // Right + private TalonFX m_secondaryMotor = new TalonFX(Constants.CANIDs.LEFT_FLYWHEEL_ID, "rio"); // Left + + // TODO Tune this value + public final double FLYWHEEL_VELOCITY = 100; // rotations per second (rps) + public final double FLYWHEEL_VELOCITY_SHORT_RANGE = 40; // Not currently in use anywhere + public final double FLYWHEEL_VELOCITY_LONG_RANGE = 60; // Not currently in use anywhere, Flywheel speed for long range shots only + public final double FLYWHEEL_IDLE_VELOCITY = FLYWHEEL_VELOCITY * 0.30; // 30% of max speed + public final double FLYWHEEL_MARGIN_ERROR = FLYWHEEL_VELOCITY * 0.10; // 5% of max speed + public final double FLYWHEEL_MIN = FLYWHEEL_VELOCITY * .95; + public final double FLYWHEEL_MAX = FLYWHEEL_VELOCITY * 1.05; + + // Class member variables + private VelocityVoltage m_velocityVoltage = new VelocityVoltage(0); + + private double currentFlywheelVel = m_primaryMotor.getVelocity().getValue(); + + public FlywheelIO() { + /* Verbose? Absolutely. Effective? I hope so */ + + m_primaryMotor.setControl(m_velocityVoltage); + m_secondaryMotor.setControl(m_velocityVoltage); + + m_primaryMotor.getConfigurator().apply(new TalonFXConfiguration()); + m_secondaryMotor.getConfigurator().apply(new TalonFXConfiguration()); + + /* Various Configs */ + /* + | Ks | output to overcome static friction (output) + | Kv | output per unit of velocity (output/rps) + | Ka | output per unit of acceleration; unused, as there is no target acceleration (output/(rps/s)) + | Kp | output per unit of error (output/rps) + | Ki | output per unit of integrated error in velocity (output/rotation) + | kd | output per unit of error derivative in velocity (output/(rps/s)) + */ + + /* Example provided by CTRE */ + /* + slot0Configs.kS = 0.05; // Add 0.05 V output to overcome static friction + slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kP = 0.11; // An error of 1 rps results in 0.11 V output + slot0Configs.kI = 0; // no output for integrated error + slot0Configs.kD = 0; // no output for error derivative + */ + + var flywheelConfigs0 = new Slot0Configs(); + flywheelConfigs0 + // https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java + .withKP(0.10) + .withKI(0.00) + .withKS(0.00) // Should low for a light flywheel? Maybe the pulley strength would impact it though? + .withKV(0.00); + + + var flywheelVelocityConfig = new VoltageConfigs(); + flywheelVelocityConfig + .withPeakForwardVoltage(12) // FRC 2910 running 12 + .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away + + var flywheelCurrentConfigs = new CurrentLimitsConfigs(); + flywheelCurrentConfigs + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true); + + var flywheelMotorOutput = new MotorOutputConfigs(); + flywheelMotorOutput + .withNeutralMode(NeutralModeValue.Coast); + + /* Apply Configs */ + m_primaryMotor.getConfigurator().apply(flywheelConfigs0); + m_secondaryMotor.getConfigurator().apply(flywheelConfigs0); + + m_primaryMotor.getConfigurator().apply(flywheelVelocityConfig); + m_secondaryMotor.getConfigurator().apply(flywheelVelocityConfig); + + m_primaryMotor.getConfigurator().apply(flywheelCurrentConfigs); + m_secondaryMotor.getConfigurator().apply(flywheelCurrentConfigs); + + m_primaryMotor.getConfigurator().apply(flywheelMotorOutput); + m_secondaryMotor.getConfigurator().apply(flywheelMotorOutput); + + m_primaryMotor.setInverted(true); + m_secondaryMotor.setInverted(true); + + } +}