Skip to content

Refactor #11

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Refactor #11

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3ff892d
Refactor DriveSubsystem.java
UnnatPar Dec 24, 2024
87ed684
refactor V2 because im moderately sped
UnnatPar Dec 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
151 changes: 53 additions & 98 deletions src/main/java/frc/robot/subsystems/DriveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,88 +24,79 @@
import frc.robot.Robot;

public class DriveSubsystem extends SubsystemBase {
private final SwerveModule m_frontLeft = new SwerveModule(
private final SwerveModule frontLeft = new SwerveModule(
DriveConstants.kFrontLeftDriveMotorPort,
DriveConstants.kFrontLeftTurningMotorPort,
DriveConstants.kFrontLeftTurningEncoderPort,
DriveConstants.kFrontLeftDriveMotorReversed,
DriveConstants.kFrontLeftTurningEncoderOffset);

private final SwerveModule m_rearLeft = new SwerveModule(
private final SwerveModule rearLeft = new SwerveModule(
DriveConstants.kRearLeftDriveMotorPort,
DriveConstants.kRearLeftTurningMotorPort,
DriveConstants.kRearLeftTurningEncoderPort,
DriveConstants.kRearLeftDriveMotorReversed,
DriveConstants.kRearLeftTurningEncoderOffset);

private final SwerveModule m_frontRight = new SwerveModule(
private final SwerveModule frontRight = new SwerveModule(
DriveConstants.kFrontRightDriveMotorPort,
DriveConstants.kFrontRightTurningMotorPort,
DriveConstants.kFrontRightTurningEncoderPort,
DriveConstants.kFrontRightDriveMotorReversed,
DriveConstants.kFrontRightTurningEncoderOffset);

private final SwerveModule m_rearRight = new SwerveModule(
private final SwerveModule rearRight = new SwerveModule(
DriveConstants.kRearRightDriveMotorPort,
DriveConstants.kRearRightTurningMotorPort,
DriveConstants.kRearRightTurningEncoderPort,
DriveConstants.kRearRightDriveMotorReversed,
DriveConstants.kRearRightTurningEncoderOffset);

private final AHRS m_gyro = new AHRS();
private double m_gyroAngle;

private final Timer m_headingCorrectionTimer = new Timer();
private final PIDController m_headingCorrectionPID = new PIDController(DriveConstants.kPHeadingCorrectionController,
0, 0);
private SwerveModulePosition[] m_swerveModulePositions = new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
private final AHRS gyro = new AHRS();
private double GYRO_ANGLE;
private SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[] {
frontLeft.getPosition(),
frontRight.getPosition(),
rearLeft.getPosition(),
rearRight.getPosition()
};

private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics,
m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
private final SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics,
gyro.getRotation2d(), swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
VisionConstants.kVisionSTDDevs);
private final Field2d field = new Field2d();

private final Field2d m_field = new Field2d();

/** Creates a new DriveSubsystem. */
// Creates a new DriveSubsystem.
public DriveSubsystem() {
SmartDashboard.putData("Field", m_field);
m_headingCorrectionTimer.restart();
m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI);
SmartDashboard.putData("Field", field);
}

@Override
public void periodic() {
// This method will be called once per scheduler run

m_swerveModulePositions = new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
swerveModulePositions = new SwerveModulePosition[] {
frontLeft.getPosition(),
frontRight.getPosition(),
rearLeft.getPosition(),
rearRight.getPosition()
};

m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
m_swerveModulePositions);
poseEstimator.update(Robot.isReal() ? gyro.getRotation2d() : new Rotation2d(GYRO_ANGLE), swerveModulePositions);

m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
field.setRobotPose(poseEstimator.getEstimatedPosition());

SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX());
SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY());
SmartDashboard.putNumber("gyro angle", gyro.getAngle());
SmartDashboard.putNumber("odometryX", poseEstimator.getEstimatedPosition().getX());
SmartDashboard.putNumber("odometryY", poseEstimator.getEstimatedPosition().getY());

// AdvantageScope Logging
double[] logData = {
m_frontLeft.getPosition().angle.getDegrees(), m_frontLeft.driveOutput,
m_frontRight.getPosition().angle.getDegrees(), m_frontRight.driveOutput,
m_rearLeft.getPosition().angle.getDegrees(), m_rearLeft.driveOutput,
m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput,
double[] LOG_DATA = {
frontLeft.getPosition().angle.getDegrees(), frontLeft.driveOutput,
frontRight.getPosition().angle.getDegrees(), frontRight.driveOutput,
rearLeft.getPosition().angle.getDegrees(), rearLeft.driveOutput,
rearRight.getPosition().angle.getDegrees(), rearRight.driveOutput,
};
SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
SmartDashboard.putNumberArray("AdvantageScope Swerve States", LOG_DATA);
}

/**
Expand All @@ -114,7 +105,7 @@ public void periodic() {
* @return The pose.
*/
public Pose2d getPose() {
return m_poseEstimator.getEstimatedPosition();
return poseEstimator.getEstimatedPosition();
}

/**
Expand All @@ -127,49 +118,13 @@ public Pose2d getPose() {
* field.
*/
public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
// If we are rotating, reset the timer
if (rotation != 0) {
m_headingCorrectionTimer.reset();
}

/*
* Heading correction helps maintain the same heading and
* prevents rotational drive while our robot is translating
*
* For heading correction we use a timer to ensure that we
* lose all rotational momentum before saving the heading
* that we want to maintain
*/

// TODO: Test heading correction without timer
// TODO: Test heading correction using gyro's rotational velocity (if it is 0
// then set heading instead of timer)

// Save our desired rotation to a variable we can add our heading correction
// adjustments to
double calculatedRotation = rotation;

double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians());

// If we are not translating or if not enough time has passed since the last
// time we rotated
if ((xSpeed == 0 && ySpeed == 0)
|| m_headingCorrectionTimer.get() < DriveConstants.kHeadingCorrectionTurningStopTime) {
// Update our desired angle
m_headingCorrectionPID.setSetpoint(currentAngle);
} else {
// If we are translating or if we have not rotated for a long enough time
// then maintain our desired angle
calculatedRotation = m_headingCorrectionPID.calculate(currentAngle);
}

// Depending on whether the robot is being driven in field relative, calculate
// the desired states for each of the modules
double calculatedRotation = rotation;
SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
: new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
Robot.isReal() ? gyro.getRotation2d() : new Rotation2d(GYRO_ANGLE)) :
new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));

setModuleStates(swerveModuleStates);
}
Expand All @@ -180,25 +135,25 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_poseEstimator.resetPosition(
Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
poseEstimator.resetPosition(
Robot.isReal() ? gyro.getRotation2d() : new Rotation2d(GYRO_ANGLE),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
frontLeft.getPosition(),
frontRight.getPosition(),
rearLeft.getPosition(),
rearRight.getPosition()
},
pose);
}

/** Zeroes the heading of the robot. */
public void zeroHeading() {
m_gyro.reset();
m_gyroAngle = 0;
gyro.reset();
GYRO_ANGLE = 0;
}

public void addVisionMeasurement(Pose2d pose, double timestamp) {
m_poseEstimator.addVisionMeasurement(pose, timestamp);
poseEstimator.addVisionMeasurement(pose, timestamp);
}

/**
Expand All @@ -209,23 +164,23 @@ public void addVisionMeasurement(Pose2d pose, double timestamp) {
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(desiredStates[0]);
m_frontRight.setDesiredState(desiredStates[1]);
m_rearLeft.setDesiredState(desiredStates[2]);
m_rearRight.setDesiredState(desiredStates[3]);
frontLeft.setDesiredState(desiredStates[0]);
frontRight.setDesiredState(desiredStates[1]);
rearLeft.setDesiredState(desiredStates[2]);
rearRight.setDesiredState(desiredStates[3]);

// AdvantageScope Logging
double[] logData = {
double[] LOG_DATA = {
desiredStates[0].angle.getDegrees(), desiredStates[0].speedMetersPerSecond,
desiredStates[1].angle.getDegrees(), desiredStates[1].speedMetersPerSecond,
desiredStates[2].angle.getDegrees(), desiredStates[2].speedMetersPerSecond,
desiredStates[3].angle.getDegrees(), desiredStates[3].speedMetersPerSecond,
};
SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logData);
SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", LOG_DATA);

// Takes the integral of the rotation speed to find the current angle for the
// simulator
m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond
GYRO_ANGLE += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond
* Robot.kDefaultPeriod;
}

Expand Down
Loading