diff --git a/src/main/kotlin/frc/team449/ControllerBindings.kt b/src/main/kotlin/frc/team449/ControllerBindings.kt index ce8c3cb..71ee39e 100644 --- a/src/main/kotlin/frc/team449/ControllerBindings.kt +++ b/src/main/kotlin/frc/team449/ControllerBindings.kt @@ -46,7 +46,7 @@ class ControllerBindings( ) println("drive configured") - //temp + // temp // reef location passed in alla webappp, this is temp robot.driveController.x().onTrue( @@ -63,7 +63,7 @@ class ControllerBindings( autoscore.currentCommand.schedule() }) ) - //on red alliance side passed in by webapp, this is temp + // on red alliance side passed in by webapp, this is temp robot.driveController.b().onTrue( runOnce({ autoscore.currentCommand = autoscore.net(true) diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt index fe44dff..643e2ca 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt @@ -36,14 +36,14 @@ class FollowPathCommandCooked(val poseSubsystem: PoseSubsystem, command: Command } override fun isFinished(): Boolean { - if(autodistanceTimer < 0) { + if (autodistanceTimer < 0) { println("autoscore command finished") resetAndEndCommand() return true } if (poseSubsystem.pose.translation.getDistance(poseSubsystem.autoscoreCommandPose.translation) < poseSubsystem.autoDistance) { autodistanceTimer -= 0.05 - //prevent command from timing out if we're close + // prevent command from timing out if we're close } return false } @@ -51,7 +51,6 @@ class FollowPathCommandCooked(val poseSubsystem: PoseSubsystem, command: Command override fun end(interrupted: Boolean) { currentCommand.end(interrupted) } - } class AutoScoreCommands( @@ -123,7 +122,7 @@ class AutoScoreCommands( } } poseSubsystem.autoscoreCommandPose = reefPose - var returnCommand : Command = MagnetizePIDPoseAlign(drive, poseSubsystem, reefPose, controller) + var returnCommand: Command = MagnetizePIDPoseAlign(drive, poseSubsystem, reefPose, controller) if (usingPathfinding) { /*** pathfinding ***/ returnCommand = AutoBuilder.pathfindToPose( @@ -155,7 +154,7 @@ class AutoScoreCommands( constraints, 0.0, ) - if(!usingPathfinding) { + if (!usingPathfinding) { returnCommand = MagnetizePIDPoseAlign( drive, poseSubsystem, @@ -178,13 +177,13 @@ class AutoScoreCommands( println("coral intake called") val coralIntakePose: Pose2d if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - if(isAtTopSource) { + if (isAtTopSource) { coralIntakePose = AutoScoreCommandConstants.coralIntakePoseRedTop } else { coralIntakePose = AutoScoreCommandConstants.coralIntakePoseRedBottom } } else { - if(isAtTopSource) { + if (isAtTopSource) { coralIntakePose = AutoScoreCommandConstants.coralIntakePoseBlueTop } else { coralIntakePose = AutoScoreCommandConstants.coralIntakePoseBlueBottom @@ -238,7 +237,6 @@ class AutoScoreCommands( } return FollowPathCommandCooked(poseSubsystem, returnCommand) - } /** @@ -254,7 +252,7 @@ class AutoScoreCommands( AutoScoreCommandConstants.ReefLevel.L3 -> robot.superstructureManager.requestGoal(SuperstructureGoal.L3) AutoScoreCommandConstants.ReefLevel.L4 -> robot.superstructureManager.requestGoal(SuperstructureGoal.L4) } - //premove will be added in the bindings instead of here + // premove will be added in the bindings instead of here } /** @@ -294,7 +292,7 @@ class AutoScoreCommands( return coralCommand } - fun reef(reefLocation: AutoScoreCommandConstants.ReefLocation, reefLevel: AutoScoreCommandConstants.ReefLevel ): Command { + fun reef(reefLocation: AutoScoreCommandConstants.ReefLocation, reefLevel: AutoScoreCommandConstants.ReefLevel): Command { val reefCommand = moveToReefCommand(reefLocation).andThen(putCoralInReef(reefLevel)) return reefCommand } @@ -303,5 +301,4 @@ class AutoScoreCommands( val processorCommand = moveToProcessorCommand().andThen(scoreProcessorCommand()) return processorCommand } - -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/MagnetizePIDPoseAlign.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/MagnetizePIDPoseAlign.kt index a2b27b8..c1c29cf 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/MagnetizePIDPoseAlign.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/MagnetizePIDPoseAlign.kt @@ -73,6 +73,7 @@ class MagnetizePIDPoseAlign( private var desiredVel = doubleArrayOf(0.0, 0.0, 0.0) private var magnetizationPower = 5.0 + // Time in seconds until magnetization will stop if the driver is opposing magnetization private var magnetizationStopTime = 1.2 private var timeUntilMagnetizationStop = magnetizationStopTime @@ -218,9 +219,13 @@ class MagnetizePIDPoseAlign( ) } - var angle = abs(atan2(controllerDesVel.vxMetersPerSecond, - controllerDesVel.vyMetersPerSecond) - - atan2(pose.translation.x, pose.translation.y)) + var angle = abs( + atan2( + controllerDesVel.vxMetersPerSecond, + controllerDesVel.vyMetersPerSecond + ) - + atan2(pose.translation.x, pose.translation.y) + ) if (angle > Math.PI) { angle = 2 * Math.PI - angle } @@ -240,8 +245,10 @@ class MagnetizePIDPoseAlign( // Values need to be adjusted I haven't tested yet // pid controller from pose rn to pose controller given speeds for chassis speeds - drivetrain.set(calculate(poseSubsystem.pose, pose) + controllerDesVel * - (magnetizationPower * (poseSubsystem.pose.translation.getDistance(pose.translation) / 10.0))) + drivetrain.set( + calculate(poseSubsystem.pose, pose) + controllerDesVel * + (magnetizationPower * (poseSubsystem.pose.translation.getDistance(pose.translation) / 10.0)) + ) // constant pose rn distance to desired pose } diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/WebConnection.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/WebConnection.kt index 81c7361..677d4e9 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/WebConnection.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/WebConnection.kt @@ -1,21 +1,17 @@ package frc.team449.commands.autoscoreCommands import edu.wpi.first.networktables.NetworkTableInstance -import edu.wpi.first.networktables.Publisher import edu.wpi.first.networktables.StringPublisher import edu.wpi.first.networktables.StringSubscriber -import edu.wpi.first.networktables.TimestampedString import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.InstantCommand class WebConnection : Runnable { private val instance = NetworkTableInstance.getDefault() private val webComTable = instance.getTable("webcom") private val allianceTopic = webComTable.getStringTopic("Alliance") private val isDoneTopic = webComTable.getBooleanTopic("isDone") - val commandSubscriber : StringSubscriber - val commandPublisher : StringPublisher + val commandSubscriber: StringSubscriber + val commandPublisher: StringPublisher val isDonePublish = isDoneTopic.publish() private val alliancePublish = allianceTopic.publish() var command = "none" @@ -26,13 +22,13 @@ class WebConnection : Runnable { val alliance = if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) "Red" else "Blue" alliancePublish.set(alliance) isDonePublish.set(true) - //have to set command here because topics will not be created yet. + // have to set command here because topics will not be created yet. commandSubscriber = webComTable.getStringTopic("Command").subscribe("none") commandPublisher = webComTable.getStringTopic("Command").publish() commandPublisher.set("none") } - fun closeServer () { + fun closeServer() { instance.stopServer() instance.close() } @@ -41,5 +37,4 @@ class WebConnection : Runnable { commandPublisher.set("none") isDonePublish.set(true) } - -} \ No newline at end of file +}