diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 5a8aa4fa..b13796e1 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -82,8 +82,6 @@ public static class Indexer { public static final double INTAKE_MOI = 0.025; public static final double FEEDER_MOI = 0.025; public static final int VELOCITY_ERR_TOLERANCE_RPM = 10; - - } @@ -121,7 +119,7 @@ public static class Flywheels { public static final double SIM_MOI = 4.08232288e-4; // TODO: if it's tuned in simulation, it's tuned in real life - public static final double KP = 0.4; + public static final double KP = 0.1; public static final double KI = 0.0; public static final double KD = 0.0; public static final double KS = 0.26925; @@ -143,7 +141,7 @@ public static class Pivot { public static final double TOTAL_GEAR_RATIO = MOTOR_TO_CANCODER * CANCODER_TO_PIVOT; public static final double SIM_LENGTH_METERS = Units.inchesToMeters(12.910); // TODO: find the moi - public static final double SIM_MOI = 0.22552744227754662; + public static final double SIM_MOI = 0.04064471269; public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(60.0); public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(8.0); @@ -156,8 +154,15 @@ public static class Pivot { public static final double KG = 0.1; public static final double KV = 0.1; - public static final double CRUISE_VELOCITY_RPS = 4.0; - public static final double MAX_ACCEL_RPS2 = 40.0; + public static final double SIM_KP = 254; + public static final double SIM_KI = 0.0; + public static final double SIM_KD = 0.0; + public static final double SIM_KS = 0.0; + public static final double SIM_KG = 0.15; + public static final double SIM_KV = 0.187; + + public static final double CRUISE_VELOCITY_RPS = 1.0; + public static final double MAX_ACCEL_RPS2 = 10.0; public static final double JERK_RPS3 = 2000; public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2); diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index c87d1983..5e284c96 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -45,6 +45,10 @@ public void setFeederVelocity(double setpointRPM) { io.setFeederVelocity(setpointRPM); } + public void setFeederPercent(double percent) { + io.setFeederVoltage(12.0 * percent); + } + public Command feedToAmp() { return Commands.runOnce(() -> io.setFeederVelocity(-600), this); } @@ -70,6 +74,7 @@ public boolean isFeederAtSetpoint() { public void stopFeeder() { io.setFeederVoltage(0); } + public void stopIntake() { io.setIntakeVoltage(0); } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java index 1922ea27..dce3632b 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java @@ -25,8 +25,9 @@ public IndexerIOSparkMax() { intakeMotor.setSmartCurrentLimit(30); feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + feederMotor.setInverted(true); feederMotor.enableVoltageCompensation(12.0); - feederMotor.setSmartCurrentLimit(30); + feederMotor.setSmartCurrentLimit(40); feederPID = feederMotor.getPIDController(); feederPID.setP(FEEDER_KP, 0); diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java index 65296bfa..b6d25052 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import org.team1540.robot2024.util.LoggedTunableNumber; +import org.team1540.robot2024.util.MechanismVisualiser; import org.team1540.robot2024.util.math.AverageFilter; import java.util.function.Supplier; @@ -27,7 +28,7 @@ public class Shooter extends SubsystemBase { private double leftFlywheelSetpointRPM; private double rightFlywheelSetpointRPM; - private Rotation2d pivotSetpoint; + private Rotation2d pivotSetpoint = new Rotation2d(); private final LoggedTunableNumber flywheelsKP = new LoggedTunableNumber("Shooter/Flywheels/kP", Flywheels.KP); private final LoggedTunableNumber flywheelsKI = new LoggedTunableNumber("Shooter/Flywheels/kI", Flywheels.KI); @@ -49,6 +50,7 @@ public void periodic() { flywheelsIO.updateInputs(flywheelInputs); Logger.processInputs("Shooter/Pivot", pivotInputs); Logger.processInputs("Shooter/Flywheels", flywheelInputs); + MechanismVisualiser.setShooterPivotRotation(getPivotPosition()); // Update tunable numbers if (flywheelsKP.hasChanged(hashCode()) || flywheelsKI.hasChanged(hashCode()) || flywheelsKD.hasChanged(hashCode())) { @@ -62,6 +64,7 @@ public void periodic() { leftSpeedFilter.add(getLeftFlywheelSpeed()); rightSpeedFilter.add(getRightFlywheelSpeed()); pivotPositionFilter.add(getPivotPosition().getRotations()); + Logger.recordOutput("Shooter/Pivot/Setpoint", pivotSetpoint.getDegrees()); } /** @@ -180,5 +183,4 @@ public Command setPivotPositionCommand(Rotation2d setpoint) { this::isPivotAtSetpoint, this); } - } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java index 64a79449..bd25f722 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java @@ -24,8 +24,8 @@ public class ShooterPivotIOSim implements ShooterPivotIO { MIN_ANGLE.getRadians()); private final ProfiledPIDController controller = - new ProfiledPIDController(KP, KI, KD, new TrapezoidProfile.Constraints(CRUISE_VELOCITY_RPS, MAX_ACCEL_RPS2)); - private final ArmFeedforward feedforward = new ArmFeedforward(KS, KG, KV); + new ProfiledPIDController(SIM_KP, SIM_KI, SIM_KD, new TrapezoidProfile.Constraints(CRUISE_VELOCITY_RPS, MAX_ACCEL_RPS2)); + private final ArmFeedforward feedforward = new ArmFeedforward(SIM_KS, SIM_KG, SIM_KV); private boolean isClosedLoop; private TrapezoidProfile.State goalState; @@ -38,8 +38,8 @@ public void updateInputs(ShooterPivotIOInputs inputs) { appliedVolts = controller.calculate(Units.radiansToRotations(sim.getAngleRads()), goalState) + feedforward.calculate( - Units.rotationsToRadians(controller.getSetpoint().position), - controller.getSetpoint().velocity); + Units.rotationsToRadians(controller.getSetpoint().position), + controller.getSetpoint().velocity); } sim.setInputVoltage(appliedVolts); @@ -58,7 +58,7 @@ public void setPosition(Rotation2d position) { Units.radiansToRotations(sim.getVelocityRadPerSec()) ); isClosedLoop = true; - goalState = new TrapezoidProfile.State(position.getRadians(), 0); + goalState = new TrapezoidProfile.State(position.getRotations(), 0); } @Override