style: reformat code and optimize imports

FRC3636 · Nov 16, 2023 · b0f2ec4 · b0f2ec4
1 parent 6903e94
commit b0f2ec4
Show file tree

Hide file tree

Showing 11 changed files with 171 additions and 183 deletions.
diff --git a/src/main/java/frc/robot/CAN.kt b/src/main/java/frc/robot/CAN.kt
@@ -9,7 +9,7 @@ enum class CANDevice(val id: Int) {
     BackLeftDrivingMotor(1),
     BackRightTurningMotor(8),
     BackRightDrivingMotor(7),
-IndexerMotor(17),                                           
+    IndexerMotor(17),
     TurretMotor(18),
     YeetWheelMotor(19),
     YeetFeedMotor(20),

diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt
@@ -35,12 +35,14 @@ object Robot : LoggedRobot() {
             PowerDistribution(1, PowerDistribution.ModuleType.kRev) // Enables power distribution logging
         } else {
             setUseTiming(false) // Run as fast as possible
-            val logPath: String = LogFileUtil.findReplayLog() // Pull the replay log from AdvantageScope (or prompt the user)
+            val logPath: String =
+                LogFileUtil.findReplayLog() // Pull the replay log from AdvantageScope (or prompt the user)
             Logger.getInstance().setReplaySource(WPILOGReader(logPath)) // Read replay log
             Logger.getInstance()
                 .addDataReceiver(WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))) // Save outputs to a new log
         }
-        Logger.getInstance().start() // Start logging! No more data receivers, replay sources, or metadata values may be added.
+        Logger.getInstance()
+            .start() // Start logging! No more data receivers, replay sources, or metadata values may be added.
 
         // Access the RobotContainer object so that it is initialized. This will perform all our
         // button bindings, and put our autonomous chooser on the dashboard.

diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt
@@ -16,8 +16,8 @@ object RobotContainer {
 
     init {
         configureBindings()
-    
-        DriverStation.silenceJoystickConnectionWarning(RobotBase.isSimulation());
+
+        DriverStation.silenceJoystickConnectionWarning(RobotBase.isSimulation())
     }
 
     private fun configureBindings() {

diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.kt b/src/main/java/frc/robot/commands/DriveWithJoysticks.kt
@@ -1,7 +1,7 @@
 package frc.robot.commands
 
-import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.wpilibj.Joystick
 import edu.wpi.first.wpilibj2.command.Command
 import frc.robot.subsystems.drivetrain.Drivetrain

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.kt b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.kt
@@ -1,13 +1,16 @@
 package frc.robot.subsystems.drivetrain
 
 import MAXSwerveModuleIO
+import ModuleIO
+import SimSwerveModuleIO
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
 import edu.wpi.first.math.geometry.Pose2d
-import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics
 import edu.wpi.first.math.kinematics.SwerveModuleState
+import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.CommandScheduler
@@ -17,30 +20,21 @@ import frc.robot.RobotContainer
 import frc.robot.utils.PerCorner
 import frc.robot.utils.cornerStatesToChassisSpeeds
 import frc.robot.utils.toCornerSwerveModuleStates
-import SimSwerveModuleIO
 import org.littletonrobotics.junction.Logger
-import edu.wpi.first.math.MathShared
-import edu.wpi.first.math.util.Units
-import ModuleIO
-import ModuleIO.Inputs
 
 // A singleton object representing the drivetrain.
 object Drivetrain : Subsystem {
 
 
-
-
-
     private val modules = if (RobotBase.isReal()) {
         MODULE_CAN_DEVICES.zip(MODULE_POSITIONS).map { (can, pose) ->
             MAXSwerveModuleIO(can.first, can.second, pose.rotation)
-        }.zip ( PerCorner.generate { ModuleIO.Inputs() } )
+        }.zip(PerCorner.generate { ModuleIO.Inputs() })
     } else {
-        PerCorner.generate { SimSwerveModuleIO() }.zip( PerCorner.generate { ModuleIO.Inputs() } )
+        PerCorner.generate { SimSwerveModuleIO() }.zip(PerCorner.generate { ModuleIO.Inputs() })
     }
 
 
-
     private val positions = MODULE_POSITIONS
 
     private val gyro: GyroIO = if (RobotBase.isReal()) {
@@ -52,7 +46,7 @@ object Drivetrain : Subsystem {
     val gyroInputs = GyroInputs()
 
     // Create swerve drivetrain kinematics using the translation parts of the module positions.
-    private val kinematics = SwerveDriveKinematics(*positions.map(Pose2d::getTranslation).toList().toTypedArray())
+    private val kinematics = SwerveDriveKinematics(*positions.map { it.translation }.toList().toTypedArray())
 
     private val poseEstimator = SwerveDrivePoseEstimator(
         kinematics, // swerve drive kinematics
@@ -77,20 +71,21 @@ object Drivetrain : Subsystem {
 
 
         Logger.getInstance().recordOutput("Odometry/pose", estimatedPose)
-        Logger.getInstance().recordOutput("Drive/ModulesState/Measured", *modules.map{it.second.state}.toTypedArray())
-        Logger.getInstance().recordOutput("Drive/ModulesState/Desired", *modules.map{it.second.desiredState}.toTypedArray())
-
+        Logger.getInstance()
+            .recordOutput("Drive/ModulesState/Measured", *modules.map { it.second.state }.toTypedArray())
+        Logger.getInstance()
+            .recordOutput("Drive/ModulesState/Desired", *modules.map { it.second.desiredState }.toTypedArray())
 
 
         // Log the swerve module states to SmartDashboard.
         SmartDashboard.putNumberArray(
             "Drive/ModuleStates/Measured",
-            modules.map {it.second.state}.flatMap { listOf(it.angle.radians, it.speedMetersPerSecond) }
+            modules.map { it.second.state }.flatMap { listOf(it.angle.radians, it.speedMetersPerSecond) }
                 .toTypedArray()
         )
         SmartDashboard.putNumberArray(
             "Drive/ModuleStates/Desired",
-            modules.map {it.second.desiredState}.flatMap { listOf(it.angle.radians, it.speedMetersPerSecond) }
+            modules.map { it.second.desiredState }.flatMap { listOf(it.angle.radians, it.speedMetersPerSecond) }
                 .toTypedArray()
         )
 
@@ -100,7 +95,7 @@ object Drivetrain : Subsystem {
 
     // The current speed of chassis relative to the ground, assuming that the wheels have traction with the ground.
     val chassisSpeeds: ChassisSpeeds
-        get() = kinematics.cornerStatesToChassisSpeeds(modules.map {it.second.state})
+        get() = kinematics.cornerStatesToChassisSpeeds(modules.map { it.second.state })
 
     // Set the drivetrain to an X-formation to passively prevent movement in any direction.
     fun brake() {
@@ -114,15 +109,13 @@ object Drivetrain : Subsystem {
     //
     // Note that the speeds are relative to the chassis, not the field.
     fun drive(speeds: ChassisSpeeds) {
-        
+
         // TODO: desaturate wheel speeds
         // SwerveDriveKinematics.desaturateWheelSpeeds(speeds, MAX_SPEED)
 
 
-
-
         kinematics.toCornerSwerveModuleStates(speeds).zip(modules).forEach { (state, module) ->
-            module.first.setDesiredState(state) 
+            module.first.setDesiredState(state)
         }
     }
 
@@ -138,9 +131,9 @@ internal val TRACK_WIDTH_HALF: Double = Units.inchesToMeters(14.0)
 
 // Constants
 internal val MODULE_POSITIONS = PerCorner(
-    frontLeft = Pose2d(Translation2d(WHEEL_BASE_HALF, TRACK_WIDTH_HALF), Rotation2d.fromDegrees(-90.0)), 
-    frontRight = Pose2d(Translation2d(WHEEL_BASE_HALF, -TRACK_WIDTH_HALF), Rotation2d.fromDegrees(0.0)), 
-    backRight = Pose2d(Translation2d(-WHEEL_BASE_HALF, TRACK_WIDTH_HALF), Rotation2d.fromDegrees(90.0)), 
+    frontLeft = Pose2d(Translation2d(WHEEL_BASE_HALF, TRACK_WIDTH_HALF), Rotation2d.fromDegrees(-90.0)),
+    frontRight = Pose2d(Translation2d(WHEEL_BASE_HALF, -TRACK_WIDTH_HALF), Rotation2d.fromDegrees(0.0)),
+    backRight = Pose2d(Translation2d(-WHEEL_BASE_HALF, TRACK_WIDTH_HALF), Rotation2d.fromDegrees(90.0)),
     backLeft = Pose2d(Translation2d(-WHEEL_BASE_HALF, -TRACK_WIDTH_HALF), Rotation2d.fromDegrees(180.0))
 )
 

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.kt b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.kt
@@ -7,29 +7,27 @@ import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 
 
-
-
 interface GyroIO {
 
-    class GyroRawInputs : LoggableInputs{
+    class GyroRawInputs : LoggableInputs {
 
-          var rotationYaw: Double = 0.0
-          var rotationPitch: Double = 0.0
-          var rotationRoll: Double = 0.0
+        var rotationYaw: Double = 0.0
+        var rotationPitch: Double = 0.0
+        var rotationRoll: Double = 0.0
 
-          override fun toLog(table: LogTable?){
-              table?.put("RotationYaw", rotationYaw)
-              table?.put("RotationPitch", rotationPitch)
-              table?.put("RotationRoll", rotationRoll)
-          }
+        override fun toLog(table: LogTable?) {
+            table?.put("RotationYaw", rotationYaw)
+            table?.put("RotationPitch", rotationPitch)
+            table?.put("RotationRoll", rotationRoll)
+        }
 
-          override fun fromLog(table: LogTable?){
-              table?.getDouble("RotationYaw", rotationYaw)?.let { rotationYaw = it }
-              table?.getDouble("RotationPitch", rotationPitch)?.let {rotationPitch = it}
-              table?.getDouble("RotationRoll", rotationRoll)?.let {rotationRoll = it}
+        override fun fromLog(table: LogTable?) {
+            table?.getDouble("RotationYaw", rotationYaw)?.let { rotationYaw = it }
+            table?.getDouble("RotationPitch", rotationPitch)?.let { rotationPitch = it }
+            table?.getDouble("RotationRoll", rotationRoll)?.let { rotationRoll = it }
 
-          }
-     }
+        }
+    }
 
     fun updateInputs(inputs: GyroInputs)
 
@@ -38,9 +36,9 @@ interface GyroIO {
 
 class GyroInputs {
     private val rawInputs = GyroIO.GyroRawInputs()
-    var rotation: Rotation3d = Rotation3d(0.0,0.0,0.0)
+    var rotation: Rotation3d = Rotation3d(0.0, 0.0, 0.0)
 
-     fun updateRaw(){
+    fun updateRaw() {
         rawInputs.rotationRoll = rotation.x
         rawInputs.rotationPitch = rotation.y
         rawInputs.rotationYaw = rotation.z
@@ -49,13 +47,11 @@ class GyroInputs {
 }
 
 
-
-
-class NavXGyroIO(private var offset: Rotation3d = Rotation3d()) : GyroIO{
+class NavXGyroIO(private var offset: Rotation3d = Rotation3d()) : GyroIO {
 
     private val ahrs = AHRS()
 
-      override fun updateInputs(inputs: GyroInputs){
+    override fun updateInputs(inputs: GyroInputs) {
         inputs.rotation = offset.rotateBy(
             Rotation3d(
                 Quaternion(
@@ -69,7 +65,7 @@ class NavXGyroIO(private var offset: Rotation3d = Rotation3d()) : GyroIO{
         inputs.updateRaw()
     }
 
-    fun setRotation(rotation: Rotation3d){
+    fun setRotation(rotation: Rotation3d) {
         ahrs.reset()
         offset = rotation
     }
@@ -80,9 +76,9 @@ class NavXGyroIO(private var offset: Rotation3d = Rotation3d()) : GyroIO{
 
 }
 
-class SimGyroIO : GyroIO{
+class SimGyroIO : GyroIO {
 
-    override fun updateInputs(inputs: GyroInputs){
+    override fun updateInputs(inputs: GyroInputs) {
         inputs.rotation = Rotation3d()
         inputs.updateRaw()
     }

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.kt b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.kt
@@ -19,7 +19,7 @@ import org.littletonrobotics.junction.inputs.LoggableInputs
 
 
 interface ModuleIO {
-    class Inputs: LoggableInputs {
+    class Inputs : LoggableInputs {
 
         // The current "state" of the swerve module.
         //
@@ -33,7 +33,7 @@ interface ModuleIO {
         // The desired state of the module.
         //
         // This is the wheel velocity that we're trying to get to.
-        var desiredState =  SwerveModuleState()
+        var desiredState = SwerveModuleState()
 
         // The measured position of the module.
         //
@@ -53,7 +53,10 @@ interface ModuleIO {
             val angle = Rotation2d.fromRadians(table?.getDouble("Current Angle", 0.0)!!)
 
             state = SwerveModuleState(table.getDouble("Current Speed(Meters/Sec)", 0.0), angle)
-            desiredState = SwerveModuleState(table.getDouble("Target Speed (Meters / Sec", 0.0), Rotation2d.fromRadians(table.getDouble("Target Angle", 0.0)))
+            desiredState = SwerveModuleState(
+                table.getDouble("Target Speed (Meters / Sec", 0.0),
+                Rotation2d.fromRadians(table.getDouble("Target Angle", 0.0))
+            )
             position = SwerveModulePosition(table.getDouble("Distance Travelled", 0.0), angle)
         }
 
@@ -67,26 +70,27 @@ interface ModuleIO {
 class MAXSwerveModuleIO(drivingCAN: CANDevice, turningCAN: CANDevice, val chassisAngle: Rotation2d) : ModuleIO {
     private var desiredState: SwerveModuleState = SwerveModuleState(0.0, chassisAngle.unaryMinus())
 
-    override fun setDesiredState(state: SwerveModuleState){
+    override fun setDesiredState(state: SwerveModuleState) {
 
         //.minus(chassisAngle)
         val corrected = SwerveModuleState(state.speedMetersPerSecond, state.angle.minus(chassisAngle))
         // optimize the state to avoid rotating more than 90 degrees
         desiredState = SwerveModuleState.optimize(corrected, Rotation2d.fromRadians(turningEncoder.position))
-        
+
         drivingPIDController.setReference(desiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity)
         turningPIDController.setReference(desiredState.angle.radians, CANSparkMax.ControlType.kPosition)
     }
 
-    override fun updateInputs(inputs: ModuleIO.Inputs){
+    override fun updateInputs(inputs: ModuleIO.Inputs) {
 
         inputs.state = SwerveModuleState(
             drivingEncoder.velocity, Rotation2d.fromRadians(turningEncoder.position).plus(chassisAngle)
         )
 
         inputs.position = SwerveModulePosition(drivingEncoder.position, inputs.state.angle)
 
-        inputs.desiredState = SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle.plus(chassisAngle))
+        inputs.desiredState =
+            SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle.plus(chassisAngle))
     }
 
 
@@ -159,13 +163,13 @@ class MAXSwerveModuleIO(drivingCAN: CANDevice, turningCAN: CANDevice, val chassi
         // Bevel Pinion : Wheel Bevel Gear = 15 : 45
         val DRIVING_MOTOR_TO_WHEEL_GEARING = (DRIVING_MOTOR_PINION_TEETH.toDouble() / 22.0) * (15.0 / 45.0)
 
-      
+
         // take the known wheel diameter, divide it by two to get the radius, then get the circumference
         val WHEEL_RADIUS = Units.inchesToMeters(3.0) / 2
         val WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * TAU
 
-          // The free speed of the motor in Rotation2d per second
-          val DRIVING_WHEEL_FREE_SPEED = Rotation2d.fromRotations(5760.0 / 60.0).times(WHEEL_CIRCUMFERENCE)
+        // The free speed of the motor in Rotation2d per second
+        val DRIVING_WHEEL_FREE_SPEED = Rotation2d.fromRotations(5760.0 / 60.0).times(WHEEL_CIRCUMFERENCE)
 
 
         val DRIVING_PID_COEFFICIENTS = PIDCoefficients(p = 0.02)
@@ -178,7 +182,7 @@ class MAXSwerveModuleIO(drivingCAN: CANDevice, turningCAN: CANDevice, val chassi
     }
 }
 
-class SimSwerveModuleIO : ModuleIO{
+class SimSwerveModuleIO : ModuleIO {
 
     override fun updateInputs(inputs: ModuleIO.Inputs) {
         inputs.state = SwerveModuleState(
@@ -194,7 +198,6 @@ class SimSwerveModuleIO : ModuleIO{
         )
 
 
-
         //periodic
 
         // TODO: there should maybe be a proper dt here instead