From abdf468030f10604f051e88a60444d4fd9217372 Mon Sep 17 00:00:00 2001 From: EdzmLeGoat Date: Mon, 3 Mar 2025 12:32:26 -0500 Subject: [PATCH] changes at competition --- src/main/deploy/autoscore/reefPlacement.js | 4 +- .../deploy/autoscore/robotCommunication.js | 36 ++++++--- src/main/deploy/autoscore/style.css | 5 +- .../autoscoreCommands/AutoScoreCommands.kt | 74 ++++++++++++++----- .../autoscoreCommands/AutoScorePathfinder.kt | 55 ++++++-------- .../frc/team449/subsystems/WebConnection.kt | 40 ++++++---- .../superstructure/SuperstructureGoal.kt | 59 --------------- .../superstructure/SuperstructureManager.kt | 2 +- .../superstructure/elevator/Elevator.kt | 14 ++++ .../subsystems/superstructure/pivot/Pivot.kt | 17 ++++- .../subsystems/superstructure/wrist/Wrist.kt | 15 ++++ .../subsystems/vision/PoseSubsystem.kt | 19 ++--- 12 files changed, 189 insertions(+), 151 deletions(-) diff --git a/src/main/deploy/autoscore/reefPlacement.js b/src/main/deploy/autoscore/reefPlacement.js index 920e35e..f64e435 100644 --- a/src/main/deploy/autoscore/reefPlacement.js +++ b/src/main/deploy/autoscore/reefPlacement.js @@ -32,7 +32,9 @@ const confirmReefButton = document.getElementById("confirmReefButton"); ["touchend", "mouseleave"].forEach(_ => { locationImg.addEventListener(_, () => { - locationImg.src = "locationSelectorImages/locationSelectorNone.png"; + if(!areaSelected) { + locationImg.src = "locationSelectorImages/locationSelectorNone.png"; + } }); }); diff --git a/src/main/deploy/autoscore/robotCommunication.js b/src/main/deploy/autoscore/robotCommunication.js index 875fe3e..8d38eb2 100644 --- a/src/main/deploy/autoscore/robotCommunication.js +++ b/src/main/deploy/autoscore/robotCommunication.js @@ -46,6 +46,20 @@ function valueUpdateHandler( topic, timestamp_us, value ) { console.log("value updated: " + topic.name); if(topic.name == "/webcom/Alliance") { setAlliance(value); + } else if(topic.name == "/webcom/isDone" && value===true) { + document.getElementById("manualText").innerText = `Scoring on Area ${numberToLetter[11-(reefArea+4)%12]} and Level ${coralLevel}`; + showContainer("manualContainer"); + ["pivotForward", "pivotBack", "elevatorUp", "elevatorDown", "wristForward", "wristBack"].forEach( + id => { + const elt = document.getElementById(id); + const eltinterval = setInterval(() => { + if(window.getComputedStyle(elt).backgroundColor == "rgb(200, 0, 0)") { + setCommand(id); + } + }, 0.02); + intervalList.push(eltinterval); + } + ); } } @@ -97,8 +111,9 @@ let coralLevel = -1; let reefArea = -1; let coralSelected = false; let areaSelected = false; +let intervalList = []; -const resetReefStuff = () => { +const resetReef = () => { coralSelected = false; areaSelected = false; coralLevel = -1; @@ -108,12 +123,14 @@ const resetReefStuff = () => { document.getElementById("areaText").innerText = "Reef Area: None"; document.getElementById("coralText").innerText = "Coral Level: None"; document.getElementById("locationSelect").src = "locationSelectorImages/locationSelectorNone.png"; + intervalList.forEach(ivl => clearInterval(ivl)); + intervalList.length = 0; } document.getElementById("confirmReefButton").onclick = async () => { if(coralSelected && areaSelected) { - document.getElementById("manualText").innerText = `Scoring on Area ${numberToLetter[11-(reefArea+4)%12]} and Level ${coralLevel}` - showContainer("manualContainer"); + scoreReef(reefArea, coralLevel); + document.getElementById("confirmReefButton").innerText = "Scoring..."; } } @@ -132,7 +149,6 @@ const scoreProcessor = async () => { } const intakeCoral = async (isAtTopSide) => { - console.log("intaking coral"); if(isAtTopSide) { setCommand("intakeCoralTop"); } else { @@ -145,11 +161,6 @@ const scoreReef = async (location, level) => { setCommand(`l${level} location${location > 3 ? location-3 : location+9}`); } -["pivotForward", "pivotBack", "elevatorUp", "elevatorDown", "wristForward", "wristBack"].forEach( - id => document.getElementById(id).onclick = () => { - setCommand(id); - }); - const scoringContainer = document.querySelector(".scoringContainer"); const messageContainer = document.querySelector(".messageContainer"); const msgDisplay = document.getElementById("messageDisplay"); @@ -169,18 +180,19 @@ const connection = () => { messageContainer.style.display = "none"; scoringContainer.style.display = "flex"; msgDisplay.innerText = ""; + resetReef(); showContainer("reefContainer"); } document.getElementById("cancel").onclick = () => { setCommand("cancel"); - resetReefStuff(); + resetReef(); showContainer("reefContainer"); } document.getElementById("score").onclick = () => { - scoreReef(); - resetReefStuff(); + setCommand("score"); + resetReef(); showContainer("reefContainer"); } diff --git a/src/main/deploy/autoscore/style.css b/src/main/deploy/autoscore/style.css index 4b00c85..84e81f3 100644 --- a/src/main/deploy/autoscore/style.css +++ b/src/main/deploy/autoscore/style.css @@ -47,6 +47,8 @@ body { .coralLevelImage { width: 25%; + align-items: center; + align-content: center; } .reefText { @@ -78,7 +80,7 @@ body { #coral { width: 100%; - height: 100%; + aspect-ratio: 0.5; } #locationSelect { @@ -122,6 +124,7 @@ body { justify-content: center; text-align: center; padding: 0 1vh; + user-select: none; } #confirmReefButton { diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt index 05c0106..c8624c3 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt @@ -1,7 +1,6 @@ package frc.team449.commands.autoscoreCommands import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.DriverStation.Alliance import edu.wpi.first.wpilibj.RobotBase @@ -13,9 +12,26 @@ import frc.team449.subsystems.superstructure.SuperstructureGoal class AutoScoreCommands(private val robot: Robot) { private var currentCommand: Command = InstantCommand() + private var scoreCommand: Command = InstantCommand() + var waitingForScore = false + private var moving = false + + fun scoreCommand(): Command { + waitingForScore = false + return scoreCommand + } + + fun currentCommandFinished(): Boolean { + if(!currentCommand.isScheduled && moving) { + moving = false + return true + } + return false + } fun getReefCommand(rl: AutoScoreCommandConstants.ReefLocation, cl: AutoScoreCommandConstants.CoralLevel): Command { return runOnce({ + moving = true val reefLocationPose = if (DriverStation.getAlliance().get() == Alliance.Red) { when (rl) { @@ -62,64 +78,82 @@ class AutoScoreCommands(private val robot: Robot) { } robot.poseSubsystem.autoscoreCommandPose = reefLocationPose - val scoreCommand = if (!RobotBase.isSimulation()) { + scoreCommand = if (!RobotBase.isSimulation()) { robot.superstructureManager.requestGoal(scoreGoal) - } else InstantCommand() + } else { + InstantCommand() + } val premoveCommand = robot.superstructureManager.requestGoal(premoveGoal) currentCommand = AutoScoreWrapperCommand( - robot, AutoScorePathfinder(robot, reefLocationPose), premoveCommand - ).andThen(InstantCommand({ - scoreCommand.schedule() - robot.drive.defaultCommand = robot.driveCommand - })) + robot, + AutoScorePathfinder(robot, reefLocationPose), + premoveCommand + ).andThen( + InstantCommand({ + robot.drive.defaultCommand = robot.driveCommand + waitingForScore = true + }) + ) currentCommand.schedule() }) } fun getProcessorCommand(): Command { return runOnce({ + moving = true val processorPose = if (DriverStation.getAlliance().get() == Alliance.Red) AutoScoreCommandConstants.processorPoseRed else AutoScoreCommandConstants.processorPoseBlue - val scoreCommand = InstantCommand() + scoreCommand = InstantCommand() val premoveCommand = InstantCommand() robot.poseSubsystem.autoscoreCommandPose = processorPose currentCommand = AutoScoreWrapperCommand( - robot, AutoScorePathfinder(robot, processorPose), premoveCommand - ).andThen(InstantCommand({ - scoreCommand.schedule() - robot.drive.defaultCommand = robot.driveCommand - })) + robot, + AutoScorePathfinder(robot, processorPose), + premoveCommand + ).andThen( + InstantCommand({ + robot.drive.defaultCommand = robot.driveCommand + waitingForScore = true + }) + ) currentCommand.schedule() }) } fun getNetCommand(atRedSide: Boolean): Command { return runOnce({ + moving = true val netPose = Pose2d( AutoScoreCommandConstants.centerOfField + AutoScoreCommandConstants.centerOfField * if (atRedSide) 1 else -1, robot.poseSubsystem.pose.translation.y, if (atRedSide) AutoScoreCommandConstants.netRotation2dRed else AutoScoreCommandConstants.netRotation2dBlue ) - val scoreCommand = InstantCommand() + scoreCommand = InstantCommand() val premoveCommand = InstantCommand() currentCommand = AutoScoreWrapperCommand( - robot, AutoScorePathfinder(robot, netPose), premoveCommand - ).andThen(InstantCommand({ - scoreCommand.schedule() - robot.drive.defaultCommand = robot.driveCommand - })) + robot, + AutoScorePathfinder(robot, netPose), + premoveCommand + ).andThen( + InstantCommand({ + robot.drive.defaultCommand = robot.driveCommand + waitingForScore = true + }) + ) currentCommand.schedule() }) } fun cancelCommand(): Command { return InstantCommand({ + moving = false currentCommand.cancel() robot.drive.defaultCommand = robot.driveCommand + waitingForScore = false }) } } diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScorePathfinder.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScorePathfinder.kt index 59c041e..d9fd1a6 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScorePathfinder.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScorePathfinder.kt @@ -21,21 +21,18 @@ import frc.team449.Robot import frc.team449.commands.driveAlign.SimpleReefAlign import frc.team449.subsystems.RobotConstants import frc.team449.subsystems.drive.swerve.SwerveDrive -import frc.team449.subsystems.superstructure.SuperstructureGoal import kotlin.math.PI import kotlin.math.abs class AutoScorePathfinder(val robot: Robot, private val endPose: Pose2d) { private var ADStar = LocalADStar() - private val pathPub: StructArrayPublisher private val velXPub: DoublePublisher private val velYPub: DoublePublisher private val velRotationPub: DoublePublisher private val setpointPub: BooleanPublisher private val rotPub: BooleanPublisher private val autodistancePub: BooleanPublisher - private val pathSub: StructArraySubscriber private val admagpub: DoublePublisher private val distpub: DoublePublisher private lateinit var path: PathPlannerPath @@ -64,46 +61,42 @@ class AutoScorePathfinder(val robot: Robot, private val endPose: Pose2d) { private var yPIDSpeed = 0.0 private val pidOffsetTime = 0.15 - private var thetaController: PIDController = PIDController(7.0, 0.5, 0.0) + private var thetaController: PIDController = PIDController(10.0, 3.0, 0.1) private var xController = PIDController(7.0, 2.0, 0.0) private var yController = PIDController(7.0, 2.0, 0.0) var distance: Double private var adMag = 1.0 private var pidMag = 0.0 - private val rotTol = 0.0 private val adDecRate = 0.07 private val pidIncRate = 0.04 init { timer.restart() - velXPub = NetworkTableInstance.getDefault().getDoubleTopic("/pathVelocityX").publish() - velYPub = NetworkTableInstance.getDefault().getDoubleTopic("/pathVelocityY").publish() - velRotationPub = NetworkTableInstance.getDefault().getDoubleTopic("/pathRotation").publish() - setpointPub = NetworkTableInstance.getDefault().getBooleanTopic("/atSetpoint").publish() - rotPub = NetworkTableInstance.getDefault().getBooleanTopic("/atRotSetpoint").publish() - autodistancePub = NetworkTableInstance.getDefault().getBooleanTopic("/inPIDDistance").publish() - pathPub = NetworkTableInstance.getDefault().getStructArrayTopic("/zactivePath", Pose2d.struct).publish(*arrayOf()) - distpub = NetworkTableInstance.getDefault().getDoubleTopic("/distance").publish() - pathSub = - NetworkTableInstance.getDefault().getStructArrayTopic("/zactivePath", Pose2d.struct) - .subscribe(arrayOf(zeroPose, zeroPose)) + velXPub = NetworkTableInstance.getDefault().getDoubleTopic("/pathfinder/pathVelocityX").publish() + velYPub = NetworkTableInstance.getDefault().getDoubleTopic("/pathfinder/pathVelocityY").publish() + velRotationPub = NetworkTableInstance.getDefault().getDoubleTopic("/pathfinder/pathRotation").publish() + setpointPub = NetworkTableInstance.getDefault().getBooleanTopic("/pathfinder/atSetpoint").publish() + rotPub = NetworkTableInstance.getDefault().getBooleanTopic("/pathfinder/atRotSetpoint").publish() + autodistancePub = NetworkTableInstance.getDefault().getBooleanTopic("/pathfinder/inPIDDistance").publish() + distpub = NetworkTableInstance.getDefault().getDoubleTopic("/pathfinder/distance").publish() xController.reset() - xController.reset() - thetaController.reset() + yController.reset() + xController.setTolerance(0.0) + yController.setTolerance(0.0) + thetaController.reset() + thetaController.setpoint = endPose.rotation.radians thetaController.enableContinuousInput(-PI, PI) - thetaController.setTolerance(rotTol) - xController.setTolerance(tolerance) - yController.setTolerance(tolerance) + thetaController.setTolerance(0.0) + adMag = 1.0 - admagpub = NetworkTableInstance.getDefault().getDoubleTopic("/admag").publish() + admagpub = NetworkTableInstance.getDefault().getDoubleTopic("/pathfinder/admag").publish() distance = 100.0 } fun runSetup() { timer.restart() - PathPlannerPath.clearCache() ADStar.setStartPosition(robot.poseSubsystem.pose.translation) ADStar.setGoalPosition(endPose.translation) velXPub.set(velocityX) @@ -112,8 +105,8 @@ class AutoScorePathfinder(val robot: Robot, private val endPose: Pose2d) { distance = robot.poseSubsystem.pose.translation.getDistance(endPose.translation) adMag = 1.0 pidMag = 0.0 - if(distance < pidDistance) { - adMag = (distance/pidDistance) + if (distance < pidDistance) { + adMag = (distance / pidDistance) pidMag = abs(1 - adMag) } } @@ -176,7 +169,6 @@ class AutoScorePathfinder(val robot: Robot, private val endPose: Pose2d) { } if (!trajectoryNull) { expectedTime = trajectory.totalTimeSeconds - pathPub.set(pathList) trajValid = (pathList[0] != endPose) startTime = currentTime } @@ -210,8 +202,9 @@ class AutoScorePathfinder(val robot: Robot, private val endPose: Pose2d) { xPIDSpeed *= pidMag yPIDSpeed *= pidMag - rotation = thetaController.calculate(MathUtil.angleModulus(robot.poseSubsystem.pose.rotation.radians), endPose.rotation.radians) - if (thetaController.atSetpoint()) { + val wrappedRotation = MathUtil.angleModulus(robot.poseSubsystem.pose.rotation.radians) + rotation = thetaController.calculate(wrappedRotation) + if (wrappedRotation == endPose.rotation.radians) { rotation = 0.0 } if (atSetpoint) { @@ -228,7 +221,7 @@ class AutoScorePathfinder(val robot: Robot, private val endPose: Pose2d) { robot.poseSubsystem.setPathMag(fieldRelative) } setpointPub.set(atSetpoint) - rotPub.set(thetaController.atSetpoint()) + rotPub.set(wrappedRotation == endPose.rotation.radians) autodistancePub.set(inPIDDistance) velXPub.set(xPIDSpeed) velYPub.set(yPIDSpeed) @@ -268,7 +261,7 @@ class AutoScoreWrapperCommand( } override fun execute() { - if(!hasPremoved && asPathfinder.atPremoveDistance) { + if (!hasPremoved && asPathfinder.atPremoveDistance) { goal.schedule() hasPremoved = true } @@ -276,7 +269,7 @@ class AutoScoreWrapperCommand( reefAlignCommand = SimpleReefAlign(robot.drive, robot.poseSubsystem) reefAlignCommand.schedule() usingReefAlign = true - } else if(!usingReefAlign) { + } else if (!usingReefAlign) { asPathfinder.pathFind() } } diff --git a/src/main/kotlin/frc/team449/subsystems/WebConnection.kt b/src/main/kotlin/frc/team449/subsystems/WebConnection.kt index f666965..e3b4a18 100644 --- a/src/main/kotlin/frc/team449/subsystems/WebConnection.kt +++ b/src/main/kotlin/frc/team449/subsystems/WebConnection.kt @@ -4,12 +4,15 @@ import edu.wpi.first.networktables.NetworkTableInstance import edu.wpi.first.networktables.StringPublisher import edu.wpi.first.networktables.StringSubscriber import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.InstantCommand import edu.wpi.first.wpilibj2.command.SubsystemBase +import edu.wpi.first.wpilibj2.command.WaitCommand import frc.team449.Robot import frc.team449.commands.autoscoreCommands.AutoScoreCommandConstants import frc.team449.commands.autoscoreCommands.AutoScoreCommands +import frc.team449.subsystems.superstructure.SuperstructureGoal class WebConnection(val robot: Robot) : SubsystemBase() { private val instance = NetworkTableInstance.getDefault() @@ -24,11 +27,15 @@ class WebConnection(val robot: Robot) : SubsystemBase() { private var ntCommandInput = "none" private var autoScore = AutoScoreCommands(robot) private var webAppCommand : Command = InstantCommand() + private val pivotAngleIncrease = 0.035 + private val wristAngleIncrease = 0.04 + private val elevatorIncrease = 0.028592106 + fun setUpNT() { instance.startClient4("localhost") instance.setServerTeam(449) - isDonePublish.set(true) + isDonePublish.set(false) commandPublisher.set("none") val alliance = if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) "Red" else "Blue" alliancePublish.set(alliance) @@ -42,19 +49,25 @@ class WebConnection(val robot: Robot) : SubsystemBase() { override fun periodic() { ntCommandInput = commandSubscriber.get().toString() - if (ntCommandInput != "none" && DriverStation.isDSAttached()) { - println("command received") + if (ntCommandInput != "none") { + println("command received: $ntCommandInput") isDonePublish.set(false) webAppCommand = when (ntCommandInput) { - "processor" -> autoScore.getProcessorCommand() - "intakeCoralTop" -> InstantCommand() - "intakeCoralBottom" -> InstantCommand() - "netRed" -> autoScore.getNetCommand(true) - "netBlue" -> autoScore.getNetCommand(false) - "cancel" -> autoScore.cancelCommand() + "processor" -> autoScore.getProcessorCommand().andThen(InstantCommand({isDonePublish.set(false)})) + "netRed" -> autoScore.getNetCommand(true).andThen(InstantCommand({isDonePublish.set(false)})) + "netBlue" -> autoScore.getNetCommand(false).andThen(InstantCommand({isDonePublish.set(false)})) + "cancel" -> autoScore.cancelCommand().andThen(WaitCommand(0.25)).andThen(robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)) + "score" -> autoScore.scoreCommand().andThen(WaitCommand(0.25)).andThen(robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)) + "pivotForward" -> robot.pivot.setPosition(robot.pivot.positionSupplier.get()-pivotAngleIncrease) + "pivotBack" -> robot.pivot.setPosition(robot.pivot.positionSupplier.get()+pivotAngleIncrease) + "wristForward" -> robot.wrist.setPosition(robot.wrist.positionSupplier.get()-wristAngleIncrease) + "wristBack" -> robot.wrist.setPosition(robot.wrist.positionSupplier.get()+wristAngleIncrease) + "elevatorUp" -> robot.elevator.setPosition(robot.elevator.positionSupplier.get()+elevatorIncrease) + "elevatorDown" -> robot.elevator.setPosition(robot.elevator.positionSupplier.get()-elevatorIncrease) else -> { //format will be l_ location__ + isDonePublish.set(false) val level = ntCommandInput.slice(0..1) val location = ntCommandInput.slice(3.. 0.3) { + if (dt > 0.3) { resetMagVars() } @@ -233,7 +233,6 @@ class PoseSubsystem( } desVel.omegaRadiansPerSecond = MathUtil.clamp(desVel.omegaRadiansPerSecond, -AutoScoreCommandConstants.MAX_ROT_SPEED, AutoScoreCommandConstants.MAX_ROT_SPEED) drive.set(desVel) - println("pathfinder") } else { val controllerSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( vel.x * directionCompensation.invoke(), @@ -256,10 +255,10 @@ class PoseSubsystem( var controllerTranslationVector = Translation2d(controllerSpeeds.vxMetersPerSecond, controllerSpeeds.vyMetersPerSecond) var desiredTranslationVector = Translation2d(desVel.vxMetersPerSecond, desVel.vyMetersPerSecond) - if(controllerTranslationVector != Translation2d(0.0, 0.0)) { + if (controllerTranslationVector != Translation2d(0.0, 0.0)) { controllerTranslationVector /= max(abs(controllerSpeeds.vxMetersPerSecond), abs(controllerSpeeds.vyMetersPerSecond)) } - if(desiredTranslationVector != Translation2d(0.0, 0.0)) { + if (desiredTranslationVector != Translation2d(0.0, 0.0)) { desiredTranslationVector /= max(abs(desVel.vxMetersPerSecond), abs(desVel.vyMetersPerSecond)) } val vectorDistance = controllerTranslationVector.getDistance(desiredTranslationVector) @@ -270,27 +269,23 @@ class PoseSubsystem( desVel.omegaRadiansPerSecond += rotScaled } combinedChassisSpeeds = desVel - println("agree") } else if (currentControllerPower > 16) { combinedChassisSpeeds = controllerSpeeds * 8.0 - if(abs(rotScaled) < 0.05) { + if (abs(rotScaled) < 0.05) { combinedChassisSpeeds.omegaRadiansPerSecond = desVel.omegaRadiansPerSecond } - println("driver") } else if (abs(rotScaled) > 0.1) { combinedChassisSpeeds = desVel - if(abs(rotScaled) > 0.5) { + if (abs(rotScaled) > 0.5) { combinedChassisSpeeds.omegaRadiansPerSecond = rotScaled * (if (currentControllerPower > 5) 5.0 else currentControllerPower) / 3 } else { combinedChassisSpeeds.omegaRadiansPerSecond += rotScaled } - println("combination driver rotation") } else { - println("combination") println(controllerSpeeds) println(desVel) println(currentControllerPower) - combinedChassisSpeeds = controllerSpeeds + desVel + combinedChassisSpeeds = controllerSpeeds * (currentControllerPower / 2) + desVel } combinedChassisSpeeds.vxMetersPerSecond = MathUtil.clamp(combinedChassisSpeeds.vxMetersPerSecond, -AutoScoreCommandConstants.MAX_LINEAR_SPEED, AutoScoreCommandConstants.MAX_LINEAR_SPEED)