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 050fbb2 according to the output
from Ktlint.

Details: None
  • Loading branch information
@deepsource-autofix @TheGamer1002
deepsource-autofix[bot] authored and TheGamer1002 committed Nov 13, 2024
1 parent 050fbb2 commit e63166c
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 77 deletions.
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@ class Robot : TimedRobot() {

private var m_robotContainer: RobotContainer? = null



init {
// if (isReal()) {
PowerDistribution(1, PowerDistribution.ModuleType.kRev) // Enables power distribution logging
Expand Down Expand Up @@ -93,7 +91,7 @@ class Robot : TimedRobot() {

companion object {
fun sim(): Boolean {
return isSimulation()
return isSimulation()
}
}
}
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,6 @@ class RobotContainer {
// reset the field-centric heading on left bumper press
joystick.leftBumper().onTrue(drivetrain.runOnce { drivetrain::resetPose })


drivetrain.registerTelemetry { state: SwerveDriveState? ->
if (state != null) {
logger.telemeterize(state)
Expand All @@ -125,11 +124,10 @@ class RobotContainer {
get() = /* First put the drivetrain into auto run mode, then run the auto */
autoChooser.selected


fun updatePoseEstimation() {
var visionEst = visionSubsystem.leftEstimatedGlobalPose
visionEst.ifPresent {
est ->
est ->
run {
var leftEstStdDevs = visionSubsystem.leftEstimationStdDevs

Expand Down
108 changes: 58 additions & 50 deletions src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,9 @@ import com.pathplanner.lib.config.RobotConfig
import com.pathplanner.lib.controllers.PPHolonomicDriveController
import com.pathplanner.lib.util.DriveFeedforwards
import edu.wpi.first.math.Matrix
import edu.wpi.first.math.VecBuilder
import edu.wpi.first.math.controller.PIDController
import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.geometry.Translation2d
import edu.wpi.first.math.geometry.Translation3d
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.math.numbers.N1
import edu.wpi.first.math.numbers.N3
Expand All @@ -28,8 +25,6 @@ import edu.wpi.first.wpilibj.DriverStation
import edu.wpi.first.wpilibj.DriverStation.Alliance
import edu.wpi.first.wpilibj.Notifier
import edu.wpi.first.wpilibj.RobotController
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
import edu.wpi.first.wpilibj.smartdashboard.Field2d
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog
import edu.wpi.first.wpilibj2.command.Command
Expand All @@ -51,7 +46,6 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {

private var thetaController: PIDController = PIDController(1.1, 0.0, 0.125) // TODO tune this


/* Keep track if we've ever applied the operator perspective before or not */
private var m_hasAppliedOperatorPerspective = false

Expand All @@ -66,28 +60,33 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private val m_sysIdRoutineTranslation = SysIdRoutine(
SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Units.Volts.of(4.0), // Reduce dynamic step voltage to 4 V to prevent brownout
null
) // Use default timeout (10 s)
// Log state with SignalLogger class
{ state: SysIdRoutineLog.State -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) },
null, // Use default ramp rate (1 V/s)
Units.Volts.of(4.0), // Reduce dynamic step voltage to 4 V to prevent brownout
null,
) // Use default timeout (10 s)
// Log state with SignalLogger class
{ state: SysIdRoutineLog.State -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) },
Mechanism(
{ output: Voltage? -> setControl(m_translationCharacterization.withVolts(output)) }, null, this
)
{ output: Voltage? -> setControl(m_translationCharacterization.withVolts(output)) },
null,
this,
),
)

/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
private val m_sysIdRoutineSteer = SysIdRoutine(
SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Units.Volts.of(7.0), // Use dynamic voltage of 7 V
null
) // Use default timeout (10 s)
// Log state with SignalLogger class
{ state: SysIdRoutineLog.State -> SignalLogger.writeString("SysIdSteer_State", state.toString()) }, Mechanism(
{ volts: Voltage? -> setControl(m_steerCharacterization.withVolts(volts)) }, null, this
)
null, // Use default ramp rate (1 V/s)
Units.Volts.of(7.0), // Use dynamic voltage of 7 V
null,
) // Use default timeout (10 s)
// Log state with SignalLogger class
{ state: SysIdRoutineLog.State -> SignalLogger.writeString("SysIdSteer_State", state.toString()) },
Mechanism(
{ volts: Voltage? -> setControl(m_steerCharacterization.withVolts(volts)) },
null,
this,
),
)

/*
Expand All @@ -98,18 +97,21 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
private val m_sysIdRoutineRotation = SysIdRoutine(
SysIdRoutine.Config( /* This is in radians per second², but SysId only supports "volts per second" */
Units.Volts.of(Math.PI / 6)
.per(Units.Second), /* This is in radians per second, but SysId only supports "volts" */
Units.Volts.of(Math.PI), null
) // Use default timeout (10 s)
// Log state with SignalLogger class
{ state: SysIdRoutineLog.State -> SignalLogger.writeString("SysIdRotation_State", state.toString()) },
.per(Units.Second), /* This is in radians per second, but SysId only supports "volts" */
Units.Volts.of(Math.PI),
null,
) // Use default timeout (10 s)
// Log state with SignalLogger class
{ state: SysIdRoutineLog.State -> SignalLogger.writeString("SysIdRotation_State", state.toString()) },
Mechanism(
{ output: Voltage ->
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.`in`(Units.Volts)))/* also log the requested output for SysId */
setControl(m_rotationCharacterization.withRotationalRate(output.`in`(Units.Volts))) /* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.`in`(Units.Volts))
}, null, this
)
},
null,
this,
),
)

/* The SysId routine to test */
Expand All @@ -127,7 +129,8 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
* @param modules Constants for each specific module
*/
constructor(drivetrainConstants: SwerveDrivetrainConstants, vararg modules: SwerveModuleConstants?) : super(
drivetrainConstants, *modules
drivetrainConstants,
*modules,
) {
if (Utils.isSimulation()) {
startSimThread()
Expand Down Expand Up @@ -177,11 +180,17 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
* @param modules Constants for each specific module
*/
constructor(
drivetrainConstants: SwerveDrivetrainConstants, odometryUpdateFrequency: Double,
odometryStandardDeviation: Matrix<N3?, N1?>, visionStandardDeviation: Matrix<N3?, N1?>,
drivetrainConstants: SwerveDrivetrainConstants,
odometryUpdateFrequency: Double,
odometryStandardDeviation: Matrix<N3?, N1?>,
visionStandardDeviation: Matrix<N3?, N1?>,
vararg modules: SwerveModuleConstants?,
) : super(
drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, *modules
drivetrainConstants,
odometryUpdateFrequency,
odometryStandardDeviation,
visionStandardDeviation,
*modules,
) {
if (Utils.isSimulation()) {
startSimThread()
Expand All @@ -193,24 +202,24 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
try {
val config = RobotConfig.fromGUISettings()
AutoBuilder.configure(
{ state.Pose }, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
{ state.Speeds }, // Supplier of current robot speeds
{ state.Pose }, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
{ state.Speeds }, // Supplier of current robot speeds
// Consumer of ChassisSpeeds and feedforwards to drive the robot
{ speeds: ChassisSpeeds?, feedforwards: DriveFeedforwards ->
setControl(
m_applyRobotSpeeds.withSpeeds(speeds)
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()),
)
},
PPHolonomicDriveController(
PIDConstants(0.5, 0.0, 0.0), // PID constants for translation
PIDConstants(5.0, 0.0, 0.0) // PID constants for rotation
PIDConstants(0.5, 0.0, 0.0), // PID constants for translation
PIDConstants(5.0, 0.0, 0.0), // PID constants for rotation
),
config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case
config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case
{ DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red },
this // Subsystem for requirements
this, // Subsystem for requirements
)
} catch (ex: Exception) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.stackTrace)
Expand Down Expand Up @@ -266,7 +275,8 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {

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

override fun periodic() {/*
override fun periodic() {
/*
* Periodically try to apply the operator perspective.
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
* This allows us to correct the perspective in case the robot code restarts mid-match.
Expand All @@ -276,14 +286,16 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent { allianceColor: Alliance ->
setOperatorPerspectiveForward(
if (allianceColor == Alliance.Red) kRedAlliancePerspectiveRotation
else kBlueAlliancePerspectiveRotation
if (allianceColor == Alliance.Red) {
kRedAlliancePerspectiveRotation
} else {
kBlueAlliancePerspectiveRotation
},
)
m_hasAppliedOperatorPerspective = true
}
}


// if (VisionSubsystem.instance.getLeftResult().hasTargets()) {
// var left = VisionSubsystem.instance.updateLeft()
// var dist = VisionSubsystem.instance.getLeftResult().bestTarget.bestCameraToTarget.translation.getDistance(Translation3d())
Expand Down Expand Up @@ -316,8 +328,6 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
// }
// }
// }


}

// fun addVisionMeasurement(pose: Pose2d, timestamp: Double, stdDevs: Unit) {
Expand Down Expand Up @@ -361,8 +371,6 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
.withRotationalDeadband(0.0).withRotationalRate(thetaController.calculate(angle.asDouble) * 2 * PI)
.withVelocityX(0.0).withVelocityY(0.0)
}.ignoringDisable(true).addRequirements(this)


}
}
}
50 changes: 29 additions & 21 deletions src/main/java/frc/robot/subsystems/vision/BetterVisionSubsystem.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ import org.photonvision.simulation.VisionSystemSim
import org.photonvision.targeting.PhotonTrackedTarget
import java.util.*

class BetterVisionSubsystem: SubsystemBase() {
class BetterVisionSubsystem : SubsystemBase() {
private val leftCamera = PhotonCamera("Left Camera")
private val rightCamera = PhotonCamera("Right Camera")

Expand All @@ -70,6 +70,7 @@ class BetterVisionSubsystem: SubsystemBase() {
*/
var leftEstimationStdDevs: Matrix<N3, N1>? = null
private set

/**
* Returns the latest standard deviations of the estimated pose from [ ][.getEstimatedGlobalPose], for use with [ ]. This should
* only be used when there are targets visible.
Expand Down Expand Up @@ -141,7 +142,8 @@ class BetterVisionSubsystem: SubsystemBase() {
},
{
leftSimDebugField!!.getObject("VisionEstimation").setPoses()
})
},
)
}
}
return visionEst
Expand Down Expand Up @@ -172,7 +174,8 @@ class BetterVisionSubsystem: SubsystemBase() {
},
{
rightSimDebugField!!.getObject("VisionEstimation").setPoses()
})
},
)
}
}
return visionEst
Expand All @@ -186,7 +189,8 @@ class BetterVisionSubsystem: SubsystemBase() {
* @param targets All targets in this camera frame
*/
private fun updateLeftEstimationStdDevs(
estimatedPose: Optional<EstimatedRobotPose>, targets: List<PhotonTrackedTarget>,
estimatedPose: Optional<EstimatedRobotPose>,
targets: List<PhotonTrackedTarget>,
) {
if (estimatedPose.isEmpty) {
// No pose input. Default to single-tag std devs
Expand Down Expand Up @@ -219,12 +223,15 @@ class BetterVisionSubsystem: SubsystemBase() {
// Decrease std devs if multiple targets are visible
if (numTags > 1) estStdDevs = kMultiTagStdDevs
// Increase std devs based on (average) distance
estStdDevs = if (numTags == 1 && avgDist > 4) VecBuilder.fill(
Double.MAX_VALUE,
Double.MAX_VALUE,
Double.MAX_VALUE
)
else estStdDevs.times(1 + (avgDist * avgDist / 30))
estStdDevs = if (numTags == 1 && avgDist > 4) {
VecBuilder.fill(
Double.MAX_VALUE,
Double.MAX_VALUE,
Double.MAX_VALUE,
)
} else {
estStdDevs.times(1 + (avgDist * avgDist / 30))
}
leftEstimationStdDevs = estStdDevs
}
}
Expand All @@ -238,7 +245,8 @@ class BetterVisionSubsystem: SubsystemBase() {
* @param targets All targets in this camera frame
*/
private fun updateRightEstimationStdDevs(
estimatedPose: Optional<EstimatedRobotPose>, targets: List<PhotonTrackedTarget>,
estimatedPose: Optional<EstimatedRobotPose>,
targets: List<PhotonTrackedTarget>,
) {
if (estimatedPose.isEmpty) {
// No pose input. Default to single-tag std devs
Expand Down Expand Up @@ -271,12 +279,15 @@ class BetterVisionSubsystem: SubsystemBase() {
// Decrease std devs if multiple targets are visible
if (numTags > 1) estStdDevs = kMultiTagStdDevs
// Increase std devs based on (average) distance
estStdDevs = if (numTags == 1 && avgDist > 4) VecBuilder.fill(
Double.MAX_VALUE,
Double.MAX_VALUE,
Double.MAX_VALUE
)
else estStdDevs.times(1 + (avgDist * avgDist / 30))
estStdDevs = if (numTags == 1 && avgDist > 4) {
VecBuilder.fill(
Double.MAX_VALUE,
Double.MAX_VALUE,
Double.MAX_VALUE,
)
} else {
estStdDevs.times(1 + (avgDist * avgDist / 30))
}
rightEstimationStdDevs = estStdDevs
}
}
Expand Down Expand Up @@ -306,7 +317,4 @@ class BetterVisionSubsystem: SubsystemBase() {
if (!Robot.sim()) return null
return rightVisionSim!!.debugField
}



}
}

0 comments on commit e63166c

Please sign in to comment.