diff --git a/.project b/.project index dc4fc5e..f6c755e 100644 --- a/.project +++ b/.project @@ -1,6 +1,6 @@ - RoboLib + potential-octo-eureka-RoboLib Project RoboLib created by Buildship. diff --git a/README.md b/README.md index d8d0d21..7f27882 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ -# Potential Octo Eureka(POE) -The official library for Team 578 R3 - Red Raider Robotics +# RoboLib +The official library for Team 578 R3 - Fairport Robotics # Including in FRC Project To include this library in a Java FRC Robot project. diff --git a/gradle.properties b/gradle.properties new file mode 100644 index 0000000..fa55bd4 --- /dev/null +++ b/gradle.properties @@ -0,0 +1,5 @@ +wpiVersion = 2026.2.1 + +ctreVersion = 26.1.0 + +advantageKitVersion = 4.0.0 diff --git a/lib/.classpath b/lib/.classpath new file mode 100644 index 0000000..a99ea3b --- /dev/null +++ b/lib/.classpath @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/lib/.project b/lib/.project new file mode 100644 index 0000000..1ddb7d9 --- /dev/null +++ b/lib/.project @@ -0,0 +1,34 @@ + + + RoboLib-lib + Project lib created by Buildship. + + + + + org.eclipse.jdt.core.javabuilder + + + + + org.eclipse.buildship.core.gradleprojectbuilder + + + + + + org.eclipse.jdt.core.javanature + org.eclipse.buildship.core.gradleprojectnature + + + + 1743377880685 + + 30 + + org.eclipse.core.resources.regexFilterMatcher + node_modules|\.git|__CREATED_BY_JAVA_LANGUAGE_SERVER__ + + + + diff --git a/lib/build.gradle b/lib/build.gradle index c0e34dd..7fb47bc 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -25,14 +25,14 @@ repositories { mavenLocal() gradlePluginPortal() - maven {url "https://maven.photonvision.org/repository/snapshots"} - maven {url "https://maven.photonvision.org/repository/internal"} maven {url "https://maven.ctr-electronics.com/release/"} + maven {url "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"} } dependencies { // Use JUnit Jupiter for testing. - testImplementation 'org.junit.jupiter:junit-jupiter:5.8.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' // WPI Lib implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion" @@ -48,6 +48,9 @@ dependencies { // CTRE Phoenix 6 implementation "com.ctre.phoenix6:wpiapi-java:${ctreVersion}" + // AdvantageKit + implementation "org.littletonrobotics.akit:akit-java:${advantageKitVersion}" + } tasks.named('test') { diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java deleted file mode 100644 index 243bf5f..0000000 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/DriveSystems/SwerveDrive/SwerveDriveSubsystem.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class SwerveDriveSubsystem extends SubsystemBase{ - - public SwerveDriveSubsystem(){ - - } - -} diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java new file mode 100644 index 0000000..ec145fb --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java @@ -0,0 +1,344 @@ +package org.fairportrobotics.frc.robolib.drivesystems.swerve; + +import java.util.ArrayList; + +import com.ctre.phoenix6.CANBus; + +import edu.wpi.first.math.geometry.Translation2d; + +public class SwerveBuilder { + + private ArrayList modules = new ArrayList(); + private CANBus canBusObj; + + private double maxAngularVelRadiansSecond = 1.0; + private double maxLinearVelMetersSecond = Math.PI; + + private int pigeonId; + + public SwerveBuilder() { + + } + + /** + * Set the CAN ID of the Pigeon IMU + * @param pigeonId CAN Bus ID + * @return itself + */ + public SwerveBuilder withPigeonId(int pigeonId) { + this.pigeonId = pigeonId; + return this; + } + + /** + * Set the CAN Bus name + * @param canBusname Name of the CAN Bus + * @return itself + */ + public SwerveBuilder withCanbusName(String canBusname) { + this.canBusObj = new CANBus(canBusname); + return this; + } + + /** + * Add a SwerveModule + * + * Use the inner class SwerveModuleBuilder to construct a SwerveModule + * @param module + * @return itself + */ + public SwerveBuilder withSwerveModule(SwerveModule module) { + this.modules.add(module); + return this; + } + + /** + * Set the max linear velocity in meters per second + * + * @param maxVel Max linear velocity in meters per second + * @return itself + */ + public SwerveBuilder withMaxLinearVelocity(double maxVel) { + this.maxLinearVelMetersSecond = maxVel; + return this; + } + + /** + * Set the max angular velocity in radians per second + * + * @param maxVel Max angular velocity in radians per second + * @return itself + */ + public SwerveBuilder withMaxAngularVelocity(double maxVel) { + this.maxAngularVelRadiansSecond = maxVel; + return this; + } + + /** + * Build the configured instance of a SwerveDriveSystem + * @return instance of SwerveDriveSystem + */ + public SwerveDriveSystem build() { + return new SwerveDriveSystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond, + modules.toArray(size -> new SwerveModule[size])); + } + + public class SwerveModuleBuilder { + + private int driveMotorId; + private double driveKP; + private double driveKI; + private double driveKD; + private double driveKV; + private boolean driveInverted = false; + private int steerMotorId; + private double steerKP; + private double steerKI; + private double steerKD; + private double steerKS; + private double steerKV; + private double steerKA; + private int steerEncoderId; + private double steerOffset; + private Translation2d moduleLocation; + private double gearRatio; + private double wheelDiameterInMeters; + private String moduleName; + + public SwerveModuleBuilder() { + + } + + /** + * Set the drive motor CAN Bus ID + * @param driveMotorId ID of the drive motor + * @return itself + */ + public SwerveModuleBuilder withDriveMotorId(int driveMotorId) { + this.driveMotorId = driveMotorId; + return this; + } + + /** + * Set the drive motors closed loop proportional gain value. + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * + * @param driveKP Proportional gain in Volts + * @return itself + */ + public SwerveModuleBuilder withDriveKP(double driveKP) { + this.driveKP = driveKP; + return this; + } + + + /** + * Set the drive motors closed loop integral gain value. + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * + * @param driveKP Integral gain in Volts + * @return itself + */ + public SwerveModuleBuilder withDriveKI(double driveKI) { + this.driveKI = driveKI; + return this; + } + + + /** + * Set the drive motors closed loop derrivative gain value. + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * + * @param driveKP Derrivative gain in Volts + * @return itself + */ + public SwerveModuleBuilder withDriveKD(double driveKD) { + this.driveKD = driveKD; + return this; + } + + /** + * Set the drive motors closed loop feed forward velocity gain. + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * @param driveKV velocity gain in Volts + * @return itself + */ + public SwerveModuleBuilder withDriveKV(double driveKV) { + this.driveKV = driveKV; + return this; + } + + /** + * Set the drive motors direction to be inverted. + *

+ * When inverted positive values will be Counter Clockwise + * + * @return itself + */ + public SwerveModuleBuilder withDriveInverted(){ + this.driveInverted = true; + return this; + } + + /** + * Set the steer motors CAN Bus ID + * @param steerMotorId ID of steer motor + * @return itself + */ + public SwerveModuleBuilder withSteerMotorId(int steerMotorId) { + this.steerMotorId = steerMotorId; + return this; + } + + /** + * Set the steer motors closed loop proportional gain value + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * @param steerKP Proportional gain in Volts + * @return itself + */ + public SwerveModuleBuilder withSteerKP(double steerKP) { + this.steerKP = steerKP; + return this; + } + + /** + * Set the steer motors closed loop integral gain value + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * @param steerKP Integral gain in Volts + * @return itself + */ + public SwerveModuleBuilder withSteerKI(double steerKI) { + this.steerKI = steerKI; + return this; + } + + /** + * Set the steer motors closed loop derrivative gain value + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * @param steerKP Derrivative gain in Volts + * @return itself + */ + public SwerveModuleBuilder withSteerKD(double steerKD) { + this.steerKD = steerKD; + return this; + } + + /** + * Set the steer motors closed loop feed forward static gain. + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * @param steerKS Static gain in Volts + * @return itself + */ + public SwerveModuleBuilder withSteerKS(double steerKS) { + this.steerKS = steerKS; + return this; + } + + /** + * Set the steer motors closed loop feed forward velocity gain. + *

+ * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html + * @param steerKV velocity gain in Volts + * @return itself + */ + public SwerveModuleBuilder withSteerKV(double steerKV) { + this.steerKV = steerKV; + return this; + } + + /** + * Set the steer encoders CAN Bus ID + * @param steerEncoderId ID of steer encoder + * @return itself + */ + public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) { + this.steerEncoderId = steerEncoderId; + return this; + } + + /** + * Set the offset of the steer encoder. + *

+ * This value can be found from Pheonix tuner. + *

+ * Ensure the configured offset is 0.0 and then set this to + * be the Absolute Position when the wheel is properly aligned + * @param offsetRotations offset in the range of [-0.5, 0.5) + * @return itself + */ + public SwerveModuleBuilder withSteerOffset(double offsetRotations) { + this.steerOffset = offsetRotations; + return this; + } + + /** + * Set the swerve modules location relative to the center of the robot. + *

+ * Refer to https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html + * for how the cordinate system works. + *

+ * The location is in meters from the center of the robot. + * + * @param moduleLocation location of the swerve module + * @return + */ + public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) { + this.moduleLocation = moduleLocation; + return this; + } + + /** + * Set the gear ratio of the drive motor to the drive wheel. + *

+ * This ratio is the number of rotations the motor has to make for one(1) rotation + * of the drive wheel. + * @param gearRatio the gear ratio + * @return itself + */ + public SwerveModuleBuilder withGearRatio(double gearRatio) { + this.gearRatio = gearRatio; + return this; + } + + /** + * Set the diameter of the drive wheel in meters. + * @param wheelDiameterInMeters diameter of the drive wheel + * @return itself + */ + public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters) { + this.wheelDiameterInMeters = wheelDiameterInMeters; + return this; + } + + /** + * Set the friendly name of this module. This name is used in logging + * @param moduleName name of the module + * @return itself + */ + public SwerveModuleBuilder withModuleName(String moduleName) { + this.moduleName = moduleName; + return this; + } + + /** + * Build the configured instance of a SwerveModule + * @return a SwerveModule object + */ + public SwerveModule build() { + return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, driveInverted, steerMotorId, steerKP, steerKI, + steerKD, steerKS, steerKV, steerKA, + steerEncoderId, steerOffset, canBusObj, moduleLocation, gearRatio, wheelDiameterInMeters, + moduleName); + } + + } + +} diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java new file mode 100644 index 0000000..7489624 --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java @@ -0,0 +1,209 @@ +package org.fairportrobotics.frc.robolib.drivesystems.swerve; + +import java.util.Arrays; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class SwerveDriveSystem implements Subsystem{ + + private SwerveDriveKinematics driveKiniematics; + private ChassisSpeeds chassisSpeeds; + private Translation2d centerOfRotation; + + private SwerveDrivePoseEstimator3d poseEstimator; + + private SwerveModule[] modules; + + /** + * The max linear speed of the robot in meters per second + */ + private double MAX_LINEAR_SPEED = 3.9; + /** + * The max angular rotation speed in radians per second + */ + private double MAX_ANGULAR_SPEED = Math.PI * 2; + + private Pigeon2 gyro; + + /** + * Only used during simulation. + * This variable tracks when the last simulation loop started + * to calculate how long it's been since the last simulation loop + */ + private double mLastSimTime = 0; + + /** + * An implementation of Swerve drive. + * + * You should not directly instantiate this class. + * You should use SwerveBuilder to create an instance. + * @param pigeonId + * @param canBus + * @param maxLinearVelMetersSecond + * @param maxAngularVelRadiansSecond + * @param modules + */ + public SwerveDriveSystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){ + + MAX_LINEAR_SPEED = maxLinearVelMetersSecond; + MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond; + + this.modules = modules; + + driveKiniematics = new SwerveDriveKinematics( + Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size]) + ); + + gyro = new Pigeon2(pigeonId, canBus); + + poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero); + chassisSpeeds = new ChassisSpeeds(); + } + + @Override + public void periodic() { + SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation); + + for(int i=0; i m.getModuleLocation()).toArray(Translation2d[]::new); + } + + /** + * Get the requested state of the swerve modules. + * The order of the array is the same order from when they were defined + * + * @return an array of SwerveModuleState + */ + public SwerveModuleState[] getRequestedModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getRequestedModuleState()).toArray(SwerveModuleState[]::new); + } + + /** + * Get the actual state of the swerve modules. + * The order of the array is the same order from when they were defined + * @return an array of SwerveModuleState + */ + public SwerveModuleState[] getActualModuleStates(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getActualModuleState()).toArray(SwerveModuleState[]::new); + } + + /** + * Get an array of the SwerveModules configured for this system + * @return Array of SwerveModules + */ + public SwerveModule[] getModules(){ + return Arrays.stream(modules).toArray(SwerveModule[]::new); + } + + /** + * Get the current yaw angle from the Gyro + * @return The current yaw as an Angle + */ + private Angle getCurrentYaw(){ + return gyro.getYaw().getValue(); + } + + /** + * Get the instance of the gyro + * @return + */ + public Pigeon2 getGyro(){ + return gyro; + } + + /** + * Set robot speed + * @param chassisSpeeds + * @param centerOfRotation + */ + private void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){ + this.chassisSpeeds = chassisSpeeds; + this.centerOfRotation = centerOfRotation; + } + + /** + * Set robot speed relative to field + * @param x Forward robot speed in meters per second + * @param y Left robot speed in meters per second + * @param rot CCW robot speed in radians per second + */ + public void setChassisSpeedsFromJoystickFieldRelative(double x, double y, double rot){ + this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds( + MAX_LINEAR_SPEED * x, + MAX_LINEAR_SPEED * y, + MAX_ANGULAR_SPEED * rot, + Rotation2d.fromDegrees(getCurrentYaw().magnitude()) + ), new Translation2d()); + } + + /** + * Set robot speed relative to robot + * @param x Forward robot speed in meters per second + * @param y Left robot speed in meters per second + * @param rot CCW robot speed in radians per second + */ + public void setChassisSpeedsFromJoystickRobotRelative(double x, double y, double rot){ + this.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds( + MAX_LINEAR_SPEED * x, + MAX_LINEAR_SPEED * y, + MAX_ANGULAR_SPEED * rot, + Rotation2d.fromDegrees(getCurrentYaw().magnitude()) + ), new Translation2d()); + } + + /** + * Get the current field location of the robot + * @return The current pose as a Pose3d + */ + public Pose3d getRobotPose(){ + return poseEstimator.getEstimatedPosition(); + } + + private SwerveModulePosition[] getModulePositions(){ + return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new); + } + +} diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java new file mode 100644 index 0000000..a7c679a --- /dev/null +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java @@ -0,0 +1,240 @@ +package org.fairportrobotics.frc.robolib.drivesystems.swerve; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + + private final int CAN_UPDATE_FREQUENCY = 50; + private final double STEER_MAX_CURRENT = 60; + private final double DRIVE_MAX_CURRENT = 80; + + private final double gearRatio; + private final double wheelDiameterInMeters; + + private SwerveModuleState requestedModuleState; + + private TalonFX driveMotor; + private VelocityVoltage driveRequest = new VelocityVoltage(0); + + private TalonFX steerMotor; + private PositionVoltage steerPosRequest = new PositionVoltage(0); + + private CANcoder steerEncoder; + + private String moduleName; + + private Translation2d moduleLocation; + + public SwerveModule( + int driveMotorId, + double driveKP, + double driveKI, + double driveKD, + double driveKV, + boolean driveInverted, + int steerMotorId, + double steerKP, + double steerKI, + double steerKD, + double steerKS, + double steerKV, + double steerKA, + int steerEncoderId, + double steerOffset, + CANBus canBusObj, + Translation2d moduleLocation, + double gearRatio, + double wheelDiameterInMeters, + String moduleName + ){ + + steerEncoder = new CANcoder(steerEncoderId, canBusObj); + steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY); + steerEncoder.optimizeBusUtilization(); + steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset)); + + driveMotor = new TalonFX(driveMotorId, canBusObj); + driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY); + driveMotor.optimizeBusUtilization(); + driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted)); + + steerMotor = new TalonFX(steerMotorId, canBusObj); + steerMotor.optimizeBusUtilization(); + steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA)); + // steerMotor.setNeutralMode(NeutralModeValue.Brake); + + this.moduleLocation = moduleLocation; + + this.gearRatio = gearRatio; + this.wheelDiameterInMeters = wheelDiameterInMeters; + + this.moduleName = moduleName; + } + + public void setSteerRotations(Rotation2d rotations){ + steerMotor.setControl(steerPosRequest.withPosition(rotations.getRotations())); + } + + public Rotation2d getSteerRotations(){ + return new Rotation2d(steerEncoder.getAbsolutePosition().getValue()); + } + + public void setDriveSpeed(double speed){ + // Convert wheel speed to rotor speed + // (speed / Circumfrence of wheel) * gear ratio + double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio; + driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity)); + } + + /** + * Get drive motor speed in rotations per second + * @return + */ + public double getDriveMotorSpeed(){ + return driveMotor.getVelocity().refresh().getValueAsDouble(); + } + + /** + * Get drive wheel speed in meters per second + * @return + */ + public double getDriveWheelSpeed(){ + return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters); + } + + public Translation2d getModuleLocation(){ + return moduleLocation; + } + + public void setRequestedModuleState(SwerveModuleState state){ + this.setSteerRotations(state.angle); + this.setDriveSpeed(state.speedMetersPerSecond); + requestedModuleState = state; + } + + public TalonFX getSteerMotor(){ + return steerMotor; + } + + public TalonFX getDriveMotor(){ + return driveMotor; + } + + public CANcoder getSteerEncoder(){ + return steerEncoder; + } + + public SwerveModuleState getRequestedModuleState(){ + return requestedModuleState; + } + + public SwerveModuleState getActualModuleState() { + return new SwerveModuleState(getDriveWheelSpeed(), getSteerRotations()); + } + + public SwerveModulePosition getModulePosition(){ + return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations()); + } + + public void periodic(){ + SmartDashboard.putNumber(moduleName + " steerPow", steerMotor.getClosedLoopOutput().getValueAsDouble()); + SmartDashboard.putNumber(moduleName + " steer error", steerMotor.getClosedLoopError().getValueAsDouble()); + SmartDashboard.putNumber(moduleName + " steer setpoint", steerMotor.getClosedLoopReference().getValueAsDouble()); + + SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations()); + } + + public String getModuleName(){ + return moduleName; + } + + private double getModuleDistance(){ + double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) * (Math.PI * wheelDiameterInMeters); + return distance; + } + + private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV, boolean driveInverted){ + return new TalonFXConfiguration().withSlot0( + new Slot0Configs() + .withKP(kP) + .withKI(kI) + .withKD(kD) + .withKV(kV) + ) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(driveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive) + ).withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(DRIVE_MAX_CURRENT) + ); + } + + private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){ + return new TalonFXConfiguration().withSlot0( + new Slot0Configs() + .withKP(kP) + .withKI(kI) + .withKD(kD) + .withKS(kS) + .withKV(kV) + .withKA(kA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign) + ).withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(160) + .withMotionMagicAcceleration(160) + .withMotionMagicJerk(1600) + ).withFeedback( + new FeedbackConfigs() + .withFeedbackRemoteSensorID(steerEncoder.getDeviceID()) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) + .withSensorToMechanismRatio(1.0) + .withRotorToSensorRatio(12.8) + ).withClosedLoopGeneral( + new ClosedLoopGeneralConfigs() + .withContinuousWrap(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + ).withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(STEER_MAX_CURRENT) + ); + } + + private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){ + return new CANcoderConfiguration().withMagnetSensor( + new MagnetSensorConfigs() + .withMagnetOffset(-steerOffset) + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(0.5) + ); + } + +} diff --git a/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java b/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java index 2f80b17..b003bc1 100644 --- a/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java +++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/LibraryTest.java @@ -1,7 +1,7 @@ /* * This Java source file was generated by the Gradle 'init' task. */ -package com.fairportrobotics.frc.robolib; +package org.fairportrobotics.frc.robolib; import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.*;