Skip to content

Add instrumentation. #33

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 1 commit into
base: main
Choose a base branch
from
Open

Add instrumentation. #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
12 changes: 8 additions & 4 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,22 +24,26 @@ public class DriveSubsystem extends SubsystemBase {
private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
DriveConstants.kFrontLeftDrivingCanId,
DriveConstants.kFrontLeftTurningCanId,
DriveConstants.kFrontLeftChassisAngularOffset);
DriveConstants.kFrontLeftChassisAngularOffset,
"FL");

private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
DriveConstants.kFrontRightDrivingCanId,
DriveConstants.kFrontRightTurningCanId,
DriveConstants.kFrontRightChassisAngularOffset);
DriveConstants.kFrontRightChassisAngularOffset,
"FR");

private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
DriveConstants.kRearLeftDrivingCanId,
DriveConstants.kRearLeftTurningCanId,
DriveConstants.kBackLeftChassisAngularOffset);
DriveConstants.kBackLeftChassisAngularOffset,
"RL");

private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
DriveConstants.kRearRightDrivingCanId,
DriveConstants.kRearRightTurningCanId,
DriveConstants.kBackRightChassisAngularOffset);
DriveConstants.kBackRightChassisAngularOffset,
"RR");

// The gyro sensor
private final ADIS16470_IMU m_gyro = new ADIS16470_IMU();
Expand Down
24 changes: 19 additions & 5 deletions src/main/java/frc/robot/subsystems/MAXSwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
Expand All @@ -25,25 +26,29 @@ public class MAXSwerveModule {

private final RelativeEncoder m_drivingEncoder;
private final AbsoluteEncoder m_turningEncoder;
private final RelativeEncoder m_turningRelativeEncoder; // just used for debugging

private final SparkClosedLoopController m_drivingClosedLoopController;
private final SparkClosedLoopController m_turningClosedLoopController;

private double m_chassisAngularOffset = 0;
private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());

private String m_name;

/**
* Constructs a MAXSwerveModule and configures the driving and turning motor,
* encoder, and PID controller. This configuration is specific to the REV
* MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore
* Encoder.
*/
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset, String name) {
m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);

m_drivingEncoder = m_drivingSpark.getEncoder();
m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
m_turningRelativeEncoder = m_turningSpark.getEncoder();

m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
Expand All @@ -59,6 +64,8 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular
m_chassisAngularOffset = chassisAngularOffset;
m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
m_drivingEncoder.setPosition(0);

m_name = name;
}

/**
Expand All @@ -76,14 +83,18 @@ public SwerveModuleState getState() {
/**
* Returns the current position of the module.
*
* @return The current position of the module.
* @return The current position of the module
*/
public SwerveModulePosition getPosition() {
// Apply chassis angular offset to the encoder position to get the position
// relative to the chassis.
return new SwerveModulePosition(
m_drivingEncoder.getPosition(),
new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
double drivingPosition = m_drivingEncoder.getPosition();
double turningPosition = m_turningEncoder.getPosition() - m_chassisAngularOffset;
SmartDashboard.putNumber("swerve/" + m_name + "/drive/position", drivingPosition);
SmartDashboard.putNumber("swerve/" + m_name + "/drive/actual", m_drivingEncoder.getVelocity());
SmartDashboard.putNumber("swerve/" + m_name + "/turn/actual", turningPosition);
SmartDashboard.putNumber("swerve/" + m_name + "/turn/relativeactual", m_turningRelativeEncoder.getPosition());
return new SwerveModulePosition(drivingPosition, new Rotation2d(turningPosition));
}

/**
Expand All @@ -104,6 +115,9 @@ public void setDesiredState(SwerveModuleState desiredState) {
m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);

SmartDashboard.putNumber("swerve/" + m_name + "/drive/set", correctedDesiredState.speedMetersPerSecond);
SmartDashboard.putNumber("swerve/" + m_name + "/turn/set", correctedDesiredState.angle.getDegrees());

m_desiredState = desiredState;
}

Expand Down