Skip to content

Commit

Permalink
style: format code with Ktlint
Browse files Browse the repository at this point in the history
This commit fixes the style issues introduced in d98a47b according to the output
from Ktlint.

Details: #9
  • Loading branch information
deepsource-autofix[bot] authored Oct 27, 2024
1 parent d98a47b commit e387e33
Show file tree
Hide file tree
Showing 8 changed files with 52 additions and 55 deletions.
6 changes: 0 additions & 6 deletions src/main/java/frc/robot/Robot.kt
Original file line number Diff line number Diff line change
Expand Up @@ -3,26 +3,20 @@
// the WPILib BSD license file in the root directory of this project.
package frc.robot

import com.ctre.phoenix6.SignalLogger
import com.pathplanner.lib.commands.PathfindingCommand
import com.pathplanner.lib.pathfinding.Pathfinding
import edu.wpi.first.wpilibj.DriverStation
import edu.wpi.first.wpilibj.IterativeRobotBase
import edu.wpi.first.wpilibj.PowerDistribution
import edu.wpi.first.wpilibj.TimedRobot
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.CommandScheduler


class Robot : TimedRobot() {
private var m_autonomousCommand: Command? = null

private var m_robotContainer: RobotContainer? = null

override fun robotInit() {


// if (isReal()) {
PowerDistribution(1, PowerDistribution.ModuleType.kRev) // Enables power distribution logging
// }
Expand Down
30 changes: 17 additions & 13 deletions src/main/java/frc/robot/RobotContainer.kt
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,8 @@ class RobotContainer {
NamedCommands.registerCommands(
mapOf(
"extendArm" to Commands.run({ armSubsystem.extend(1.0) }, armSubsystem),
"retractArm" to Commands.run ({ armSubsystem.retract(1.0) }, armSubsystem),
)
"retractArm" to Commands.run({ armSubsystem.retract(1.0) }, armSubsystem),
),
)
}

Expand All @@ -82,23 +82,27 @@ class RobotContainer {

armSubsystem.defaultCommand = armSubsystem.getCommand {
MathUtil.applyDeadband(
joystick.rightTriggerAxis - joystick.leftTriggerAxis, 0.1
joystick.rightTriggerAxis - joystick.leftTriggerAxis, 0.1,
)
}


joystick.a().whileTrue(drivetrain.applyRequest { brake })
joystick.b().whileTrue(drivetrain.applyRequest {
point.withModuleDirection(
Rotation2d(
-joystick.leftY, -joystick.leftX
joystick.b().whileTrue(
drivetrain.applyRequest {
point.withModuleDirection(
Rotation2d(
-joystick.leftY,
-joystick.leftX,
),
)
)
})
},
)

joystick.pov(0).whileTrue(drivetrain.applyRequest { forwardStraight.withVelocityX(0.5).withVelocityY(0.0) }
joystick.pov(0).whileTrue(
drivetrain.applyRequest { forwardStraight.withVelocityX(0.5).withVelocityY(0.0) },
)
joystick.pov(180).whileTrue(drivetrain.applyRequest { forwardStraight.withVelocityX(-0.5).withVelocityY(0.0) }
joystick.pov(180).whileTrue(
drivetrain.applyRequest { forwardStraight.withVelocityX(-0.5).withVelocityY(0.0) },
)

// Run SysId routines when holding back/start and X/Y.
Expand All @@ -121,6 +125,6 @@ class RobotContainer {
}

val autonomousCommand: Command
get() =/* First put the drivetrain into auto run mode, then run the auto */
get() = /* First put the drivetrain into auto run mode, then run the auto */
autoChooser.selected
}
1 change: 0 additions & 1 deletion src/main/java/frc/robot/Util/SwerveVoltageRequest.kt
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters
import com.ctre.phoenix6.swerve.SwerveModule
import com.ctre.phoenix6.swerve.SwerveRequest


class SwerveVoltageRequest : SwerveRequest {
private val m_motionMagicControl: MotionMagicVoltage = MotionMagicVoltage(0.0, false, 0.0, 0, false, false, false, false)
private val m_voltageOutControl: VoltageOut = VoltageOut(0.0)
Expand Down
14 changes: 8 additions & 6 deletions src/main/java/frc/robot/commands/GoToNoteCommand.kt
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,17 @@ class GoToNoteCommand(
override fun execute() {
try {
commandSwerveDrivetrain.setControl(
drive.withVelocityX(-drivePid.calculate(visionSubsystem.getNoteCamPitch() * TunerConstants.kSpeedAt12VoltsMps.`in`(
Units.MetersPerSecond)))
drive.withVelocityX(
-drivePid.calculate(
visionSubsystem.getNoteCamPitch() * TunerConstants.kSpeedAt12VoltsMps.`in`(
Units.MetersPerSecond,
),
),
)
.withVelocityY(0.0).withRotationalDeadband(0.1).withDeadband(1.0)
.withRotationalRate(anglePid.calculate(visionSubsystem.getNoteCamYaw()) * 2 * PI)
.withRotationalRate(anglePid.calculate(visionSubsystem.getNoteCamYaw()) * 2 * PI),
)
} finally {}


}

override fun isFinished(): Boolean {
Expand All @@ -57,6 +60,5 @@ class GoToNoteCommand(
}

override fun end(interrupted: Boolean) {

}
}
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/commands/PutArmDownCommand.kt
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,9 @@ class PutArmDownCommand(private val armSubsystem: ArmSubsystem) : Command() {

override fun execute() {
armSubsystem.extend(1.0)
if (SmartDashboard.getNumber("Arm Max Current Limit", maxLimit) != maxLimit)
if (SmartDashboard.getNumber("Arm Max Current Limit", maxLimit) != maxLimit) {
maxLimit = SmartDashboard.getNumber("Arm Max Current Limit", maxLimit)
}
}

override fun isFinished(): Boolean {
Expand Down
11 changes: 8 additions & 3 deletions src/main/java/frc/robot/generated/TunerConstants.kt
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,10 @@ object TunerConstants {
CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low
// stator current limit to help avoid brownouts without impacting performance.
.withStatorCurrentLimit(Amps.of(60.0))
.withStatorCurrentLimitEnable(true)
.withStatorCurrentLimitEnable(true),
)
val cancoderInitialConfigs: CANcoderConfiguration = CANcoderConfiguration()

// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
val pigeonConfigs: Pigeon2Configuration? = null

Expand All @@ -70,7 +71,7 @@ object TunerConstants {

private const val kDriveGearRatio = 6.746031746031747
private const val kSteerGearRatio = 21.428571428571427
private val kWheelRadius: Distance = edu.wpi.first.units.Units.Inches.of( 4.0)
private val kWheelRadius: Distance = edu.wpi.first.units.Units.Inches.of(4.0)

private const val kInvertLeftSide = false
private const val kInvertRightSide = true
Expand Down Expand Up @@ -206,7 +207,11 @@ object TunerConstants {
*/
fun createDrivetrain(): CommandSwerveDrivetrain {
return CommandSwerveDrivetrain(
DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
DrivetrainConstants,
FrontLeft,
FrontRight,
BackLeft,
BackRight,
)
}
}
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/subsystems/ArmSubsystem.kt
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
import edu.wpi.first.math.controller.ArmFeedforward
import edu.wpi.first.math.controller.PIDController
import edu.wpi.first.networktables.GenericEntry
import edu.wpi.first.networktables.NetworkTableEntry
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import edu.wpi.first.wpilibj2.command.Command
Expand Down Expand Up @@ -80,5 +79,4 @@ class ArmSubsystem : Subsystem {

fun getSupplyCurrent(): Double = motor.supplyCurrent
fun getOutputCurrent(): Double = motor.statorCurrent

}
40 changes: 17 additions & 23 deletions src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,7 @@ package frc.robot.subsystems.vision

import edu.wpi.first.apriltag.AprilTagFieldLayout
import edu.wpi.first.apriltag.AprilTagFields
import edu.wpi.first.math.Matrix
import edu.wpi.first.math.VecBuilder
import edu.wpi.first.math.geometry.*
import edu.wpi.first.math.numbers.N1
import edu.wpi.first.math.numbers.N3
import edu.wpi.first.units.Units
import edu.wpi.first.wpilibj.DriverStation.Alliance
import edu.wpi.first.wpilibj.RobotBase
Expand All @@ -22,19 +18,18 @@ import java.util.*
import kotlin.jvm.optionals.getOrNull
import kotlin.math.abs


object VisionSubsystem : SubsystemBase() {
var alliance: Alliance? = null
private val leftCam = PhotonCamera("Left Camera")
private val rightCam = PhotonCamera("Right Camera")
private val noteCam = PhotonCamera("ShitCam")
private val botToLeftCamera = Transform3d(
Translation3d(-2.25, 11.75, 7.5),
Rotation3d(Units.Degree.of(10.0).`in`(Units.Radian), 0.0, 0.0)
Rotation3d(Units.Degree.of(10.0).`in`(Units.Radian), 0.0, 0.0),
) // TODO get this value
private val botToRightCamera = Transform3d(
Translation3d(2.25, -11.75, 7.5),
Rotation3d(Units.Degree.of(-10.0).`in`(Units.Radian), 0.0, 0.0)
Rotation3d(Units.Degree.of(-10.0).`in`(Units.Radian), 0.0, 0.0),
) // TODO get this value
private var enable = false
private var trust = false
Expand All @@ -47,7 +42,6 @@ object VisionSubsystem : SubsystemBase() {
private var leftPhotonPoseEstimator = PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, leftCam, botToLeftCamera)
private val isSim = RobotBase.isSimulation()


/** Creates a new Limelight. */
init {
rightPhotonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY)
Expand All @@ -57,22 +51,18 @@ object VisionSubsystem : SubsystemBase() {
SmartDashboard.putNumber("Limelight Error", distanceError.toDouble())
}




/**
* Calculates standard deviations for the right camera based on the number of targets detected. Heuristic.
*/
private fun updateLeftEstimationStdDevs(
visionEst: Optional<EstimatedRobotPose>,
targets: List<PhotonTrackedTarget>) {

targets: List<PhotonTrackedTarget>,
) {
}

// singleton class
val instance: VisionSubsystem = VisionSubsystem


fun useLimelight(enable: Boolean) {
this.enable = enable
}
Expand All @@ -82,27 +72,33 @@ object VisionSubsystem : SubsystemBase() {
}

override fun periodic() {

}

fun getNoteCamYaw(): Double {
try {
if (!isSim) {
return if (noteCam.getLatestResult().getBestTarget() != null) noteCam.getLatestResult()
.getBestTarget().yaw else 0.0
return if (noteCam.getLatestResult().getBestTarget() != null) {
noteCam.getLatestResult()
.getBestTarget().yaw
} else {
0.0
}
} else {
return 0.0
}
} finally {

}
}

fun getNoteCamPitch(): Double {
try {
if (!isSim) {
return if (noteCam.getLatestResult().getBestTarget() != null) noteCam.getLatestResult()
.getBestTarget().pitch else 0.0
return if (noteCam.getLatestResult().getBestTarget() != null) {
noteCam.getLatestResult()
.getBestTarget().pitch
} else {
0.0
}
} else {
return 0.0
}
Expand All @@ -126,7 +122,7 @@ object VisionSubsystem : SubsystemBase() {
}
}

private val field = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02))
private val field = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02))

@JvmStatic
fun updateLeft(): EstimatedRobotPose? {
Expand All @@ -145,6 +141,4 @@ object VisionSubsystem : SubsystemBase() {
fun getRightResult(): PhotonPipelineResult? {
return rightCam.latestResult
}


}

0 comments on commit e387e33

Please sign in to comment.