mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 08:48:04 -06:00
A lot of constants changes and copy paste example from the docs
This commit is contained in:
C4llSiqn
@@ -7,15 +7,25 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ParentConfiguration;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
import frc4388.utility.CanDevice;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
@@ -49,13 +59,48 @@ public final class Constants {
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
// public static List<CanDevice> CAN_DEVICES = new ArrayList<>();
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
public static final double HEIGHT = 18.5;
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
public static final class DefaultSwerveRotOffsets {
|
||||
public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
private static final class ModuleSpecificConstants {
|
||||
//Front Left
|
||||
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.0);
|
||||
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Front Right
|
||||
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.0);
|
||||
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = false;
|
||||
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Left
|
||||
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.0);
|
||||
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_LEFT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
|
||||
//Back Right
|
||||
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.0);
|
||||
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = false;
|
||||
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
|
||||
private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
|
||||
private static final Distance BACK_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
|
||||
}
|
||||
|
||||
private static final class DrivetrainSpecificConstants {
|
||||
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
@@ -75,7 +120,7 @@ public final class Constants {
|
||||
public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9);
|
||||
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
|
||||
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 4);
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14);
|
||||
public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50);
|
||||
}
|
||||
|
||||
@@ -127,23 +172,158 @@ public final class Constants {
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
|
||||
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
public static final double HEIGHT = 18.5;
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
|
||||
public static final SwerveModuleConstants<ParentConfiguration, ParentConfiguration, ParentConfiguration> FRONT_LEFT = new SwerveModuleConstants<>()
|
||||
.withLocationX(HALF_WIDTH)
|
||||
.withLocationX(HALF_HEIGHT)
|
||||
.withDriveMotorId(2)
|
||||
.withDriveMotorInverted(false)
|
||||
.withSteerMotorId(3)
|
||||
.withSteerMotorInverted(false)
|
||||
.withEncoderId(10)
|
||||
.withEncoderInverted(false)
|
||||
.withEncoderOffset(0.0);
|
||||
|
||||
// No mans land
|
||||
// Beware, there be dragons.
|
||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||
private static final Slot0Configs steerGains = new Slot0Configs()
|
||||
.withKP(100).withKI(0).withKD(0.5)
|
||||
.withKS(0.1).withKV(1.91).withKA(0)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
// When using closed-loop control, the drive motor uses the control
|
||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||
private static final Slot0Configs driveGains = new Slot0Configs()
|
||||
.withKP(0.1).withKI(0).withKD(0)
|
||||
.withKS(0).withKV(0.124);
|
||||
|
||||
// The closed-loop output type to use for the steer motors;
|
||||
// This affects the PID/FF gains for the steer motors
|
||||
private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
||||
// The closed-loop output type to use for the drive motors;
|
||||
// This affects the PID/FF gains for the drive motors
|
||||
private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
|
||||
|
||||
// The type of motor used for the drive motor
|
||||
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
|
||||
// The type of motor used for the drive motor
|
||||
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
|
||||
|
||||
// The remote sensor feedback type to use for the steer motors;
|
||||
// When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder
|
||||
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder;
|
||||
|
||||
// The stator current at which the wheels start to slip;
|
||||
// This needs to be tuned to your individual robot
|
||||
private static final double kSlipCurrent = 120; // TODO: tune???
|
||||
|
||||
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
|
||||
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
|
||||
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
// Swerve azimuth does not require much torque output, so we can set a relatively low
|
||||
// stator current limit to help avoid brownouts without impacting performance.
|
||||
.withStatorCurrentLimit(100) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
);
|
||||
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
// Swerve azimuth does not require much torque output, so we can set a relatively low
|
||||
// stator current limit to help avoid brownouts without impacting performance.
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
);
|
||||
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
|
||||
|
||||
// Theoretical free speed (m/s) at 12 V applied output;
|
||||
// This needs to be tuned to your individual robot
|
||||
public static final double kSpeedAt12Volts = 4.69;// todo: tune???
|
||||
|
||||
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
|
||||
// This may need to be tuned to your individual robot
|
||||
private static final double kCoupleRatio = 3.8181818181818183; //todo: find
|
||||
|
||||
private static final double kDriveGearRatio = 6.12;
|
||||
private static final double kSteerGearRatio = (150/7);
|
||||
private static final Distance kWheelRadius = Inches.of(2); // 2 inches in meters
|
||||
|
||||
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
|
||||
.withPigeon2Id(IDs.DRIVE_PIGEON.id);
|
||||
|
||||
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
|
||||
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
|
||||
.withDriveMotorGearRatio(kDriveGearRatio)
|
||||
.withSteerMotorGearRatio(kSteerGearRatio)
|
||||
.withCouplingGearRatio(kCoupleRatio)
|
||||
.withWheelRadius(kWheelRadius)
|
||||
.withSteerMotorGains(steerGains)
|
||||
.withDriveMotorGains(driveGains)
|
||||
.withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
|
||||
.withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
|
||||
.withSlipCurrent(kSlipCurrent)
|
||||
.withSpeedAt12Volts(kSpeedAt12Volts)
|
||||
.withDriveMotorType(kDriveMotorType)
|
||||
.withSteerMotorType(kSteerMotorType)
|
||||
.withFeedbackSource(kSteerFeedbackType)
|
||||
.withDriveMotorInitialConfigs(driveInitialConfigs)
|
||||
.withSteerMotorInitialConfigs(steerInitialConfigs)
|
||||
.withEncoderInitialConfigs(encoderInitialConfigs);
|
||||
|
||||
// Front Left
|
||||
private static final int kFrontLeftDriveMotorId = 3;
|
||||
private static final int kFrontLeftSteerMotorId = 2;
|
||||
private static final int kFrontLeftEncoderId = 10;
|
||||
private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
|
||||
private static final boolean kFrontLeftSteerMotorInverted = true;
|
||||
private static final boolean kFrontLeftEncoderInverted = false;
|
||||
|
||||
private static final Distance kFrontLeftXPos = Inches.of(10);
|
||||
private static final Distance kFrontLeftYPos = Inches.of(10);
|
||||
|
||||
// Front Right
|
||||
private static final int kFrontRightDriveMotorId = 1;
|
||||
private static final int kFrontRightSteerMotorId = 0;
|
||||
private static final int kFrontRightEncoderId = 0;
|
||||
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
|
||||
private static final boolean kFrontRightSteerMotorInverted = true;
|
||||
private static final boolean kFrontRightEncoderInverted = false;
|
||||
|
||||
private static final Distance kFrontRightXPos = Inches.of(10);
|
||||
private static final Distance kFrontRightYPos = Inches.of(-10);
|
||||
|
||||
// Back Left
|
||||
private static final int kBackLeftDriveMotorId = 7;
|
||||
private static final int kBackLeftSteerMotorId = 6;
|
||||
private static final int kBackLeftEncoderId = 3;
|
||||
private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
|
||||
private static final boolean kBackLeftSteerMotorInverted = true;
|
||||
private static final boolean kBackLeftEncoderInverted = false;
|
||||
|
||||
private static final Distance kBackLeftXPos = Inches.of(-10);
|
||||
private static final Distance kBackLeftYPos = Inches.of(10);
|
||||
|
||||
// Back Right
|
||||
private static final int kBackRightDriveMotorId = 5;
|
||||
private static final int kBackRightSteerMotorId = 4;
|
||||
private static final int kBackRightEncoderId = 2;
|
||||
private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
|
||||
private static final boolean kBackRightSteerMotorInverted = true;
|
||||
private static final boolean kBackRightEncoderInverted = false;
|
||||
|
||||
private static final Distance kBackRightXPos = Inches.of(-10);
|
||||
private static final Distance kBackRightYPos = Inches.of(-10);
|
||||
|
||||
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, Rotations.of(SwerveRotOffsets.FRONT_LEFT_ROT_OFFSET),
|
||||
Inches.of(HALF_WIDTH), Inches.of(HALF_HEIGHT), false, false, false
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
|
||||
kFrontRightXPos, kFrontRightYPos, false, false, false
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
|
||||
ConstantCreator.createModuleConstants(
|
||||
kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
|
||||
kBackLeftXPos, kBackLeftYPos, false, false, false
|
||||
);
|
||||
public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
|
||||
Reference in New Issue
Block a user