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

Adding AHRS from Studica to the drivetrain so the NavX is assumed to be used, also cleaned up some unused imports

Browse Source
This commit is contained in:
Bradley Bickford 2025-01-20 19:33:33 -05:00
parent 4d9aa82520
commit 198d105741
2 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -7,6 +7,9 @@ package frc.robot.subsystems;
import java.util.function.BooleanSupplier; import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier; import java.util.function.DoubleSupplier;
import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
@ -30,7 +33,7 @@ public class Drivetrain extends SubsystemBase {
protected MAXSwerveModule m_rearRight; protected MAXSwerveModule m_rearRight;
// The gyro sensor // The gyro sensor
private ADIS16470_IMU m_gyro; private AHRS ahrs;
// Odometry class for tracking robot pose // Odometry class for tracking robot pose
private SwerveDriveOdometry m_odometry; private SwerveDriveOdometry m_odometry;
@ -61,11 +64,11 @@ public class Drivetrain extends SubsystemBase {
DrivetrainConstants.kBackRightChassisAngularOffset DrivetrainConstants.kBackRightChassisAngularOffset
); );
m_gyro = new ADIS16470_IMU(); ahrs = new AHRS(NavXComType.kMXP_SPI);
m_odometry = new SwerveDriveOdometry( m_odometry = new SwerveDriveOdometry(
DrivetrainConstants.kDriveKinematics, DrivetrainConstants.kDriveKinematics,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), Rotation2d.fromDegrees(ahrs.getAngle()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
m_frontLeft.getPosition(), m_frontLeft.getPosition(),
m_frontRight.getPosition(), m_frontRight.getPosition(),
@ -78,7 +81,7 @@ public class Drivetrain extends SubsystemBase {
public void periodic() { public void periodic() {
// Update the odometry in the periodic block // Update the odometry in the periodic block
m_odometry.update( m_odometry.update(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
m_frontLeft.getPosition(), m_frontLeft.getPosition(),
m_frontRight.getPosition(), m_frontRight.getPosition(),
@ -103,7 +106,7 @@ public class Drivetrain extends SubsystemBase {
*/ */
public void resetOdometry(Pose2d pose) { public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition( m_odometry.resetPosition(
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
m_frontLeft.getPosition(), m_frontLeft.getPosition(),
m_frontRight.getPosition(), m_frontRight.getPosition(),
@ -144,7 +147,7 @@ public class Drivetrain extends SubsystemBase {
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ))) Rotation2d.fromDegrees(getGyroValue()))
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
SwerveDriveKinematics.desaturateWheelSpeeds( SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@ -194,7 +197,11 @@ public class Drivetrain extends SubsystemBase {
/** Zeroes the heading of the robot. */ /** Zeroes the heading of the robot. */
public void zeroHeading() { public void zeroHeading() {
m_gyro.reset(); ahrs.reset();;
}
public double getGyroValue() {
return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
} }
/** /**
@ -203,7 +210,7 @@ public class Drivetrain extends SubsystemBase {
* @return the robot's heading in degrees, from -180 to 180 * @return the robot's heading in degrees, from -180 to 180
*/ */
public double getHeading() { public double getHeading() {
return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees(); return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
} }
/** /**
@ -212,6 +219,6 @@ public class Drivetrain extends SubsystemBase {
* @return The turn rate of the robot, in degrees per second * @return The turn rate of the robot, in degrees per second
*/ */
public double getTurnRate() { public double getTurnRate() {
return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
} }
} }

3
src/main/java/frc/robot/subsystems/sysid/ArmSysID.java
View File

@ -4,11 +4,8 @@ import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngle;
import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.units.measure.MutDistance;
import edu.wpi.first.units.measure.MutLinearVelocity;
import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;