Skip to content

Commit

Permalink
chore(standards): remove Hungarian notation
Browse files Browse the repository at this point in the history
  • Loading branch information
TheGamer1002 committed Jan 13, 2025
1 parent 9d49044 commit f480d6a
Show file tree
Hide file tree
Showing 4 changed files with 151 additions and 153 deletions.
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/RobotContainer.kt
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
import frc.robot.Constants.OperatorConstants
import frc.robot.commands.Autos
import frc.robot.commands.ExampleCommand
import frc.robot.generated.TunerConstants
import frc.robot.subsystems.CommandSwerveDrivetrain
import frc.robot.subsystems.ExampleSubsystem

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -29,7 +27,7 @@ import frc.robot.subsystems.ExampleSubsystem
object RobotContainer
{
private val MaxSpeed =
TunerConstants.kSpeedAt12Volts.`in`(Units.MetersPerSecond) // kSpeedAt12Volts desired top speed
TunerConstants.speedAt12Volts.`in`(Units.MetersPerSecond) // kSpeedAt12Volts desired top speed
private val MaxAngularRate = Units.RotationsPerSecond.of(0.75)
.`in`(Units.RadiansPerSecond) // 3/4 of a rotation per second max angular velocity

Expand Down
58 changes: 29 additions & 29 deletions src/main/java/frc/robot/Telemetry.kt
Original file line number Diff line number Diff line change
Expand Up @@ -41,40 +41,40 @@ class Telemetry(private val MaxSpeed: Double) {
private val fieldTypePub: StringPublisher = table.getStringTopic(".type").publish()

/* Mechanisms to represent the swerve module states */
private val m_moduleMechanisms = arrayOf(
private val moduleMechanisms = arrayOf(
Mechanism2d(1.0, 1.0),
Mechanism2d(1.0, 1.0),
Mechanism2d(1.0, 1.0),
Mechanism2d(1.0, 1.0),
)

/* A direction and length changing ligament for speed representation */
private val m_moduleSpeeds = arrayOf(
m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5)
private val moduleSpeeds = arrayOf(
moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5)
.append(MechanismLigament2d("Speed", 0.5, 0.0)),
m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5)
moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5)
.append(MechanismLigament2d("Speed", 0.5, 0.0)),
m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5)
moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5)
.append(MechanismLigament2d("Speed", 0.5, 0.0)),
m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5)
moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5)
.append(MechanismLigament2d("Speed", 0.5, 0.0)),
)

/* A direction changing and length constant ligament for module direction */
private val m_moduleDirections = arrayOf(
m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
private val moduleDirections = arrayOf(
moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
.append(MechanismLigament2d("Direction", 0.1, 0.0, 0.0, Color8Bit(Color.kWhite))),
m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
.append(MechanismLigament2d("Direction", 0.1, 0.0, 0.0, Color8Bit(Color.kWhite))),
m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
.append(MechanismLigament2d("Direction", 0.1, 0.0, 0.0, Color8Bit(Color.kWhite))),
m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
.append(MechanismLigament2d("Direction", 0.1, 0.0, 0.0, Color8Bit(Color.kWhite))),
)

private val m_poseArray = DoubleArray(3)
private val m_moduleStatesArray = DoubleArray(8)
private val m_moduleTargetsArray = DoubleArray(8)
private val poseArray = DoubleArray(3)
private val moduleStatesArray = DoubleArray(8)
private val moduleTargetsArray = DoubleArray(8)

/**
* Construct a telemetry object, with the specified max speed of the robot
Expand All @@ -97,33 +97,33 @@ class Telemetry(private val MaxSpeed: Double) {
driveOdometryFrequency.set(1.0 / state.OdometryPeriod)

/* Also write to log file */
m_poseArray[0] = state.Pose.x
m_poseArray[1] = state.Pose.y
m_poseArray[2] = state.Pose.rotation.degrees
poseArray[0] = state.Pose.x
poseArray[1] = state.Pose.y
poseArray[2] = state.Pose.rotation.degrees
for (i in 0..3) {
m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.radians
m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond
m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.radians
m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond
moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.radians
moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond
moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.radians
moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond
}

SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray)
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray)
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray)
SignalLogger.writeDoubleArray("DriveState/Pose", poseArray)
SignalLogger.writeDoubleArray("DriveState/ModuleStates", moduleStatesArray)
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", moduleTargetsArray)
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds")

/* Telemeterize the pose to a Field2d */
fieldTypePub.set("Field2d")
fieldPub.set(m_poseArray)
fieldPub.set(poseArray)

/* Telemeterize the module states to a Mechanism2d */
for (i in 0..3) {
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle)
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle)
m_moduleSpeeds[i].length =
moduleSpeeds[i].setAngle(state.ModuleStates[i].angle)
moduleDirections[i].setAngle(state.ModuleStates[i].angle)
moduleSpeeds[i].length =
state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)

SmartDashboard.putData("Module $i", m_moduleMechanisms[i])
SmartDashboard.putData("Module $i", moduleMechanisms[i])
}
}
}
Loading

0 comments on commit f480d6a

Please sign in to comment.