58 lines
2.6 KiB
Java
58 lines
2.6 KiB
Java
package frc.robot.constants;
|
|
|
|
import edu.wpi.first.math.Matrix;
|
|
import edu.wpi.first.math.VecBuilder;
|
|
import edu.wpi.first.math.geometry.Translation2d;
|
|
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
|
import edu.wpi.first.math.numbers.N1;
|
|
import edu.wpi.first.math.numbers.N3;
|
|
import edu.wpi.first.math.util.Units;
|
|
|
|
public class DrivetrainConstants {
|
|
public static final double kMaxSpeedMetersPerSecond = 4.663;
|
|
public static final double kMaxAngularSpeed = 2 * Math.PI;
|
|
|
|
public static final double kTrackWidth = Units.inchesToMeters(23.75);
|
|
public static final double kWheelBase = Units.inchesToMeters(18.75);
|
|
|
|
public static final double kFrontLeftMagEncoderOffset = 2.965;
|
|
public static final double kFrontRightMagEncoderOffset = 1.120;
|
|
public static final double kRearLeftMagEncoderOffset = 3.761;
|
|
public static final double kRearRightMagEncoderOffset = 2.573;
|
|
|
|
public static final int kFrontLeftDrivingCANID = 4;
|
|
public static final int kFrontRightDrivingCANID = 3;
|
|
public static final int kRearLeftDrivingCANID = 1;
|
|
public static final int kRearRightDrivingCANID = 2;
|
|
|
|
public static final int kFrontLeftTurningCANID = 7; // 8
|
|
public static final int kFrontRightTurningCANID = 21; //9
|
|
public static final int kRearLeftTurningCANID = 6; //7
|
|
public static final int kRearRightTurningCANID = 8; //6
|
|
|
|
public static final int kFrontLeftAnalogInPort = 3;
|
|
public static final int kFrontRightAnalogInPort = 2;
|
|
public static final int kRearLeftAnalogInPort = 0;
|
|
public static final int kRearRightAnalogInPort = 1;
|
|
|
|
public static final boolean kGyroReversed = true;
|
|
|
|
// TODO Hold over from 2025, adjust?
|
|
public static final double kHeadingP = .1;
|
|
public static final double kXTranslationP = .5;
|
|
public static final double kYTranslationP = .5;
|
|
|
|
// TODO How much do we trust gyro and encoders vs vision estimates.
|
|
// NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
|
|
public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
|
|
public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.9, 0.9, 0.9);
|
|
|
|
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
|
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
|
|
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
|
|
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
|
|
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
|
|
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)
|
|
);
|
|
}
|