Skip to content

Commit

Permalink
Fixed units. Corrected logging names.
Browse files Browse the repository at this point in the history
Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
  • Loading branch information
thenetworkgrinch committed Jan 30, 2024
1 parent 114e816 commit 0bcee3a
Showing 1 changed file with 7 additions and 7 deletions.
14 changes: 7 additions & 7 deletions src/main/java/swervelib/SwerveDriveTest.java
Original file line number Diff line number Diff line change
Expand Up @@ -288,17 +288,17 @@ public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swe
* @param log - the logger
*/
public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log) {
SmartDashboard.putNumber("Angle Volt " + module.moduleNumber, module.getAngleMotor().getVoltage());
SmartDashboard.putNumber("Angle Pos " + module.moduleNumber, module.getAbsolutePosition());
SmartDashboard.putNumber("Angle Vel " + module.moduleNumber, module.getAbsoluteEncoder().getVelocity());
log.motor("angle-" + module.moduleNumber)
SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Voltage", module.getAngleMotor().getVoltage());
SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Position", module.getAbsolutePosition());
SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Absolute Encoder Velocity", module.getAbsoluteEncoder().getVelocity());
log.motor("angle-" + module.configuration.name)
.voltage(
m_appliedVoltage.mut_replace(
module.getAngleMotor().getVoltage(), Volts))
.angularPosition(
m_rotations.mut_replace(module.getAbsolutePosition(), Rotations))
m_rotations.mut_replace(module.getAbsolutePosition(), Degrees))
.angularVelocity(m_angVelocity.mut_replace(module.getAngleMotor().getVelocity(),
RotationsPerSecond));
DegreesPerSecond));
}

/**
Expand All @@ -312,7 +312,7 @@ public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swe
SwerveDrive swerveDrive) {
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
(Measure<Voltage> voltage) -> {
SwerveDriveTest.powerAngleMotors(swerveDrive, voltage.in(Volts));
SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
},
log -> {
Expand Down

0 comments on commit 0bcee3a

Please sign in to comment.