diff --git a/README.md b/README.md index ce7ffa33..d82f0379 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # 3061-lib
-Huskie Robotics, FRC Team 3061's, starter project and library focused on a swerve-based drivetrain. Supports Swerve Drive Specialties (SDS) MK4/MK4i/MK4n swerve modules using devices from Cross the Road Electronics (CTRE): 2 Falcon 500 / Kraken motors and a CTRE CANCoder, a CTRE Pigeon Gyro along with REV Robotics power distribution hub and pneumatics hub. While, due to the hardware abstraction layer, this code can be adapted to other motor controllers, encoders, and gyros as well as different swerve module designs, only CTRE devices and SDS swerve modules are supported "out of the box." +Huskie Robotics, FRC Team 3061's, starter project and library focused on a swerve-based drivetrain. Supports Swerve Drive Specialties (SDS) MK4/MK4i/MK4n/MK5n swerve modules using devices from Cross the Road Electronics (CTRE): 2 Falcon 500 / Kraken motors and a CTRE CANCoder, a CTRE Pigeon Gyro along with REV Robotics power distribution hub and pneumatics hub. While, due to the hardware abstraction layer, this code can be adapted to other motor controllers, encoders, and gyros as well as different swerve module designs, only CTRE devices and SDS swerve modules are supported "out of the box." **Is 3061-lib a Good Fit for Your Team?** ---- diff --git a/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrain.java b/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrain.java index 148677e9..9a074971 100644 --- a/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrain.java +++ b/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrain.java @@ -91,7 +91,7 @@ public class SwerveDrivetrain extends SubsystemBase implements CustomPoseEstimat private final PIDController autoThetaController = new PIDController(autoTurnKp.get(), autoTurnKi.get(), autoTurnKd.get()); - private boolean isFieldRelative = true; + private boolean isFieldRelative = false; private boolean isTranslationSlowMode = false; private boolean isRotationSlowMode = false; diff --git a/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIO.java b/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIO.java index c8bcf52f..d4d814d5 100644 --- a/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIO.java +++ b/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIO.java @@ -27,6 +27,10 @@ public static class SwerveIOInputs { public double steerAbsolutePositionDeg = 0.0; + public double steerPositionRot = 0.0; + public double steerRefPositionRot = 0.0; + public double steerSensorPositionRot = 0.0; + public boolean steerEnabled = false; public double steerStatorCurrentAmps = 0.0; public double steerSupplyCurrentAmps = 0.0; diff --git a/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIOCTRE.java b/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIOCTRE.java index 9e69c7d2..aa86db37 100644 --- a/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIOCTRE.java +++ b/src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainIOCTRE.java @@ -598,6 +598,9 @@ private void updateSwerveModuleInputs( inputs.steerAbsolutePositionDeg = module.getEncoder().getAbsolutePosition().getValue().in(Degrees); + inputs.steerPositionRot = module.getSteerMotor().getPosition().getValueAsDouble(); + inputs.steerRefPositionRot = module.getSteerMotor().getClosedLoopReference().getValueAsDouble(); + inputs.steerSensorPositionRot = module.getEncoder().getPosition().getValueAsDouble(); inputs.steerEnabled = module.getSteerMotor().getDeviceEnable().getValue() == DeviceEnableValue.Enabled; inputs.steerStatorCurrentAmps = module.getSteerMotor().getStatorCurrent().getValue().in(Amps); diff --git a/src/main/java/frc/lib/team3061/swerve_drivetrain/swerve/SwerveConstants.java b/src/main/java/frc/lib/team3061/swerve_drivetrain/swerve/SwerveConstants.java index 98fa6ddf..d4e2371d 100644 --- a/src/main/java/frc/lib/team3061/swerve_drivetrain/swerve/SwerveConstants.java +++ b/src/main/java/frc/lib/team3061/swerve_drivetrain/swerve/SwerveConstants.java @@ -183,6 +183,102 @@ public boolean isCanCoderInverted() { } }; + public static final SwerveConstants MK5N_R1_CONSTANTS = + new SwerveConstants() { + @Override + public double getDriveGearRatio() { + return MK5N_R1_DRIVE_GEAR_RATIO; + } + + @Override + public boolean isDriveMotorInverted() { + return MK5N_DRIVE_MOTOR_INVERTED; + } + + @Override + public double getAngleGearRatio() { + return MK5N_ANGLE_GEAR_RATIO; + } + + @Override + public boolean isAngleMotorInverted() { + return MK5N_ANGLE_MOTOR_INVERTED; + } + + @Override + public boolean isCanCoderInverted() { + return MK5N_CAN_CODER_INVERTED; + } + }; + + public static final SwerveConstants MK5N_R2_CONSTANTS = + new SwerveConstants() { + @Override + public double getDriveGearRatio() { + return MK5N_R2_DRIVE_GEAR_RATIO; + } + + @Override + public boolean isDriveMotorInverted() { + return MK5N_DRIVE_MOTOR_INVERTED; + } + + @Override + public double getAngleGearRatio() { + return MK5N_ANGLE_GEAR_RATIO; + } + + @Override + public boolean isAngleMotorInverted() { + return MK5N_ANGLE_MOTOR_INVERTED; + } + + @Override + public boolean isCanCoderInverted() { + return MK5N_CAN_CODER_INVERTED; + } + }; + + public static final SwerveConstants MK5N_R3_CONSTANTS = + new SwerveConstants() { + @Override + public double getDriveGearRatio() { + return MK5N_R3_DRIVE_GEAR_RATIO; + } + + @Override + public boolean isDriveMotorInverted() { + return MK5N_DRIVE_MOTOR_INVERTED; + } + + @Override + public double getAngleGearRatio() { + return MK5N_ANGLE_GEAR_RATIO; + } + + @Override + public boolean isAngleMotorInverted() { + return MK5N_ANGLE_MOTOR_INVERTED; + } + + @Override + public boolean isCanCoderInverted() { + return MK5N_CAN_CODER_INVERTED; + } + }; + + /* MK5n */ + private static final double MK5N_R1_DRIVE_GEAR_RATIO = + 1.0 / ((12.0 / 54.0) * (32.0 / 25.0) * (15.0 / 30.0)); + private static final double MK5N_R2_DRIVE_GEAR_RATIO = + 1.0 / ((14.0 / 54.0) * (32.0 / 25.0) * (15.0 / 30.0)); + private static final double MK5N_R3_DRIVE_GEAR_RATIO = + 1.0 / ((16.0 / 54.0) * (32.0 / 25.0) * (15.0 / 30.0)); + private static final boolean MK5N_DRIVE_MOTOR_INVERTED = false; + private static final double MK5N_ANGLE_GEAR_RATIO = 287.0 / 11.0; + private static final boolean MK5N_ANGLE_MOTOR_INVERTED = false; + private static final boolean MK5N_CAN_CODER_INVERTED = false; + /* MK4n L3 Plus */ private static final double MK4N_L3_PLUS_DRIVE_GEAR_RATIO = 1 / ((16.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0)); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c811dc7c..564ceeee 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,10 +26,10 @@ public final class Constants { // set to true in order to change all Tunable values via AdvantageScope - public static final boolean TUNING_MODE = false; + public static final boolean TUNING_MODE = true; public static final boolean DEMO_MODE = false; - private static final RobotType ROBOT = RobotType.ROBOT_COMPETITION; + private static final RobotType ROBOT = RobotType.ROBOT_PRACTICE; private static final Alert invalidRobotAlert = new Alert("Invalid robot selected, using competition robot as default.", AlertType.kError); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9df4b26a..e0c9b300 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -180,10 +180,10 @@ private void createRobotConfig() { case ROBOT_DEFAULT: config = new DefaultRobotConfig(); break; - case ROBOT_PRACTICE: + case ROBOT_PRACTICE, ROBOT_SIMBOT: config = new NewPracticeRobotConfig(); break; - case ROBOT_COMPETITION, ROBOT_SIMBOT: + case ROBOT_COMPETITION: config = new CalypsoRobotConfig(); break; case ROBOT_PRACTICE_BOARD: diff --git a/src/main/java/frc/robot/configs/NewPracticeRobotConfig.java b/src/main/java/frc/robot/configs/NewPracticeRobotConfig.java index 1fd6f0e7..8c990374 100644 --- a/src/main/java/frc/robot/configs/NewPracticeRobotConfig.java +++ b/src/main/java/frc/robot/configs/NewPracticeRobotConfig.java @@ -18,27 +18,25 @@ import frc.lib.team3061.swerve_drivetrain.swerve.SwerveConstants; public class NewPracticeRobotConfig extends RobotConfig { - // 2 mk4n's on the front, 2 mk4is on the back - - private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 43; - private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 44; + private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 37; + private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 61; private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14; - private static final Angle FRONT_LEFT_MODULE_STEER_OFFSET = Rotations.of(0.271484); + private static final Angle FRONT_LEFT_MODULE_STEER_OFFSET = Rotations.of(0.382324 + 0.5); - private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 41; - private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 42; - private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 8; - private static final Angle FRONT_RIGHT_MODULE_STEER_OFFSET = Rotations.of(-0.487793 + 0.5); + private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 40; + private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 25; + private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17; + private static final Angle FRONT_RIGHT_MODULE_STEER_OFFSET = Rotations.of(0.471191 + 0.5); - private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 38; - private static final int BACK_LEFT_MODULE_STEER_MOTOR = 37; - private static final int BACK_LEFT_MODULE_STEER_ENCODER = 17; - private static final Angle BACK_LEFT_MODULE_STEER_OFFSET = Rotations.of(-0.280029); + private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 39; + private static final int BACK_LEFT_MODULE_STEER_MOTOR = 60; + private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8; + private static final Angle BACK_LEFT_MODULE_STEER_OFFSET = Rotations.of(0.121094 + 0.5); - private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 39; - private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 40; + private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 38; + private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 59; private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11; - private static final Angle BACK_RIGHT_MODULE_STEER_OFFSET = Rotations.of(-0.105957 + 0.5); + private static final Angle BACK_RIGHT_MODULE_STEER_OFFSET = Rotations.of(-0.342041 + 0.5); private static final int GYRO_ID = 3; @@ -53,9 +51,9 @@ public class NewPracticeRobotConfig extends RobotConfig { private static final double COUPLE_RATIO = 3.125; /* Angle Motor PID Values */ - private static final double ANGLE_KP = 100.0; + private static final double ANGLE_KP = 10.0; private static final double ANGLE_KI = 0.0; - private static final double ANGLE_KD = 0.5; + private static final double ANGLE_KD = 0.1; // values from sysid routines private static final double ANGLE_KS = 0.28516; @@ -234,17 +232,7 @@ public double getDriveKA() { @Override public SwerveConstants getSwerveConstants() { - return SwerveConstants.MK4N_L3_PLUS_CONSTANTS; - } - - @Override - public SwerveConstants getFrontSwerveConstants() { - return SwerveConstants.MK4N_L3_PLUS_CONSTANTS; - } - - @Override - public SwerveConstants getBackSwerveConstants() { - return SwerveConstants.MK4I_L3_PLUS_CONSTANTS; + return SwerveConstants.MK5N_R2_CONSTANTS; } @Override