Skip to content

R2 154 tuning pid constants #74

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rhit-halseysh merged 3 commits into from
Mar 15, 2026
Merged

R2 154 tuning pid constants #74

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c899e1a
Button mappings & Tuning 3/15/26
GhostR406 Mar 15, 2026
ca8e75d
Merge branch 'R2-154-tuning-pid-constants' of https://github.com/FRC5…
GhostR406 Mar 15, 2026
08ab4a6
applied spotlessly
GhostR406 Mar 15, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -479,7 +479,7 @@ public class IntakeFlywheelConstants {
public static final Angle MIN_ANGLE = Rotations.of(0.0);
public static final Angle MAX_ANGLE = Rotations.of(1);
public static final Angle STARTING_ANGLE = Rotations.of(0.0);
public static final double PICKUP_SPEED = 0.0;
public static final double PICKUP_SPEED = 30.0;
public static final Distance WHEEL_RADIUS = Meters.of(0.05);
public static final Translation3d OFFSET = Translation3d.kZero;
public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125);
Expand Down Expand Up @@ -615,7 +615,7 @@ public static VisionSystemSim getSystemSim() {
public class IntakePivotConstants {
public static final String NAME = "Intake";

public static final Angle PICKUP_ANGLE = Degrees.of(130.0);
public static final Angle PICKUP_ANGLE = Degrees.of(123.0);
public static final Angle STOW_ANGLE = Degrees.of(0.0);

public static final Angle TOLERANCE = Degrees.of(1.0);
Expand All @@ -628,7 +628,7 @@ public class IntakePivotConstants {
private static final double SENSOR_TO_MECHANISM = 17.35;

public static final Angle MIN_ANGLE = Degrees.of(0.0);
public static final Angle MAX_ANGLE = Degrees.of(130.0);
public static final Angle MAX_ANGLE = Degrees.of(126.0);
public static final Angle STARTING_ANGLE = Degrees.of(0.0);
public static final Distance ARM_LENGTH = Foot.one();

Expand Down
39 changes: 23 additions & 16 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@

import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;

import com.ctre.phoenix6.BaseStatusSignal;
import com.pathplanner.lib.auto.AutoBuilder;
Expand Down Expand Up @@ -73,7 +72,7 @@ public class RobotContainer {

// Subsystems
public final Drive drive;
private final Hopper hopper;
public final Hopper hopper;
private final Shooter shooter;
public final Intake intake;
// private final BallCounter ballCounter;
Expand Down Expand Up @@ -366,28 +365,36 @@ private void configureButtonBindings() {
controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(100)));
controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0)));

// Intake + Spindexer + Tower
// Intake Rollers 11 Motor: 9 Intake

// ALPHA KATIE REQUESTS INTAKE RIGHT TRIGGER, SHOOT LEFT TRIGGER, AUTO ALIGN "A"

// Intake
controller.rightTrigger().whileTrue(intake.intake());
controller.rightTrigger().onFalse(intake.stowAndStopRollers());

// Shoot
controller
.rightBumper()
.leftTrigger()
.whileTrue(
Commands.parallel(
// intake.runRollers(RotationsPerSecond.of(30)),
hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(30))));
shooter.prepareToShoot(RotationsPerSecond.of(100), RotationsPerSecond.of(30)),
hopper.runSpindexer(RotationsPerSecond.of(15))));

controller
.rightBumper()
.leftTrigger()
.onFalse(
Commands.parallel(
intake.runRollers(RotationsPerSecond.of(0)),
hopper.runSpindexer(0),
shooter.runTower(RotationsPerSecond.of(0))));
shooter.prepareToShoot(RotationsPerSecond.of(0), RotationsPerSecond.of(0)),
hopper.runSpindexer(RotationsPerSecond.of(0))));

// Intake Rollers 11 Motor: 9 Intake
// Align
// controller.a().onTrue();

controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5))));
controller
.a()
.onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake));
// controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5))));
// controller
// .a()
// .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake));

// Spindexer 1:1
// controller.x().whileTrue(hopper.runSpindexer(18));
Expand All @@ -399,7 +406,7 @@ private void configureButtonBindings() {
// controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0)));

// controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude()));
controller.povLeft().onTrue(shooter.calibrateHood());
// controller.povLeft().onTrue(shooter.calibrateHood());
// controller.povLeft().onTrue(shooter.setHoodAngle(10));
// controller.povDown().onTrue(shooter.setHoodAngle(15));
// controller.povRight().onTrue(shooter.setHoodAngle(20));
Expand Down
17 changes: 7 additions & 10 deletions src/main/java/frc/robot/subsystems/Hopper.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,16 +30,13 @@ public void setGoal(double position) {
PIDSlot.SLOT_0);
}

// Velocity of Rollers
public void setVelocity(double velocity) {
AngularVelocity angVelo = RotationsPerSecond.of(velocity);

_io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0);
targetVelocity = angVelo;
}

public Command runSpindexer(double velocity) {
return Commands.runOnce(() -> setVelocity(velocity), this);
public Command runSpindexer(AngularVelocity velocity) {
return Commands.runOnce(
() -> {
_io.runVelocity(velocity, HopperConstants.ACCELERATION, PIDSlot.SLOT_0);
targetVelocity = velocity;
},
this);
}

@Override
Expand Down
18 changes: 7 additions & 11 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,13 +76,12 @@ public void stop() {
}

public Command runRollers(AngularVelocity velocity) {
return Commands.run(() -> setVelocity(velocity), this);
return Commands.run(() -> setVelocity(velocity));
}

public Command intake() {
return Commands.sequence(
Commands.run(
() -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))),
return Commands.parallel(
runRollers(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED)),
setPivotAngle(IntakePivotConstants.PICKUP_ANGLE));
}

Expand All @@ -98,17 +97,14 @@ public boolean canIntake() {
}

public Command stowAndStopRollers() {
return Commands.sequence(
Commands.run(
() -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))),
setStowAngle(IntakePivotConstants.STOW_ANGLE));
return Commands.parallel(runRollers(RotationsPerSecond.of(0.0)), setStowAngle());
}

private Command setStowAngle(Angle stowAngle) {
private Command setStowAngle() {
return this.runOnce(
() ->
_pivotIO.runPosition(
stowAngle,
IntakePivotConstants.STOW_ANGLE,
IntakePivotConstants.CRUISE_VELOCITY,
IntakePivotConstants.ACCELERATION,
IntakePivotConstants.JERK,
Expand All @@ -132,7 +128,7 @@ public void periodic() {
Logger.recordOutput("Intake/TargetSpeed", targetSpeed);

_pivotIO.periodic();
// Logger.recordOutput("Intake/TargetPivot", null);
Logger.recordOutput("Intake/TargetPivot", _pivotIO.getPosition().in(Degrees));
Logger.recordOutput(
"3DField/1_Intake",
new Pose3d(
Expand Down
15 changes: 5 additions & 10 deletions src/main/java/frc/robot/subsystems/Shooter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,17 +181,12 @@ public Command calibrateHood() {
// _hood.setEncoderPosition(Angle.ofBaseUnits(0, Degrees))));
// }

public Command shoot(AngularVelocity velocity) {
public Command prepareToShoot(AngularVelocity flywheelVelocity, AngularVelocity towerVelocity) {
// Prepare targets
return Commands.sequence(
// Set and wait in parallel for both hood and flywheel
Commands.parallel(
Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity),
Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)),
// feed once ready
Commands.runOnce(() -> runFeeder(FeederConstants.FEED_SPEED)),
// stop flywheel when finished
Commands.runOnce(() -> setFlywheelVelocity(RotationsPerSecond.of(0.0))));
return Commands.parallel(
Commands.run(() -> setFlywheelVelocity(flywheelVelocity)).until(this::flyAtVelocity),
Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle),
runTower(towerVelocity));
}

public Command runFlywheel(AngularVelocity velocity) {
Expand Down
Loading