From 0b7eb9b07bd907f06262ddbb4f599dc9b766c6cd Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Tue, 19 Mar 2024 17:08:57 -0400 Subject: [PATCH] updates --- .../experimental/SetShooterVelocity.java | 5 +- .../robot/experimental/ShootWhenReady.java | 8 ++- .../robot/subsystems/ShooterSubsystem2.java | 57 ++++++++++++------- 3 files changed, 45 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/experimental/SetShooterVelocity.java b/src/main/java/frc/robot/experimental/SetShooterVelocity.java index 3fd3a46..1cca413 100644 --- a/src/main/java/frc/robot/experimental/SetShooterVelocity.java +++ b/src/main/java/frc/robot/experimental/SetShooterVelocity.java @@ -1,7 +1,6 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystem2; public class SetShooterVelocity extends Command { @@ -23,13 +22,13 @@ public void initialize() { @Override public void execute() { - shooter.setShooterVelocity(velocity); + shooter.setTargetFlywheelVelocity(velocity); } @Override public void end(boolean interrupted) { - shooter.setShooterVelocity(0); + shooter.setTargetFlywheelVelocity(0); } @Override diff --git a/src/main/java/frc/robot/experimental/ShootWhenReady.java b/src/main/java/frc/robot/experimental/ShootWhenReady.java index 9fd33d5..5474807 100644 --- a/src/main/java/frc/robot/experimental/ShootWhenReady.java +++ b/src/main/java/frc/robot/experimental/ShootWhenReady.java @@ -1,6 +1,7 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystem2; @@ -23,8 +24,9 @@ public void initialize() { @Override public void execute() { - if (shooter.isShooterAtTargetVelocity() & index.HasCargo()) { - index.RunIndexing(); + if (shooter.isFlywheelAtTarget() & index.HasCargo()) { + // index.RunIndexing(); + index.SetPower(Constants.IndexConstants.INDEX_POWER); } else { index.SetPower(0); } @@ -33,7 +35,7 @@ public void execute() { @Override public void end(boolean interrupted) { - shooter.setShooterVelocity(0); + shooter.setTargetFlywheelVelocity(0); } @Override diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java index 41502bf..64e679a 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java @@ -10,7 +10,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -30,7 +29,7 @@ public class ShooterSubsystem2 extends SubsystemBase{ private VelocityVoltage m_velocity = new VelocityVoltage(0); private double flywheelVelocity; private boolean flywheelDisabled = false; - private double targetFlywheelSpeed; + private double targetFlywheelVelocity; public ShooterSubsystem2() { @@ -41,9 +40,6 @@ public ShooterSubsystem2() { m_primaryMotor.getConfigurator().apply(new TalonFXConfiguration()); m_secondaryMotor.getConfigurator().apply(new TalonFXConfiguration()); - m_primaryMotor.setInverted(false); - m_secondaryMotor.setInverted(false); - /* Various Configs */ var flywheelConfigs0 = new Slot0Configs(); flywheelConfigs0 @@ -53,13 +49,11 @@ public ShooterSubsystem2() { .withKS(0.0001) .withKV(0.12); - var flywheelVelocityConfig = new VoltageConfigs(); flywheelVelocityConfig - .withPeakForwardVoltage(12) // FRC 2910 running 12 - .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away + .withPeakForwardVoltage(8) // FRC 2910 running 12 + .withPeakReverseVoltage(8); // Originally -8, with negative the "helper" text goes away - var flywheelCurrentConfigs = new CurrentLimitsConfigs(); flywheelCurrentConfigs .withStatorCurrentLimit(60) @@ -81,33 +75,58 @@ public ShooterSubsystem2() { m_primaryMotor.getConfigurator().apply(flywheelMotorOutput); m_secondaryMotor.getConfigurator().apply(flywheelMotorOutput); - + + m_primaryMotor.setInverted(true); + m_secondaryMotor.setInverted(true); } // end of constructor + /* + // Reference: 1 /* Sets the velocity the motors are using */ - public void setTargetFlywheelVelocity(double flywheelVelocity) { - this.flywheelVelocity = flywheelVelocity; - // m_primaryMotor.setControl(m_primaryVelocity.withVelocity(flywheelVelocity)); -// m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(flywheelVelocity * 0.95)); + public void setTargetFlywheelVelocity(double targetFlywheelVelocity) { + this.targetFlywheelVelocity = targetFlywheelVelocity; + /* + m_primaryMotor.setControl(m_primaryVelocity.withVelocity(flywheelVelocity)); + m_secondaryMotor.setControl(m_secondaryVelocity.withVelocity(flywheelVelocity * 0.95)); + */ } + // Reference: 2 + /* public void setTargetFlywheelSpeed(double targetFlywheelSpeed) { this.targetFlywheelSpeed = targetFlywheelSpeed; } + */ + // Reference: 3 /* Returns the velocity of the flywheel */ + /* public double getTargetFlywheelSpeed(double targetFlywheelSpeed) { return targetFlywheelSpeed; } + */ + // Reference: 4 // Returns the velocity of the flywheel - public double getFlywheelVelocity() { - return m_primaryMotor.getVelocity() ; // TODO check if this is the right method - } - public boolean isFlywheelAtTargetVelocity() { - return Math.abs(getFlywheelVelocity() - targetFlywheelVelocity) < 5; // TODO arbitrary error for now + public StatusSignal getFlywheelVelocity() { + return m_primaryMotor.getVelocity(); } + + StatusSignal currentFlywheelVelocity = getFlywheelVelocity(); + // currentFlywheelVelocity = getFlywheelVelocity(); + + public boolean isFlywheelAtTarget() { + double velocityMarginOfError = 5.0; // TODO: Adjust this value as needed + if (Math.abs(targetFlywheelVelocity - currentFlywheelVelocity.getValue()) < velocityMarginOfError) + return true; + else + return false; + } // end of logic test + + // call this method with the target velocity as an argument + // boolean isAtTarget = isFlywheelAtTarget(targetFlywheelSpeed); + } // end of class ShooterSubsystem2