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

Details: None
  • Loading branch information
deepsource-autofix[bot] authored and TheGamer1002 committed Oct 25, 2024
1 parent 0201123 commit ec8f8e2
Show file tree
Hide file tree
Showing 7 changed files with 244 additions and 204 deletions.
12 changes: 4 additions & 8 deletions src/main/java/frc/robot/Robot.kt
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,12 @@ import org.littletonrobotics.junction.networktables.NT4Publisher
import org.littletonrobotics.junction.wpilog.WPILOGReader
import org.littletonrobotics.junction.wpilog.WPILOGWriter


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

private var m_robotContainer: RobotContainer? = null

override fun robotInit() {

Pathfinding.setPathfinder(LocalADStarAK())

Logger.recordMetadata("ProjectName", "FRC2024-offseason") // Set a metadata value
Expand All @@ -42,24 +40,22 @@ class Robot : LoggedRobot() {
WPILOGWriter(
LogFileUtil.addPathSuffix(
logPath,
"_sim"
)
)
"_sim",
),
),
) // Save outputs to a new log
}


// Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the "Understanding Data Flow" page
Logger.start() // Start logging! No more data receivers, replay sources, or metadata values may be added.
m_robotContainer = RobotContainer()

m_robotContainer!!.drivetrain.daqThread.setThreadPriority(99)


DriverStation.silenceJoystickConnectionWarning(true)
SignalLogger.start()

PathfindingCommand.warmupCommand().schedule();
PathfindingCommand.warmupCommand().schedule()
}

override fun robotPeriodic() {
Expand Down
20 changes: 7 additions & 13 deletions src/main/java/frc/robot/RobotContainer.kt
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@ import edu.wpi.first.wpilibj2.command.Commands
import edu.wpi.first.wpilibj2.command.button.CommandXboxController
import edu.wpi.first.wpilibj2.command.button.Trigger
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
import frc.robot.subsystems.vision.VisionSubsystem
import frc.robot.generated.TunerConstants
import frc.robot.subsystems.ArmSubsystem
import frc.robot.subsystems.drive.CommandSwerveDrivetrain
import frc.robot.subsystems.vision.VisionSubsystem
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser
import java.util.function.Supplier
import kotlin.math.atan2
Expand All @@ -48,7 +48,6 @@ class RobotContainer {
val armSubsystem: ArmSubsystem = ArmSubsystem
val visionSubsystem: VisionSubsystem = VisionSubsystem(drivetrain)


// Slew Rate Limiters to limit acceleration of joystick inputs
private val xLimiter: SlewRateLimiter = SlewRateLimiter(2.0)
private val yLimiter: SlewRateLimiter = SlewRateLimiter(0.5)
Expand Down Expand Up @@ -89,10 +88,10 @@ class RobotContainer {
point.withModuleDirection(
Rotation2d(
-drv.leftY,
-drv.leftX
)
-drv.leftX,
),
)
}
},
)

// reset the field-centric heading on start button press
Expand All @@ -103,16 +102,15 @@ class RobotContainer {
Commands.runOnce({
MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * TurtleSpeed
})
.andThen({ AngularRate = TurtleAngularRate })
.andThen({ AngularRate = TurtleAngularRate }),
)
drv.leftBumper().onFalse(
Commands.runOnce({
MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * speedChooser.get()
})
.andThen({ AngularRate = MaxAngularRate })
.andThen({ AngularRate = MaxAngularRate }),
)


if (Utils.isSimulation()) {
drivetrain.seedFieldRelative(Pose2d(Translation2d(), Rotation2d.fromDegrees(0.0)))
}
Expand All @@ -139,10 +137,8 @@ class RobotContainer {
// Drivetrain needs to be placed against a sturdy wall and test stopped immediately upon wheel slip
drv.back().and(drv.pov(0)).whileTrue(drivetrain.runDriveSlipTest())


drv.rightBumper().whileTrue(drivetrain.rotateToAngleDiff(visionSubsystem.getNoteCamAngle()))
/* TODO make sure this works and probably also see if we can't get it to also move the robot forward a bit to center it w/ the arm */

}

init {
Expand Down Expand Up @@ -174,7 +170,7 @@ class RobotContainer {
}

val autoCommand: 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.get()

private fun newControlStyle() {
Expand Down Expand Up @@ -222,6 +218,4 @@ class RobotContainer {
private fun conditionX(joystick: Double, deadband: Double): Double {
return xLimiter.calculate(MathUtil.applyDeadband(joystick, deadband))
}


}
8 changes: 0 additions & 8 deletions src/main/java/frc/robot/subsystems/ArmSubsystem.kt
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +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.wpilibj.shuffleboard.ComplexWidget
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard

import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.Subsystem

Expand All @@ -21,8 +15,6 @@ object ArmSubsystem : Subsystem {
var config = TalonSRXConfiguration()
var ff: ArmFeedforward



init {
// Initialize the arm subsystem
motor = TalonSRX(31)
Expand Down
76 changes: 45 additions & 31 deletions src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt
Original file line number Diff line number Diff line change
Expand Up @@ -90,16 +90,14 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
}

private fun configurePathPlanner() {


AutoBuilder.configureHolonomic(
{ this.state.Pose }, // Supplier of current robot pose
this::seedFieldRelative, // Consumer for seeding pose against auto
{ this.state.Pose }, // Supplier of current robot pose
this::seedFieldRelative, // Consumer for seeding pose against auto
{ this.currentRobotChassisSpeeds },
{ speeds -> this.setControl(autoRequest.withSpeeds(speeds)) }, // Consumer of ChassisSpeeds to drive the robot
{ speeds -> this.setControl(autoRequest.withSpeeds(speeds)) }, // Consumer of ChassisSpeeds to drive the robot
holonomicPathFollowerConfig(),
getAlliance(),
this
this,
) // Subsystem for requirements
}

Expand All @@ -108,7 +106,7 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
PIDConstants(10.0, 0.0, 0.0),
TunerConstants.kSpeedAt12VoltsMps,
driveBaseRadius,
ReplanningConfig()
ReplanningConfig(),
)

fun applyRequest(requestSupplier: Supplier<SwerveRequest?>): Command {
Expand Down Expand Up @@ -172,9 +170,11 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
}

private fun getHolonomicDriveController() = PPHolonomicDriveController(
PIDConstants(10.0, 0.0, 0.0), PIDConstants(10.0, 0.0, 0.0),
PIDConstants(10.0, 0.0, 0.0),
PIDConstants(10.0, 0.0, 0.0),
// max module speed is 9.46 m/s, convert to ft/s
FeetPerSecond.of(9.46).`in`(MetersPerSecond), driveBaseRadius
FeetPerSecond.of(9.46).`in`(MetersPerSecond),
driveBaseRadius,

)

Expand All @@ -191,7 +191,8 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {

private fun getReplanningConfig() = ReplanningConfig(true, true)

override fun simulationPeriodic() {/* Assume 20ms update rate, get battery voltage from WPILib */
override fun simulationPeriodic() {
/* Assume 20ms update rate, get battery voltage from WPILib */
updateSimState(0.02, RobotController.getBatteryVoltage())
}

Expand All @@ -202,33 +203,39 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
private val driveVoltageRequest: SwerveVoltageRequest = SwerveVoltageRequest(true)

private val m_driveSysIdRoutine: SysIdRoutine = SysIdRoutine(
SysIdRoutine.Config(null, null, null, ModifiedSignalLogger.logState()), SysIdRoutine.Mechanism(
SysIdRoutine.Config(null, null, null, ModifiedSignalLogger.logState()),
SysIdRoutine.Mechanism(
{ volts: Measure<Voltage?> ->
setControl(
driveVoltageRequest.withVoltage(
volts.`in`(
Volts
)
)
Volts,
),
),
)
}, null, this
)
},
null,
this,
),
)

private val steerVoltageRequest: SwerveVoltageRequest = SwerveVoltageRequest(false)

private val m_steerSysIdRoutine: SysIdRoutine = SysIdRoutine(
SysIdRoutine.Config(null, null, null, ModifiedSignalLogger.logState()), SysIdRoutine.Mechanism(
SysIdRoutine.Config(null, null, null, ModifiedSignalLogger.logState()),
SysIdRoutine.Mechanism(
{ volts: Measure<Voltage?> ->
setControl(
steerVoltageRequest.withVoltage(
volts.`in`(
Volts
)
)
Volts,
),
),
)
}, null, this
)
},
null,
this,
),
)
override fun periodic() {
/* Periodically try to apply the operator perspective */
Expand All @@ -239,10 +246,11 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent { allianceColor: Alliance ->
this.setOperatorPerspectiveForward(
if (allianceColor == Alliance.Red)
if (allianceColor == Alliance.Red) {
redAlliancePerspectiveRotation
else
} else {
blueAlliancePerspectiveRotation
},
)
hasAppliedOperatorPerspective = true
}
Expand All @@ -251,18 +259,24 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {

private val m_slipSysIdRoutine: SysIdRoutine = SysIdRoutine(
SysIdRoutine.Config(
Volts.of(0.25).per(Seconds.of(1.0)), null, null, ModifiedSignalLogger.logState()
), SysIdRoutine.Mechanism(
Volts.of(0.25).per(Seconds.of(1.0)),
null,
null,
ModifiedSignalLogger.logState(),
),
SysIdRoutine.Mechanism(
{ volts: Measure<Voltage?> ->
setControl(
driveVoltageRequest.withVoltage(
volts.`in`(
Volts
)
)
Volts,
),
),
)
}, null, this
)
},
null,
this,
),
)

fun runDriveQuasiTest(direction: SysIdRoutine.Direction?): Command {
Expand Down
5 changes: 1 addition & 4 deletions src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt
Original file line number Diff line number Diff line change
Expand Up @@ -32,18 +32,15 @@ class VisionSubsystem(var drivetrain: CommandSwerveDrivetrain) : SubsystemBase()
init {
SmartDashboard.putNumber("Field Error", fieldError.toDouble())
SmartDashboard.putNumber("Limelight Error", distanceError.toDouble())

}


fun updateLeft(): EstimatedRobotPose? {
return leftPhotonPoseEstimator.update().getOrNull()
}
fun updateRight(): EstimatedRobotPose? {
return rightPhotonPoseEstimator.update().getOrNull()
}


fun useLimelight(enable: Boolean) {
this.enable = enable
}
Expand All @@ -70,4 +67,4 @@ class VisionSubsystem(var drivetrain: CommandSwerveDrivetrain) : SubsystemBase()
companion object {
private val field = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02))
}
}
}
Loading

0 comments on commit ec8f8e2

Please sign in to comment.