Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pose estimation, VisualPoseProvider setup, plus a little more cleanup of Drivetrain subsystem to have things make slightly more sense

Browse Source
This commit is contained in:
Bradley Bickford
2024-01-10 20:24:59 -05:00
parent 73b068903e
commit 69fd4b76cc
5 changed files with 114 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
src/main/java/frc/robot/utilities/MAXSwerveModule.java
View File

@@ -11,7 +11,7 @@ import com.revrobotics.SparkPIDController;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import frc.robot.constants.ModuleConstants;
import frc.robot.constants.SwerveModuleConstants;
public class MAXSwerveModule {
private final CANSparkMax m_drivingSparkMax;
@@ -52,49 +52,49 @@ public class MAXSwerveModule {
// Apply position and velocity conversion factors for the driving encoder. The
// native units for position and velocity are rotations and RPM, respectively,
// but we want meters and meters per second to use with WPILib's swerve APIs.
m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor);
m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor);
m_drivingEncoder.setPositionConversionFactor(SwerveModuleConstants.kDrivingEncoderPositionFactor);
m_drivingEncoder.setVelocityConversionFactor(SwerveModuleConstants.kDrivingEncoderVelocityFactor);
// Apply position and velocity conversion factors for the turning encoder. We
// want these in radians and radians per second to use with WPILib's swerve
// APIs.
m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor);
m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor);
m_turningEncoder.setPositionConversionFactor(SwerveModuleConstants.kTurningEncoderPositionFactor);
m_turningEncoder.setVelocityConversionFactor(SwerveModuleConstants.kTurningEncoderVelocityFactor);
// Invert the turning encoder, since the output shaft rotates in the opposite direction of
// the steering motor in the MAXSwerve Module.
m_turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted);
m_turningEncoder.setInverted(SwerveModuleConstants.kTurningEncoderInverted);
// Enable PID wrap around for the turning motor. This will allow the PID
// controller to go through 0 to get to the setpoint i.e. going from 350 degrees
// to 10 degrees will go through 0 rather than the other direction which is a
// longer route.
m_turningPIDController.setPositionPIDWrappingEnabled(true);
m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput);
m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput);
m_turningPIDController.setPositionPIDWrappingMinInput(SwerveModuleConstants.kTurningEncoderPositionPIDMinInput);
m_turningPIDController.setPositionPIDWrappingMaxInput(SwerveModuleConstants.kTurningEncoderPositionPIDMaxInput);
// Set the PID gains for the driving motor. Note these are example gains, and you
// may need to tune them for your own robot!
m_drivingPIDController.setP(ModuleConstants.kDrivingP);
m_drivingPIDController.setI(ModuleConstants.kDrivingI);
m_drivingPIDController.setD(ModuleConstants.kDrivingD);
m_drivingPIDController.setFF(ModuleConstants.kDrivingFF);
m_drivingPIDController.setOutputRange(ModuleConstants.kDrivingMinOutput,
ModuleConstants.kDrivingMaxOutput);
m_drivingPIDController.setP(SwerveModuleConstants.kDrivingP);
m_drivingPIDController.setI(SwerveModuleConstants.kDrivingI);
m_drivingPIDController.setD(SwerveModuleConstants.kDrivingD);
m_drivingPIDController.setFF(SwerveModuleConstants.kDrivingFF);
m_drivingPIDController.setOutputRange(SwerveModuleConstants.kDrivingMinOutput,
SwerveModuleConstants.kDrivingMaxOutput);
// Set the PID gains for the turning motor. Note these are example gains, and you
// may need to tune them for your own robot!
m_turningPIDController.setP(ModuleConstants.kTurningP);
m_turningPIDController.setI(ModuleConstants.kTurningI);
m_turningPIDController.setD(ModuleConstants.kTurningD);
m_turningPIDController.setFF(ModuleConstants.kTurningFF);
m_turningPIDController.setOutputRange(ModuleConstants.kTurningMinOutput,
ModuleConstants.kTurningMaxOutput);
m_turningPIDController.setP(SwerveModuleConstants.kTurningP);
m_turningPIDController.setI(SwerveModuleConstants.kTurningI);
m_turningPIDController.setD(SwerveModuleConstants.kTurningD);
m_turningPIDController.setFF(SwerveModuleConstants.kTurningFF);
m_turningPIDController.setOutputRange(SwerveModuleConstants.kTurningMinOutput,
SwerveModuleConstants.kTurningMaxOutput);
m_drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode);
m_turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode);
m_drivingSparkMax.setSmartCurrentLimit(ModuleConstants.kDrivingMotorCurrentLimit);
m_turningSparkMax.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit);
m_drivingSparkMax.setIdleMode(SwerveModuleConstants.kDrivingMotorIdleMode);
m_turningSparkMax.setIdleMode(SwerveModuleConstants.kTurningMotorIdleMode);
m_drivingSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kDrivingMotorCurrentLimit);
m_turningSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kTurningMotorCurrentLimit);
// Save the SPARK MAX configurations. If a SPARK MAX browns out during
// operation, it will maintain the above configurations.

9
src/main/java/frc/robot/utilities/VisualPoseProvider.java Normal file
View File

@@ -0,0 +1,9 @@
package frc.robot.utilities;
import edu.wpi.first.math.geometry.Pose2d;
public interface VisualPoseProvider {
public record VisualPose(Pose2d visualPose, double timestamp) {}
public VisualPose getVisualPose();
}