Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

style: format code with Ktlint #12

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

style: format code with Ktlint #12

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
110 changes: 61 additions & 49 deletions src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,10 @@
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.kinematics.SwerveDriveOdometry
import edu.wpi.first.math.numbers.N1
import edu.wpi.first.math.numbers.N3
import edu.wpi.first.units.Units
Expand All @@ -34,8 +32,8 @@
import edu.wpi.first.wpilibj2.command.Subsystem
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
import frc.robot.util.RectanglePoseArea
import frc.robot.subsystems.vision.VisionSubsystem
import frc.robot.util.RectanglePoseArea
import java.util.function.DoubleSupplier
import java.util.function.Supplier
import kotlin.math.PI
Expand All @@ -50,7 +48,6 @@

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 @@ -65,28 +62,33 @@
/* 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

Check warning

Code scanning / detekt

Report magic numbers. Magic number is a numeric literal that is not defined as a constant and hence it's unclear what the purpose of this number is. It's better to declare such numbers as constants and give them a proper name. By default, -1, 0, 1, and 2 are not considered to be magic numbers. Warning

This expression contains a magic number. Consider defining it to a well named constant.
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

Check warning

Code scanning / detekt

Report magic numbers. Magic number is a numeric literal that is not defined as a constant and hence it's unclear what the purpose of this number is. It's better to declare such numbers as constants and give them a proper name. By default, -1, 0, 1, and 2 are not considered to be magic numbers. Warning

This expression contains a magic number. Consider defining it to a well named constant.
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 @@ -97,18 +99,21 @@
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 */

Check warning

Code scanning / detekt

Line detected that is longer than the defined maximum line length in the code style. Warning

Line detected that is longer than the defined maximum line length in the code style.
SignalLogger.writeDouble("Rotational_Rate", output.`in`(Units.Volts))
}, null, this
)
},
null,
this,
),
)

/* The SysId routine to test */
Expand All @@ -126,7 +131,8 @@
* @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 @@ -176,11 +182,17 @@
* @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 @@ -192,24 +204,24 @@
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

Check warning

Code scanning / detekt

Report magic numbers. Magic number is a numeric literal that is not defined as a constant and hence it's unclear what the purpose of this number is. It's better to declare such numbers as constants and give them a proper name. By default, -1, 0, 1, and 2 are not considered to be magic numbers. Warning

This expression contains a magic number. Consider defining it to a well named constant.
PIDConstants(5.0, 0.0, 0.0), // PID constants for rotation

Check warning

Code scanning / detekt

Report magic numbers. Magic number is a numeric literal that is not defined as a constant and hence it's unclear what the purpose of this number is. It's better to declare such numbers as constants and give them a proper name. By default, -1, 0, 1, and 2 are not considered to be magic numbers. Warning

This expression contains a magic number. Consider defining it to a well named constant.
),
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 @@ -254,7 +266,8 @@

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 @@ -264,8 +277,11 @@
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
}
Expand All @@ -277,19 +293,17 @@
if (left !== null) {
var dist = VisionSubsystem.instance.getLeftResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d())
var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range)
if(field.isPoseWithinArea(left.estimatedPose.toPose2d())) {
if (field.isPoseWithinArea(left.estimatedPose.toPose2d())) {
addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta]
}
}
if (right !== null) {
var dist = VisionSubsystem.instance.getRightResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d())
var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range)
if(field.isPoseWithinArea(right.estimatedPose.toPose2d())) {
if (field.isPoseWithinArea(right.estimatedPose.toPose2d())) {
addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta]
}
}


}

private fun startSimThread() {
Expand Down Expand Up @@ -329,8 +343,6 @@
.withRotationalDeadband(0.0).withRotationalRate(thetaController.calculate(angle.asDouble) * 2 * PI)
.withVelocityX(0.0).withVelocityY(0.0)
}.ignoringDisable(true).addRequirements(this)


}
}
}
Loading