Skip to content

wednesday tuning #69

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

Closed
rhit-collinkn wants to merge 8 commits into from
Closed

wednesday tuning #69

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
d6b46da
wednesday tuning
GhostR406 Mar 12, 2026
dc5f00f
spotless + tuning + switch swerve to torque control
GhostR406 Mar 12, 2026
f812c55
Tuning from 3/12/16
GhostR406 Mar 14, 2026
cbb30f7
Added Flywheel as Follower
shlap Mar 14, 2026
9e6b862
Tuned PID Shooter
shlap Mar 14, 2026
64d2a1e
Tuning
shlap Mar 14, 2026
c2d13f0
fixing hood constants
rhit-collinkn Mar 15, 2026
8605669
spotless
rhit-collinkn 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
147 changes: 76 additions & 71 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 @@ -19,7 +19,6 @@
import static edu.wpi.first.units.Units.KilogramSquareMeters;
import static edu.wpi.first.units.Units.Kilograms;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import static edu.wpi.first.units.Units.Rotations;
Expand All @@ -40,6 +39,7 @@
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.RGBWColor;
import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
Expand Down Expand Up @@ -67,6 +67,7 @@
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
import frc.lib.W8.io.motor.MotorIOTalonFX.TalonFXFollower;
import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics;
import frc.lib.W8.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics;
import frc.lib.W8.util.Device;
Expand Down Expand Up @@ -292,7 +293,7 @@ public final class ShooterConstants {
public static final RotaryMechCharacteristics CONSTANTS =
new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);

public static final double HARD_STOP_CURRENT_LIMIT = 2.5;
public static final double HARD_STOP_CURRENT_LIMIT = 3.0;
}

public class ShooterFlywheelConstants {
Expand All @@ -302,16 +303,19 @@ public class ShooterFlywheelConstants {
public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI);
public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second);

private static final double GEARING = (2.0 / 1.0);
private static final double GEARING = (1.0 / 1.0);

public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1);

public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0);

public static final TalonFXFollower FOLLOWER_1 =
new TalonFXFollower(Ports.LeftFlywheel, MotorAlignmentValue.Opposed);

// Velocity PID
private static Slot0Configs SLOT0CONFIG =
new Slot0Configs().withKP(18.0).withKI(0.0).withKD(0.01).withKV(0.05).withKS(8.0);
new Slot0Configs().withKP(3.0).withKI(0.15).withKD(0.0).withKV(0.1).withKS(8.5);

public static TalonFXConfiguration getFXConfig(boolean invert) {
TalonFXConfiguration config = new TalonFXConfiguration();
Expand Down Expand Up @@ -359,14 +363,14 @@ public class ShooterRotaryConstants {
public static final Velocity<AngularAccelerationUnit> JERK = ACCELERATION.per(Second);

private static final double ROTOR_TO_SENSOR = (1.0 / 1.0);
private static final double SENSOR_TO_MECHANISM = 1 / 50;
private static final double SENSOR_TO_MECHANISM = 1.0 / 50.0;

public static final Translation3d OFFSET = Translation3d.kZero;

public static final Angle MIN_ANGLE = Degrees.of(0.0);
public static final Angle MAX_ANGLE = Degrees.of(25);
public static final Angle STARTING_ANGLE = Degrees.of(0.0);
public static final Distance ARM_LENGTH = Meters.of(0.0);
public static final Distance ARM_LENGTH = Meters.of(0.1);

public static final RotaryMechCharacteristics CONSTANTS =
new RotaryMechCharacteristics(OFFSET, ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
Expand All @@ -381,7 +385,7 @@ public class ShooterRotaryConstants {

// Positional PID
private static Slot0Configs SLOT0CONFIG =
new Slot0Configs().withKP(250.0).withKI(0.0).withKD(0.0).withKS(10.0).withKV(0.0);
new Slot0Configs().withKP(600.0).withKI(0.0).withKD(0.0).withKS(10.0).withKV(0.0);
// Velocity PID
private static Slot1Configs SLOT1CONFIG =
new Slot1Configs().withKP(30.0).withKI(0.0).withKD(0.0).withKS(40.0).withKV(0.0);
Expand Down Expand Up @@ -423,7 +427,7 @@ public static TalonFXConfiguration getFXConfig() {
config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Degrees);

config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Degrees);

config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR;
Expand Down Expand Up @@ -486,7 +490,7 @@ public class IntakeFlywheelConstants {
public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second);
public static final Velocity<AngularAccelerationUnit> JERK = ACCELERATION.per(Second);

public static final double GEARING = (5.0 / 1.0);
public static final double GEARING = (22.0 / 18.0);
public static final Distance MIN_DISTANCE = Inches.of(0.0);
public static final Distance MAX_DISTANCE = Inches.of(10.0);
public static final Distance STARTING_DISTANCE = Inches.of(0.0);
Expand All @@ -513,53 +517,9 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim) {
return config;
}

public class VisionConstants {
// AprilTag layout
public static AprilTagFieldLayout aprilTagLayout =
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// Camera names, must match names configured on coprocessor
public static String camera0Name = "camera_0";
public static String camera1Name = "camera_1";

// Robot to camera transforms
// (Not used by Limelight, configure in web UI instead)
public static Transform3d robotToCamera0 =
new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
public static Transform3d robotToCamera1 =
new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));

// Basic filtering thresholds
public static double maxAmbiguity = 0.3;
public static double maxZError = 0.75;

// Standard deviation baselines, for 1 meter distance and 1 tag
// (Adjusted automatically based on distance and # of tags)
public static double linearStdDevBaseline = 0.02; // Meters
public static double angularStdDevBaseline = 0.06; // Radians

// Standard deviation multipliers for each camera
// (Adjust to trust some cameras more than others)
public static double[] cameraStdDevFactors =
new double[] {
1.0, // Camera 0
1.0 // Camera 1
};

/** Tags used for reef alignment */
public static List<Integer> alignmentTags =
Arrays.asList(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22);

public static VisionSystemSim getSystemSim() {
var system = new VisionSystemSim("main");
system.addAprilTags(aprilTagLayout);
return system;
}
}

// config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0);
private static Slot0Configs SLOT0CONFIG =
new Slot0Configs().withKP(30).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0);
new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0);

public static TalonFXConfiguration getFXConfig() {
TalonFXConfiguration config = new TalonFXConfiguration();
Expand Down Expand Up @@ -600,25 +560,68 @@ public static TalonFXConfiguration getFXConfig() {
}
}

public class VisionConstants {
// AprilTag layout
public static AprilTagFieldLayout aprilTagLayout =
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// Camera names, must match names configured on coprocessor
public static String camera0Name = "camera_0";
public static String camera1Name = "camera_1";

// Robot to camera transforms
// (Not used by Limelight, configure in web UI instead)
public static Transform3d robotToCamera0 =
new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
public static Transform3d robotToCamera1 =
new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));

// Basic filtering thresholds
public static double maxAmbiguity = 0.3;
public static double maxZError = 0.75;

// Standard deviation baselines, for 1 meter distance and 1 tag
// (Adjusted automatically based on distance and # of tags)
public static double linearStdDevBaseline = 0.02; // Meters
public static double angularStdDevBaseline = 0.06; // Radians

// Standard deviation multipliers for each camera
// (Adjust to trust some cameras more than others)
public static double[] cameraStdDevFactors =
new double[] {
1.0, // Camera 0
1.0 // Camera 1
};

/** Tags used for reef alignment */
public static List<Integer> alignmentTags =
Arrays.asList(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22);

public static VisionSystemSim getSystemSim() {
var system = new VisionSystemSim("main");
system.addAprilTags(aprilTagLayout);
return system;
}
}

public class IntakePivotConstants {
public static final String NAME = "Intake";

public static final Angle PICKUP_ANGLE = Degrees.of(120.0);
public static final Angle PICKUP_ANGLE = Degrees.of(106.0);
public static final Angle STOW_ANGLE = Degrees.of(0.0);

public static final Angle TOLERANCE = Degrees.of(1.0);

public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10);
public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(100);
public static final Velocity<AngularAccelerationUnit> JERK =
RadiansPerSecondPerSecond.per(Second).of(0.1);
public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(100);
public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(200);
public static final Velocity<AngularAccelerationUnit> JERK = ACCELERATION.per(Second);

private static final double ROTOR_TO_SENSOR = (50.0 / 1.0);
private static final double SENSOR_TO_MECHANISM = 1.0;
private static final double ROTOR_TO_SENSOR = (1.0 / 1.0);
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 STARTING_ANGLE = Radians.zero();
public static final Angle STARTING_ANGLE = Degrees.of(0.0);
public static final Distance ARM_LENGTH = Foot.one();

public static final RotaryMechCharacteristics CONSTANTS =
Expand All @@ -635,9 +638,10 @@ public class IntakePivotConstants {

// Positional PID
public static final Slot0Configs SLOT_0_CONFIG =
new Slot0Configs().withKP(.0).withKI(0.0).withKD(0).withKS(0.00).withKV(0.0);
new Slot0Configs().withKP(300.0).withKI(1000).withKD(80).withKS(8.0).withKV(0.0);

// ^^^ CHANGE
// .withKG(15)
// .withGravityType(GravityTypeValue.Arm_Cosine);

public static TalonFXConfiguration getFXConfig() {
TalonFXConfiguration config = new TalonFXConfiguration();
Expand All @@ -656,10 +660,10 @@ public static TalonFXConfiguration getFXConfig() {
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;

config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Rotations);

config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Rotations);

config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR;
Expand All @@ -685,7 +689,7 @@ public class FeederConstants {
public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI);
public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second);

private static final double GEARING = (15.0 / 30.0);
private static final double GEARING = (15.0 / 7.0);

public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1);

Expand Down Expand Up @@ -738,19 +742,19 @@ public class ClimberConstants {
public static final String MOTOR_NAME = "Climber motor";
public static final String NAME = "Climber";
public static final Distance TOLERANCE = Inches.of(0.1);
public static final double GEARING = 1;
public static final double GEARING = 140.8;
public static final Distance MIN_DISTANCE = Inches.of(0.0);
public static final Distance MAX_DISTANCE = Inches.of(10.0);
public static final Distance STARTING_DISTANCE = Inches.of(0.0);
public static final Distance DRUM_RADIUS = Inches.of(2.0);
public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS);

public static final AngularVelocity CRUISE_VELOCITY =
RadiansPerSecond.of(2 * Math.PI).times(8000.0);
RadiansPerSecond.of(2 * Math.PI).times(800.0 / GEARING);
public static final AngularVelocity LOWER_VELOCITY =
RadiansPerSecond.of(-2 * Math.PI).times(8000.0);
RadiansPerSecond.of(-2 * Math.PI).times(800.0 / GEARING);
public static final AngularVelocity CALIBRATE_VELOCITY =
RadiansPerSecond.of(-2 * Math.PI).times(1000.0);
RadiansPerSecond.of(-2 * Math.PI).times(100.0 / GEARING);

public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second);
public static final Velocity<AngularAccelerationUnit> JERK = ACCELERATION.per(Second);
Expand Down Expand Up @@ -797,6 +801,7 @@ public static TalonFXConfiguration getFXConfig() {
config.Feedback.SensorToMechanismRatio = GEARING;

config.Slot0 = new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0);
config.Slot1 = new Slot1Configs().withKP(1).withKI(0.0).withKD(0.0);

config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond);
config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond);
Expand Down
Loading
Loading