Skip to content
This repository has been archived by the owner on Apr 20, 2024. It is now read-only.

Commit

Permalink
shooter tuning
Browse files Browse the repository at this point in the history
  • Loading branch information
Owen756 committed Feb 23, 2024
1 parent 738944b commit c8d4652
Showing 1 changed file with 6 additions and 15 deletions.
21 changes: 6 additions & 15 deletions src/main/java/frc/robot/shooter/ShooterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
import frc.robot.util.scheduling.LifecycleSubsystem;
import frc.robot.util.scheduling.SubsystemPriority;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

public class ShooterSubsystem extends LifecycleSubsystem {
private static final double SPIN_RATIO = 3.0 / 5.0;
Expand All @@ -21,7 +20,6 @@ public class ShooterSubsystem extends LifecycleSubsystem {
private static final double TOLERANCE_RPM = 100;
private final TalonFX leftMotor;
private final TalonFX rightMotor;
private final LoggedDashboardNumber ntRPM = new LoggedDashboardNumber("Shooter/RPMOverride", -1);
private final VelocityTorqueCurrentFOC velocityRequest =
new VelocityTorqueCurrentFOC(0).withSlot(0).withLimitReverseMotion(true);
private double speakerDistance = 0;
Expand Down Expand Up @@ -77,7 +75,7 @@ public void robotPeriodic() {
Logger.recordOutput(
"Shooter/LeftMotor/POutput", leftMotor.getClosedLoopProportionalOutput().getValue());
Logger.recordOutput(
"Shooter/LeftMotor/SupplierCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
"Shooter/LeftMotor/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble());
Logger.recordOutput("Shooter/LeftMotor/StatorCurrent", leftMotor.getStatorCurrent().getValue());
Logger.recordOutput("Shooter/LeftMotor/Voltage", leftMotor.getMotorVoltage().getValue());
Logger.recordOutput(
Expand All @@ -88,18 +86,11 @@ public void robotPeriodic() {
Logger.recordOutput(
"Shooter/RightMotor/POutput", rightMotor.getClosedLoopProportionalOutput().getValue());
Logger.recordOutput(
"Shooter/RightMotor/SupplierCurrent", rightMotor.getSupplyCurrent().getValueAsDouble());
"Shooter/RightMotor/SupplyCurrent", rightMotor.getSupplyCurrent().getValueAsDouble());
Logger.recordOutput("Shooter/AtGoal", atGoal(goalMode));

double overrideRPM = ntRPM.get();

double usedGoalRPM = overrideRPM == -1 ? goalRPM : overrideRPM;

Logger.recordOutput("Shooter/OverrideRPM", overrideRPM);
Logger.recordOutput("Shooter/UsedRPM", usedGoalRPM);

rightMotor.setControl(velocityRequest.withVelocity((usedGoalRPM) / 60));
leftMotor.setControl(velocityRequest.withVelocity((usedGoalRPM * SPIN_RATIO) / 60));
leftMotor.setControl(velocityRequest.withVelocity((goalRPM) / 60));
rightMotor.setControl(velocityRequest.withVelocity((goalRPM * SPIN_RATIO) / 60));
}

public boolean atGoal(ShooterMode mode) {
Expand All @@ -111,8 +102,8 @@ public boolean atGoal(ShooterMode mode) {
return true;
}

if (Math.abs(goalRPM - getRPM(rightMotor)) < TOLERANCE_RPM
&& Math.abs((goalRPM * SPIN_RATIO) - getRPM(leftMotor)) < TOLERANCE_RPM) {
if (Math.abs((goalRPM * SPIN_RATIO) - getRPM(rightMotor)) < TOLERANCE_RPM
&& Math.abs(goalRPM - getRPM(leftMotor)) < TOLERANCE_RPM) {
return true;
}

Expand Down

0 comments on commit c8d4652

Please sign in to comment.