Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Improve PIDs and such

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-21 12:54:16 -08:00
parent 9c7159ba3b
commit eec454e99a
11 changed files with 95 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/paths/TurnNinety.path
+8 -8
View File
@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 2.0, "x": 2.42775,
"y": 7.0 "y": 4.0055
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.0, "x": 3.4277499999999996,
"y": 7.0 "y": 4.0055
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 2.0, "x": 2.42775,
"y": 7.0 "y": 4.0055
}, },
"prevControl": { "prevControl": {
"x": 1.0, "x": 1.42775,
"y": 7.0 "y": 4.0055
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
src/main/java/frc4388/robot/RobotContainer.java
+12
View File
@@ -119,6 +119,9 @@ public class RobotContainer {
DeferredBlock.addBlock(() -> { DeferredBlock.addBlock(() -> {
TimesNegativeOne.update(); TimesNegativeOne.update();
FieldPositions.update(); FieldPositions.update();
m_robotIntake.io.updateGains();
m_robotShooter.io.updateGains();
}, true); }, true);
@@ -269,8 +272,17 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterReady(); m_robotShooter.setShooterReady();
m_robotIntake.setMode(IntakeMode.Idle);
})); }));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotShooter.setShooterShoot();
})).onFalse(new InstantCommand(() -> {
m_robotShooter.setShooterNOTShoot();
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> { .onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracting); m_robotIntake.setMode(IntakeMode.Retracting);
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = ""; public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified"; public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 71; public static final int GIT_REVISION = 72;
public static final String GIT_SHA = "32ede81ffaf43d4e730ff646db98627385e80ba1"; public static final String GIT_SHA = "9c7159ba3b2fd669f9781f1315cb481b7a1b8d57";
public static final String GIT_DATE = "2026-02-20 16:24:05 MST"; public static final String GIT_DATE = "2026-02-20 21:59:06 MST";
public static final String GIT_BRANCH = "operator-controls"; public static final String GIT_BRANCH = "operator-controls";
public static final String BUILD_DATE = "2026-02-20 21:42:37 MST"; public static final String BUILD_DATE = "2026-02-21 13:54:02 MST";
public static final long BUILD_UNIX_TIME = 1771648957538L; public static final long BUILD_UNIX_TIME = 1771707242280L;
public static final int DIRTY = 1; public static final int DIRTY = 1;
private BuildConstants(){} private BuildConstants(){}
src/main/java/frc4388/robot/constants/Constants.java
+3 -3
View File
@@ -78,10 +78,10 @@ public final class Constants {
public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
public static final Transform3d LEFT_CAMERA_POS = new Transform3d( public static final Transform3d LEFT_CAMERA_POS = new Transform3d(
new Translation3d(Units.inchesToMeters(-13-9.134-1.5), -Units.inchesToMeters(10.75), Units.inchesToMeters(9.5) new Translation3d(Units.inchesToMeters(-13-9.134), -Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)
), new Rotation3d(0,25.*(Math.PI/180.),Math.PI)); ), new Rotation3d(0,25.*(Math.PI/180.),Math.PI));
public static final Transform3d RIGHT_CAMERA_POS = new Transform3d( public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(
new Translation3d(Units.inchesToMeters(-13-9.134-1.5), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)), new Translation3d(Units.inchesToMeters(-13-9.134), Units.inchesToMeters(10.75), Units.inchesToMeters(9.5)),
new Rotation3d(0,25.*(Math.PI/180.),Math.PI) new Rotation3d(0,25.*(Math.PI/180.),Math.PI)
); );
@@ -99,7 +99,7 @@ public final class Constants {
public static final class LEDConstants { public static final class LEDConstants {
public static final int LED_SPARK_ID = 9; public static final int LED_SPARK_ID = 9;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED_ORANGE; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_TWINKLES;
// LED color for when the intake is out // LED color for when the intake is out
public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED; public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED;
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -45,7 +45,7 @@ public class LED extends SubsystemBase implements Queryable {
// Don't stall the main thread every time the setMode function is called // Don't stall the main thread every time the setMode function is called
if(this.mode != pattern) { if(this.mode != pattern) {
this.mode = pattern; this.mode = pattern;
setTo5V(); update();
} }
} }
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+22 -22
View File
@@ -31,7 +31,7 @@ public class Intake extends SubsystemBase {
Idle Idle
} }
private IntakeMode mode = IntakeMode.Retracted; private IntakeMode mode = IntakeMode.Idle;
public void setMode(IntakeMode mode) { public void setMode(IntakeMode mode) {
this.mode = mode; this.mode = mode;
@@ -67,27 +67,27 @@ public class Intake extends SubsystemBase {
io.updateInputs(state); io.updateInputs(state);
// switch (mode) { switch (mode) {
// case Extended: case Extended:
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
// io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
// break; break;
// case Retracted: case Retracted:
// io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
// io.setRollerOutput(state, 0); io.setRollerOutput(state, 0);
// break; break;
// case Extending: case Extending:
// io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
// io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
// break; break;
// case Retracting: case Retracting:
// io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get());
// io.setRollerOutput(state, 0); io.setRollerOutput(state, 0);
// break; break;
// case Idle: case Idle:
// io.stopArm(); io.stopArm();
// break; break;
// } }
// if (state.retractedLimit){ // if (state.retractedLimit){
// this.mode = IntakeMode.Retracted; // this.mode = IntakeMode.Retracted;
// } // }
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+3 -3
View File
@@ -33,7 +33,7 @@ public class IntakeConstants {
// public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
// public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0); public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.1);
public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.3); public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.3);
public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2);
public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4);
@@ -45,13 +45,13 @@ public class IntakeConstants {
public static final Slot0Configs ARM_PID = new Slot0Configs() public static final Slot0Configs ARM_PID = new Slot0Configs()
.withKP(0.2) .withKP(0.1)
.withKI(0.0) .withKI(0.0)
.withKD(0.0); .withKD(0.0);
public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.1); public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.05);
public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0); public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+1
View File
@@ -83,6 +83,7 @@ public class IntakeReal implements IntakeIO {
.withPosition(motorAngle) .withPosition(motorAngle)
.withLimitReverseMotion(!m_armLimitSwitch.get()) .withLimitReverseMotion(!m_armLimitSwitch.get())
); );
} }
@Override @Override
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+23 -6
View File
@@ -61,6 +61,7 @@ public class Shooter extends SubsystemBase {
} }
private ShooterMode mode = ShooterMode.NotReady; private ShooterMode mode = ShooterMode.NotReady;
private boolean shooterButtonReady = false;
public void setShooterReady() { public void setShooterReady() {
if(this.mode == ShooterMode.NotReady) { if(this.mode == ShooterMode.NotReady) {
@@ -72,6 +73,15 @@ public class Shooter extends SubsystemBase {
this.mode = ShooterMode.NotReady; this.mode = ShooterMode.NotReady;
} }
public void setShooterShoot() {
shooterButtonReady = true;
}
public void setShooterNOTShoot() {
shooterButtonReady = false;
}
@AutoLogOutput @AutoLogOutput
public ShooterMode getMode() { public ShooterMode getMode() {
return mode; return mode;
@@ -100,11 +110,11 @@ public class Shooter extends SubsystemBase {
if(this.mode != ShooterMode.NotReady) { if(this.mode != ShooterMode.NotReady) {
// TODO: get if the robot is within the angle of the hub // TODO: get if the robot is within the angle of the hub
boolean driverError = false; boolean driverError =
// XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
// AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
// distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() |
// distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2; double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2;
@@ -151,13 +161,20 @@ public class Shooter extends SubsystemBase {
} }
switch (mode) { switch (mode) {
case Shooting: case Shooting:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); if(shooterButtonReady) {
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
} else {
io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
}
break; break;
case Ready: case Ready:
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
break; break;
case NotReady: case NotReady:
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+14 -9
View File
@@ -18,7 +18,7 @@ public class ShooterConstants {
public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
public static final double INDEXER_GEAR_RATIO = 1.; public static final double INDEXER_GEAR_RATIO = 1.;
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40);
public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
@@ -28,23 +28,26 @@ public class ShooterConstants {
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
// Shoot mode tolerances // Shoot mode tolerances
public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3); public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3);
public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 5); public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3);
// //
public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
// Model derived from points // Model derived from points
// double speed =
// 1.11576*hubDistMeters*hubDistMeters +
// 0.318464*hubDistMeters +
// 30.6293;
double speed = double speed =
1.11576*hubDistMeters*hubDistMeters + 5.6939*hubDistMeters +
0.318464*hubDistMeters + 22.76545;
30.6293;
double max = SHOOTER_MAX_VELOCITY.get(); double max = SHOOTER_MAX_VELOCITY.get();
@@ -55,6 +58,8 @@ public class ShooterConstants {
speed = -max; speed = -max;
} }
// double speed = SHOOTER_MAX_VELOCITY.get();
return RotationsPerSecond.of(-speed); return RotationsPerSecond.of(-speed);
} }
@@ -62,7 +67,7 @@ public class ShooterConstants {
public static Slot0Configs SHOOTER_PID = new Slot0Configs() public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0) .withKV(0.0)
.withKP(0.08) .withKP(0.08)
.withKI(0.1) .withKI(0.15)
.withKD(0.0); .withKD(0.0);
@@ -71,7 +76,7 @@ public class ShooterConstants {
public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+2 -2
View File
@@ -50,7 +50,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
public double lastOdomSpeed; public double lastOdomSpeed;
public Pose2d initalPose2d = new Pose2d(); public Pose2d initalPose2d = new Pose2d(); // Mira aqui
public double rotTarget = 0.0; public double rotTarget = 0.0;
@@ -330,7 +330,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90));
heading = heading.rotateBy(Rotation2d.fromDegrees(90)); heading = heading.rotateBy(Rotation2d.fromDegrees(-90));
rotTarget = heading.getDegrees(); rotTarget = heading.getDegrees();
driveFieldAngle(leftStick, heading); driveFieldAngle(leftStick, heading);