diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3d12c66f..2610e5e3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -152,7 +152,7 @@ public enum RobotEdition { * This is for when we're testing shot and extension numbers and should be FALSE once bring up is * complete */ - public static final boolean TUNING_MODE = false; + public static final boolean TUNING_MODE = true; public boolean hasZeroedSinceStartup = false; diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 656b8d82..5a4bbc1b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import frc.robot.utils.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -48,6 +49,10 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO + private LoggedTunableNumber kickerSpeed = new LoggedTunableNumber("Kicker Speed", 100); + + private LoggedTunableNumber spinnerSpeed = new LoggedTunableNumber("Spinner Speed", 13); + public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; this.spinnerIO = indexRollerIO; @@ -73,8 +78,8 @@ public Command kick() { // .withTimeout(3), // this.run( // () -> { - spinnerIO.setRollerVelocity(30); - kickerIO.setRollerVelocity(20); + spinnerIO.setRollerVelocity(spinnerSpeed.get()); + kickerIO.setRollerVelocity(kickerSpeed.get()); })); } @@ -92,7 +97,7 @@ public Command rest() { return this.run( () -> { spinnerIO.setRollerVoltage(0.0); - kickerIO.setRollerVoltage(-2); + kickerIO.setRollerVoltage(0.0); }); } @@ -155,7 +160,7 @@ public static TalonFXConfiguration getKickerConfig() { config.Slot0.kS = 0.22251; config.Slot0.kV = 0.17199; config.Slot0.kA = 0.024802; - config.Slot0.kP = 7; + config.Slot0.kP = 21; config.Slot0.kD = 0; config.CurrentLimits.StatorCurrentLimit = 80.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 632c5f84..964ad5cd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -66,6 +67,8 @@ public static class FlywheelIOInputs { private MotionMagicVelocityVoltage motionMagicVelocityVoltage; private VelocityTorqueCurrentFOC velocityTorqueCurrentFOC = new VelocityTorqueCurrentFOC(0.0).withSlot(1); + private VelocityVoltage velocityVoltage = + new VelocityVoltage(0.0).withEnableFOC(true).withSlot(0); private double velocitySetpointRotPerSec = 100.0; // TODO can't start w 0 @@ -141,6 +144,11 @@ public void setTorqueCurrentVel(double flywheelVel) { flywheelLeader.setControl(velocityTorqueCurrentFOC.withVelocity(flywheelVel)); } + public void setFlywheelVelocity(double flywheelVelocity) { + velocitySetpointRotPerSec = flywheelVelocity; + flywheelLeader.setControl(velocityVoltage.withVelocity(flywheelVelocity)); + } + public void stop() { // thought i should add a stop command, dont think i had to though velocitySetpointRotPerSec = 0.0; flywheelLeader.setControl(voltageOut.withOutput(0.0)); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 26976910..8ec4c0a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -252,7 +252,8 @@ public Command feed( Logger.recordOutput("Robot/Feed Target", feedTarget.get()); hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - flywheelIO.setMotionProfiledFlywheelVelocity( + // flywheelIO.setMotionProfiledFlywheelVelocity( + flywheelIO.setFlywheelVelocity( shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + AutoAim.getFudgeFactor()); turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); @@ -307,7 +308,8 @@ public Command spit() { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ANGLE); - flywheelIO.setMotionProfiledFlywheelVelocity(30); + // flywheelIO.setMotionProfiledFlywheelVelocity( + flywheelIO.setFlywheelVelocity(30); // i think we want it to eject as far out from the robot as possible turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY @@ -352,7 +354,8 @@ public Command score(Supplier shotParamsSupplier) { // turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); // case NONE: hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( + // flywheelIO.setMotionProfiledFlywheelVelocity( + flywheelIO.setFlywheelVelocity( shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + AutoAim.getFudgeFactor()); turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); @@ -505,11 +508,11 @@ public static TalonFXConfiguration getFlywheelConfig() { config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO; - // slot 0 is for motion profiled velocity + // slot 0 is for velocity config.Slot0.kS = 0.33706; // 0.63933; config.Slot0.kV = 0.13893; // 0.11582; config.Slot0.kA = 0.030026; // 0.020809; - config.Slot0.kP = 0.4; + config.Slot0.kP = 0.67; config.Slot0.kD = 0; // slot 1 is for torque current