diff --git a/build.gradle b/build.gradle index 830e83a..a1b23b5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id 'org.jetbrains.kotlin.jvm' } diff --git a/simgui-ds.json b/simgui-ds.json index 9dded04..ce6ddce 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,6 +90,8 @@ } ], "robotJoysticks": [ + {}, + {}, {}, {}, { diff --git a/src/main/deploy/pathplanner/autos/3 L4 Left.auto b/src/main/deploy/pathplanner/autos/3 L4 Left.auto index 235ecce..03a46f7 100644 --- a/src/main/deploy/pathplanner/autos/3 L4 Left.auto +++ b/src/main/deploy/pathplanner/autos/3 L4 Left.auto @@ -25,7 +25,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto new file mode 100644 index 0000000..268147b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.1.path b/src/main/deploy/pathplanner/paths/1.1.path index 361ca04..651090d 100644 --- a/src/main/deploy/pathplanner/paths/1.1.path +++ b/src/main/deploy/pathplanner/paths/1.1.path @@ -3,35 +3,35 @@ "waypoints": [ { "anchor": { - "x": 8.127663934426229, - "y": 6.152817622950819 + "x": 7.010250000000001, + "y": 6.12125 }, "prevControl": null, "nextControl": { - "x": 7.185209356966076, - "y": 6.018613752832711 + "x": 6.067795422539848, + "y": 5.987046129881892 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.93, - "y": 5.2 + "x": 4.01, + "y": 5.18 }, "prevControl": { - "x": 5.390327868852459, - "y": 5.74064549180328 + "x": 3.255495797837585, + "y": 6.530247651884371 }, "nextControl": null, "isLocked": false, - "linkedName": "Reef C Right" + "linkedName": "Reef B Left" } ], "rotationTargets": [ { "waypointRelativePos": 0.6593362654938016, - "rotationDegrees": -119.99999999999999 + "rotationDegrees": -59.99999999999999 } ], "constraintZones": [], @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "PrepL4", - "waypointRelativePos": 0.5053022269353092, + "waypointRelativePos": 0.05, "endWaypointRelativePos": null, "command": { "type": "named", @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -59,7 +59,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "1 - 3 L4 Left", @@ -67,5 +67,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.2.path b/src/main/deploy/pathplanner/paths/1.2.path index 8db099c..f1edc03 100644 --- a/src/main/deploy/pathplanner/paths/1.2.path +++ b/src/main/deploy/pathplanner/paths/1.2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/1.3.path b/src/main/deploy/pathplanner/paths/1.3.path index 9dcea19..26ad5f1 100644 --- a/src/main/deploy/pathplanner/paths/1.3.path +++ b/src/main/deploy/pathplanner/paths/1.3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/1.4.path b/src/main/deploy/pathplanner/paths/1.4.path index 2eb82c9..8b40b0b 100644 --- a/src/main/deploy/pathplanner/paths/1.4.path +++ b/src/main/deploy/pathplanner/paths/1.4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/1.5.path b/src/main/deploy/pathplanner/paths/1.5.path index aafe714..5e49d8e 100644 --- a/src/main/deploy/pathplanner/paths/1.5.path +++ b/src/main/deploy/pathplanner/paths/1.5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/1.6.path b/src/main/deploy/pathplanner/paths/1.6.path index c78daeb..e988729 100644 --- a/src/main/deploy/pathplanner/paths/1.6.path +++ b/src/main/deploy/pathplanner/paths/1.6.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index be005ba..32d443e 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,38 +3,43 @@ "waypoints": [ { "anchor": { - "x": 3.29, - "y": 4.27 + "x": 1.6377996575342464, + "y": 7.428317636986301 }, "prevControl": null, "nextControl": { - "x": 4.29, - "y": 4.27 + "x": 2.211376093885291, + "y": 6.609165592697311 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 1.1867827868852459, + "y": 0.9741290983606552 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 0.8870901639344262, + "y": 0.4226946721311481 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 55.69221215528403 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.04083126241170403 + "rotation": -51.73601121411935 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef Positions.path b/src/main/deploy/pathplanner/paths/Reef Positions.path index 0a8e785..8980b7f 100644 --- a/src/main/deploy/pathplanner/paths/Reef Positions.path +++ b/src/main/deploy/pathplanner/paths/Reef Positions.path @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 4.05, - "y": 5.2 + "x": 4.01, + "y": 5.18 }, "prevControl": { - "x": 3.8157313155445705, - "y": 5.605765264099125 + "x": 3.7757313155445704, + "y": 5.585765264099124 }, "nextControl": { - "x": 4.284268684455429, - "y": 4.794234735900876 + "x": 4.244268684455429, + "y": 4.774234735900875 }, "isLocked": false, "linkedName": "Reef B Left" @@ -193,8 +193,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 10e1fdf..b897a13 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -6,8 +6,8 @@ "1 - 3 L4 Left" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 4.0, + "defaultMaxVel": 1.5, + "defaultMaxAccel": 2.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt new file mode 100644 index 0000000..2ed6eec --- /dev/null +++ b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt @@ -0,0 +1,40 @@ +package cshcyberhawks.lib.commands + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup + +//class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : Command() { +// private var i = -1 +// private var currentCommand = Commands.runOnce({}) +// +// override fun initialize() { +// i = 0 +// currentCommand = callback(i) +// currentCommand.schedule() +// } +// +// override fun execute() { +// SmartDashboard.putNumber("For Command i", i.toDouble()) +// SmartDashboard.putBoolean("For Command current finished", currentCommand.isFinished) +// if (currentCommand.isFinished) { +// println("current command for is finished") +// } +//// println("current command for finished: " + currentCommand.isFinished) +// if (currentCommand.isFinished && i < max) { +// i++ +// currentCommand = callback(i) +// currentCommand.schedule() +// } +// } +// +// override fun isFinished(): Boolean { +// SmartDashboard.putBoolean("For command finished", i >= max && currentCommand.isFinished) +// return i >= max && currentCommand.isFinished +// } +//} + +class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : SequentialCommandGroup( + *(0.. SwerveIOReal() +// RobotType.Real -> SwerveIOSim() RobotType.Simulated -> SwerveIOSim() RobotType.Empty -> SwerveIOBase() } @@ -40,24 +42,43 @@ object RobotContainer { val teleopDriveCommand = when (RobotConfiguration.robotType) { RobotType.Real -> TeleopDriveCommand() // RobotType.Real -> Commands.run({}) - RobotType.Simulated -> SimTeleopDriveCommand() + RobotType.Simulated -> TeleopDriveCommand() RobotType.Empty -> Commands.run({}) } var currentDriveCommand: Optional = Optional.empty(); + val autoCommand: Command + // val autoChooser = AutoBuilder.buildAutoChooser() init { configureBindings() - for (pathplannerCommand in AutoCommands.entries) { - NamedCommands.registerCommand(pathplannerCommand.name, pathplannerCommand.cmd) - } +// for (pathplannerCommand in AutoCommands.entries) { +// NamedCommands.registerCommand(pathplannerCommand.name, pathplannerCommand.cmd) +// } + autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Left() +// autoCommand = getAutonomousCommand() // SmartDashboard.putData("Auto Chooser", autoChooser) } + fun getAutonomousCommand(): Command { + try { + // Load the path you want to follow using its name in the GUI +// val path = PathPlannerPath.fromPathFile("3 L4 Left") + + // Create a path following command using AutoBuilder. This will also trigger event markers. +// return AutoBuilder.followPath(path) + return PathPlannerAuto("3 L4 Left"); + } catch (e: Exception) { + DriverStation.reportError("Big oops: " + e.message, e.stackTrace) +// throw Exception("Big oops: " + e.message, e) + return Commands.none() + } + } + private fun configureBindings() { // teleopDriveCommand.addRequirements(drivetrain) drivetrain.setDefaultCommand(teleopDriveCommand) diff --git a/src/main/java/frc/robot/commands/AutoAlign.kt b/src/main/java/frc/robot/commands/AutoAlign.kt deleted file mode 100644 index 24cb08c..0000000 --- a/src/main/java/frc/robot/commands/AutoAlign.kt +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.commands - -import edu.wpi.first.wpilibj2.command.Command -import frc.robot.util.AllianceFlipUtil -import frc.robot.util.input.CoralSide -import frc.robot.util.input.OperatorControls - -object AutoAlign { - fun coralReefAlign(): Command { - return GoToPose({ - val reefSide = OperatorControls.reefPosition - - AllianceFlipUtil.apply(when (OperatorControls.coralSide) { - CoralSide.Left -> reefSide.left - CoralSide.Right -> reefSide.right - }) - }, { false }) - } - - fun algaeReefAlign(): Command { - return GoToPose({ - AllianceFlipUtil.apply(OperatorControls.reefPosition.center) - }) - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutoCommands.kt b/src/main/java/frc/robot/commands/AutoCommands.kt index 818b0f3..52703b2 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.kt +++ b/src/main/java/frc/robot/commands/AutoCommands.kt @@ -1,21 +1,21 @@ -package frc.robot.commands - -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands -import frc.robot.RobotState -import frc.robot.subsystems.superstructure.Superstructure -import frc.robot.subsystems.superstructure.intake.GamePieceState - -enum class AutoCommands(val cmd: Command) { - PrepL4(Commands.runOnce({ Superstructure.Auto.prepL4() })), - ScoreL4( - Commands.runOnce({ Superstructure.Auto.justScoreL4() }) - .andThen(Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Empty }) - .andThen(Commands.waitSeconds(0.2)) - ), - IntakeFeeder( - Commands.runOnce({ Superstructure.Auto.prepL4() }) - .andThen(Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Coral }) - ), - AwaitIntake(Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Coral }) -} \ No newline at end of file +//package frc.robot.commands +// +//import edu.wpi.first.wpilibj2.command.Command +//import edu.wpi.first.wpilibj2.command.Commands +//import frc.robot.RobotState +//import frc.robot.subsystems.superstructure.Superstructure +//import frc.robot.subsystems.superstructure.intake.GamePieceState +// +//enum class AutoCommands(val cmd: Command) { +// PrepL4(Commands.runOnce({ Superstructure.Auto.prepL4() })), +// ScoreL4( +// Commands.runOnce({ Superstructure.Auto.justScoreL4() }) +// .andThen(Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Empty }) +// .andThen(Commands.waitSeconds(0.2)) +// ), +// IntakeFeeder( +// Commands.runOnce({ Superstructure.intakeFeeder() }) +// .andThen(Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Coral }) +// ), +// AwaitIntake(Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Coral }) +//} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/GoToPose.kt b/src/main/java/frc/robot/commands/GoToPose.kt index 7739d8e..dada3b9 100644 --- a/src/main/java/frc/robot/commands/GoToPose.kt +++ b/src/main/java/frc/robot/commands/GoToPose.kt @@ -10,10 +10,11 @@ import edu.wpi.first.wpilibj2.command.Command import frc.robot.RobotContainer import frc.robot.RobotState import frc.robot.subsystems.swerve.SwerveConstants +import frc.robot.util.Visualizer import java.util.* import kotlin.math.abs -class GoToPose( +open class GoToPose( val targetPoseGetter: () -> Pose2d, val endCondition: () -> Boolean = { val currentPose = RobotContainer.drivetrain.getSwervePose() val targetPose = targetPoseGetter() @@ -27,19 +28,19 @@ class GoToPose( SwerveConstants.translationPIDConstants.kP, SwerveConstants.translationPIDConstants.kI, SwerveConstants.translationPIDConstants.kD, - TrapezoidProfile.Constraints(1.0, 1.0) + TrapezoidProfile.Constraints(SwerveConstants.maxAutoSpeed, SwerveConstants.maxAutoAccel) ) val yController = ProfiledPIDController( SwerveConstants.translationPIDConstants.kP, SwerveConstants.translationPIDConstants.kI, SwerveConstants.translationPIDConstants.kD, - TrapezoidProfile.Constraints(1.0, 1.0) + TrapezoidProfile.Constraints(SwerveConstants.maxAutoSpeed, SwerveConstants.maxAutoAccel) ) val rotationController = ProfiledPIDController( SwerveConstants.rotationPIDConstants.kP, SwerveConstants.rotationPIDConstants.kI, SwerveConstants.rotationPIDConstants.kD, - TrapezoidProfile.Constraints(Units.degreesToRadians(180.0), Units.degreesToRadians(180.0)) + TrapezoidProfile.Constraints(Units.degreesToRadians(SwerveConstants.maxAutoTwistDegrees), Units.degreesToRadians(SwerveConstants.maxAutoTwistAccelDegrees)) ) var targetPose = targetPoseGetter() @@ -49,8 +50,6 @@ class GoToPose( } override fun initialize() { - targetPose = targetPoseGetter() - val robotPose = RobotContainer.drivetrain.getSwervePose() val robotVel = ChassisSpeeds.fromRobotRelativeSpeeds(RobotContainer.drivetrain.getSpeeds(), robotPose.rotation) @@ -58,13 +57,16 @@ class GoToPose( yController.reset(robotPose.y, robotVel.vyMetersPerSecond) rotationController.reset(robotPose.rotation.radians, robotVel.omegaRadiansPerSecond) - SmartDashboard.putString("Goal Position", targetPose.toString()) - RobotContainer.currentDriveCommand = Optional.of(this); RobotState.autoDriving = true } override fun execute() { + targetPose = targetPoseGetter() + + Visualizer.targetPosePublisher.set(targetPose) + SmartDashboard.putString("Goal Position", targetPose.toString()) + val currentPose = RobotContainer.drivetrain.getSwervePose() val xFeedback = xController.calculate(currentPose.x, targetPose.x) diff --git a/src/main/java/frc/robot/commands/TeleopAutoAlign.kt b/src/main/java/frc/robot/commands/TeleopAutoAlign.kt new file mode 100644 index 0000000..42ca6c5 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopAutoAlign.kt @@ -0,0 +1,92 @@ +package frc.robot.commands + +import cshcyberhawks.lib.math.MiscCalculations +import edu.wpi.first.wpilibj2.command.Command +import frc.robot.RobotContainer +import frc.robot.constants.AutoScoringConstants +import frc.robot.constants.FieldConstants +import frc.robot.util.AllianceFlipUtil +import frc.robot.util.input.CoralSide +import frc.robot.util.input.OperatorControls +import kotlin.math.min + +object TeleopAutoAlign { + fun noWalkCoralReefAlign(): Command { + return GoToPose({ + val position = OperatorControls.reefPosition + val side = OperatorControls.coralSide + + AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) + }) + } + + fun noWalkAlgaeReefAlign(): Command { + return GoToPose({ + val position = OperatorControls.reefPosition + + AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0)) + }) + } + + fun coralReefAlign(): Command { + return GoToPose({ + val position = OperatorControls.reefPosition + val side = OperatorControls.coralSide + + if (OperatorControls.noWalk) { + AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) + } else { + val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) + // Make the position get closer as it gets closer to the final goal +// val adjustX: Double = 0.3 + var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75) +// val adjustX = 1.0 + + // Once we're close enough we can just jump to the goal + if (adjustX < 0.25) { + adjustX = 0.0 + } + + AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, adjustX)) + } + }, { false }) + } + + fun algaeReefAlign(): Command { + return GoToPose({ + val position = OperatorControls.reefPosition + + if (OperatorControls.noWalk) { + AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0)) + } else { + val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0)) + // Make the position get closer as it gets closer to the final goal +// val adjustX: Double = 0.3 + var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75) +// val adjustX = 1.0 + + // Once we're close enough we can just jump to the goal + if (adjustX < 0.25) { + adjustX = 0.0 + } + + AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, adjustX)) + } + }) + } + + fun processorAlign(): Command = GoToPose({ + AllianceFlipUtil.apply(FieldConstants.processorPosition) + }, { false }) + + fun feederAlign(): Command = GoToPose({ + val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition) + val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition) + + if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(rightPos.translation)) { + leftPos + } else { + rightPos + } + }, { false }) +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TeleopDriveCommand.kt b/src/main/java/frc/robot/commands/TeleopDriveCommand.kt index 65538b7..9963ab4 100644 --- a/src/main/java/frc/robot/commands/TeleopDriveCommand.kt +++ b/src/main/java/frc/robot/commands/TeleopDriveCommand.kt @@ -30,14 +30,14 @@ class TeleopDriveCommand : Command() { val deadzonedLeftY = MiscCalculations.calculateDeadzone(RobotContainer.leftJoystick.y, .5) - SwerveConstants.ControlledSpeed = MathUtil.clamp( - SwerveConstants.ControlledSpeed + (-deadzonedLeftY * SwerveConstants.MaxSpeedConst * .02), 0.0, - SwerveConstants.MaxSpeedConst - ); - SwerveConstants.ControlledAngularRate = MathUtil.clamp( - SwerveConstants.ControlledAngularRate + (-deadzonedLeftY * SwerveConstants.MaxAngularRateConst * .02), - 0.0, SwerveConstants.MaxAngularRateConst - ); +// SwerveConstants.ControlledSpeed = MathUtil.clamp( +// SwerveConstants.ControlledSpeed + (-deadzonedLeftY * SwerveConstants.MaxSpeedConst * .02), 0.0, +// SwerveConstants.MaxSpeedConst +// ); +// SwerveConstants.ControlledAngularRate = MathUtil.clamp( +// SwerveConstants.ControlledAngularRate + (-deadzonedLeftY * SwerveConstants.MaxAngularRateConst * .02), +// 0.0, SwerveConstants.MaxAngularRateConst +// ); SmartDashboard.putNumber("ControlledSpeed", SwerveConstants.ControlledSpeed) SmartDashboard.putNumber("ControlledAngularRate", SwerveConstants.ControlledAngularRate) diff --git a/src/main/java/frc/robot/commands/auto/AutoBuilder.kt b/src/main/java/frc/robot/commands/auto/AutoBuilder.kt new file mode 100644 index 0000000..0b29a7a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoBuilder.kt @@ -0,0 +1,137 @@ +package frc.robot.commands.auto + +import cshcyberhawks.lib.commands.ForCommand +import cshcyberhawks.lib.math.Timer +import edu.wpi.first.wpilibj2.command.* +import frc.robot.RobotState +import frc.robot.constants.AutoScoringConstants +import frc.robot.subsystems.superstructure.Superstructure +import frc.robot.subsystems.superstructure.intake.GamePieceState +import frc.robot.util.input.CoralSide + +data class CoralTarget(val position: AutoScoringConstants.ReefPositions, val side: CoralSide) + +object AutoBuilder { + fun justDriveOneL4Left(): Command { + return SequentialCommandGroup( + AutoCommands.coralReefAlign(AutoScoringConstants.ReefPositions.B, CoralSide.Left), + AutoCommands.leftFeederAlign() + ) + } + + fun oneL4Left(): Command { + val autoTimer = Timer() + + return SequentialCommandGroup( + Commands.runOnce({ + autoTimer.reset() + autoTimer.start() + }), + ParallelCommandGroup( + Commands.runOnce({ Superstructure.scoreL4() }), + AutoCommands.coralReefAlign(AutoScoringConstants.ReefPositions.B, CoralSide.Left) + ), + WaitCommand(0.5), +// Commands.runOnce({Superstructure.Auto.justScoreL4()}), + Commands.runOnce({ RobotState.actionConfirmed = true }), + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Empty }, + Commands.waitSeconds(0.1), + ParallelCommandGroup( + AutoCommands.leftFeederAlign(), + Commands.runOnce({ Superstructure.intakeFeeder() }) + ), + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Coral } + ) + } + + private fun createGenericAuto( + targets: Array, + rightFeeder: Boolean, + ): Command { + val autoTimer = Timer() + + return SequentialCommandGroup( + Commands.runOnce({ + autoTimer.reset() + autoTimer.start() + }), + ForCommand(targets.size) { i -> + SequentialCommandGroup( + ParallelCommandGroup( + Commands.runOnce({ Superstructure.scoreL4() }), + AutoCommands.coralReefAlign(targets[i].position, targets[i].side) + ), + WaitCommand(0.35), + Commands.runOnce({ + RobotState.actionConfirmed = true + println("Score $i: ${autoTimer.get()}") + }), + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Empty }, + Commands.waitSeconds(0.1), +// AutoCommands.safeReefExit(targets[i].position), // Backs up to exit the reef safely if necessary + ParallelDeadlineGroup( + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Coral }, + if (rightFeeder) AutoCommands.rightFeederAlign() else AutoCommands.leftFeederAlign(), + Commands.runOnce({ Superstructure.intakeFeeder() }) + ), + Commands.waitSeconds(0.2), + Commands.runOnce({ println("Intake $i: ${autoTimer.get()}") }) + ) + } + ) + } + + fun twoL4Left() = createGenericAuto( + arrayOf( + CoralTarget(AutoScoringConstants.ReefPositions.B, CoralSide.Left), + CoralTarget(AutoScoringConstants.ReefPositions.B, CoralSide.Right) + ), + false + ) + + fun twoL4Right() = createGenericAuto( + arrayOf( + CoralTarget(AutoScoringConstants.ReefPositions.F, CoralSide.Right), + CoralTarget(AutoScoringConstants.ReefPositions.F, CoralSide.Left) + ), + true + ) + + fun threeL4Left() = createGenericAuto( + arrayOf( + CoralTarget(AutoScoringConstants.ReefPositions.C, CoralSide.Right), + CoralTarget(AutoScoringConstants.ReefPositions.B, CoralSide.Left), + CoralTarget(AutoScoringConstants.ReefPositions.B, CoralSide.Right) + ), + false + ) + + fun threeL4Right() = createGenericAuto( + arrayOf( + CoralTarget(AutoScoringConstants.ReefPositions.E, CoralSide.Left), + CoralTarget(AutoScoringConstants.ReefPositions.F, CoralSide.Right), + CoralTarget(AutoScoringConstants.ReefPositions.F, CoralSide.Left) + ), + true + ) + + fun backL4Algae() = SequentialCommandGroup( + ParallelCommandGroup( + Commands.runOnce({ Superstructure.scoreL4() }), + AutoCommands.coralReefAlign(AutoScoringConstants.ReefPositions.D, CoralSide.Left) + ), + WaitCommand(0.35), + Commands.runOnce({ + RobotState.actionConfirmed = true + }), + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Empty }, + Commands.waitSeconds(0.1), + AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D), + ParallelDeadlineGroup( + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Algae }, + Commands.runOnce({ Superstructure.removeAlgaeLow() }), + AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.D) + ), + AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D) + ) +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.kt b/src/main/java/frc/robot/commands/auto/AutoCommands.kt new file mode 100644 index 0000000..25311cf --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.kt @@ -0,0 +1,141 @@ +package frc.robot.commands.auto + +import cshcyberhawks.lib.math.MiscCalculations +import cshcyberhawks.lib.math.Timer +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import frc.robot.RobotContainer +import frc.robot.commands.GoToPose +import frc.robot.constants.AutoScoringConstants +import frc.robot.constants.FieldConstants +import frc.robot.subsystems.swerve.SwerveConstants +import frc.robot.util.AllianceFlipUtil +import frc.robot.util.input.CoralSide +import kotlin.math.abs +import kotlin.math.min + +object AutoCommands { + const val timeStartPositionDeadzone = .05 + const val timeStartAngleDeadzone = 3.5 + + fun coralReefAlign(position: AutoScoringConstants.ReefPositions, side: CoralSide): Command { + + val timer = Timer() + + return GoToPose({ + val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) + // Make the position get closer as it gets closer to the final goal +// val adjustX: Double = 0.3 + var adjustX: Double = + min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75) +// val adjustX = 1.0 + + // Once we're close enough we can just jump to the goal + if (adjustX < 0.25) { + adjustX = 0.0 + } + + AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, adjustX)) + }, { + val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) + + val currentPose = RobotContainer.drivetrain.getSwervePose() + + val minus = currentPose.minus(goalPose) + //BAD CODE +// val timerDeadzone: Boolean = !timer.isRunning && MiscCalculations.calculateDeadzone(minus.x, timeStartPositionDeadzone) == 0.0 && MiscCalculations.calculateDeadzone(minus.y, timeStartPositionDeadzone) == 0.0; + + //GOOD CODE + val timerDeadzone: Boolean = !timer.isRunning && MiscCalculations.calculateDeadzone(minus.x, timeStartPositionDeadzone) == 0.0 && MiscCalculations.calculateDeadzone(minus.y, timeStartPositionDeadzone) == 0.0 && MiscCalculations.calculateDeadzone(minus.rotation.degrees, timeStartAngleDeadzone) == 0.0; + +// if (!timer.isRunning && (abs(currentPose.x - goalPose.x) < timeStartPositionDeadzone && abs(currentPose.y - goalPose.y) < timeStartPositionDeadzone && abs( +// abs(currentPose.rotation.degrees - goalPose.rotation.degrees) +// ) < timeStartAngleDeadzone)) { + if (timerDeadzone) { + println("Starting the timer") + timer.reset() + timer.restart() + } + + val timerComplete = timer.isRunning && timer.hasElapsed(1.35) + if (timerComplete) { + println("timer done") + } + + if (timer.isRunning) { + SmartDashboard.putNumber("Auto align timer", timer.get()) + } + + val atPose: Boolean = MiscCalculations.calculateDeadzone(minus.x, SwerveConstants.positionDeadzone) == 0.0 && MiscCalculations.calculateDeadzone(minus.y, SwerveConstants.positionDeadzone) == 0.0 && MiscCalculations.calculateDeadzone(minus.rotation.degrees, SwerveConstants.rotationDeadzone) == 0.0; +// val atPose = abs(currentPose.x - goalPose.x) < SwerveConstants.positionDeadzone && abs(currentPose.y - goalPose.y) < SwerveConstants.positionDeadzone && abs( +// abs(currentPose.rotation.degrees - goalPose.rotation.degrees) +// ) < SwerveConstants.rotationDeadzone + + (timerComplete || atPose) + }) + } + + fun algaeReefAlign(position: AutoScoringConstants.ReefPositions): Command { +// val timer = Timer() + + return GoToPose({ + val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0)) + + // Make the position get closer as it gets closer to the final goal +// val adjustX: Double = 0.3 + var adjustX: Double = + min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75) +// val adjustX = 1.0 + + // Once we're close enough we can just jump to the goal + if (adjustX < 0.3) { + adjustX = 0.0 + } + + AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, adjustX)) + }, { + val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0)) + + val currentPose = RobotContainer.drivetrain.getSwervePose() + + if (/*(timer.isRunning && timer + +.hasElapsed(1.0)) || */(abs(currentPose.x - goalPose.x) < SwerveConstants.positionDeadzone && abs(currentPose.y - goalPose.y) < SwerveConstants.positionDeadzone && abs( + currentPose.rotation.degrees - goalPose.rotation.degrees + ) < SwerveConstants.rotationDeadzone)) { + println("finished") + return@GoToPose true + } + + return@GoToPose false + }) + } + + val unsafeReefPositions = arrayOf( + AutoScoringConstants.ReefPositions.A, + AutoScoringConstants.ReefPositions.C, + AutoScoringConstants.ReefPositions.E, + AutoScoringConstants.ReefPositions.D, + ) + + fun safeReefExit(lastTarget: AutoScoringConstants.ReefPositions) = if (unsafeReefPositions.contains(lastTarget)) { + GoToPose({ AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(lastTarget.ordinal, 1.0)) }, { + // TODO: Probably need to tune this + AllianceFlipUtil.apply(FieldConstants.Reef.center) + .getDistance(RobotContainer.drivetrain.getSwervePose().translation) > 2.1 // Can continue once we're 2 meters out from the center of the reef + }) + } else Commands.none() + + fun leftFeederAlign() = GoToPose({ + // TODO: Probably want to generate these positions from the field constants + AllianceFlipUtil.apply(FieldConstants.leftFeederPosition) + }) + + fun rightFeederAlign() = GoToPose({ + AllianceFlipUtil.apply(FieldConstants.rightFeederPosition) + }) +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/AutoScoringConstants.kt b/src/main/java/frc/robot/constants/AutoScoringConstants.kt index 74cc94b..8a37837 100644 --- a/src/main/java/frc/robot/constants/AutoScoringConstants.kt +++ b/src/main/java/frc/robot/constants/AutoScoringConstants.kt @@ -5,9 +5,62 @@ import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Transform2d import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.math.util.Units +import frc.robot.util.input.CoralSide object AutoScoringConstants { - private val reefFaceOffset = .6 + .74 - Units.inchesToMeters(4.0) +// private val reefFaceOffset = .6 + .74 - Units.inchesToMeters(4.0) +// private val reefFaceOffset = .6 + .74 - Units.inchesToMeters(2.05) + private val reefFaceOffset = .6 + .74 - Units.inchesToMeters(1.5) + + + private val branchOffset = Units.inchesToMeters(6.469) +// private val branchOffset = Units.inchesToMeters(7.5) + + + fun getReefPoseAtOffset(face: Int, side: CoralSide, x: Double): Pose2d { + val poseDirection = + Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble())) + val adjustX: Double = reefFaceOffset + x + val adjustY: Double = when (side) { + CoralSide.Left -> -branchOffset + CoralSide.Right -> branchOffset + } + + return Pose2d( + Translation2d( + poseDirection + .transformBy(Transform2d(adjustX, adjustY, Rotation2d())) + .x, + poseDirection + .transformBy(Transform2d(adjustX, adjustY, Rotation2d())) + .y + ), + Rotation2d( + poseDirection.rotation.radians + ).rotateBy(Rotation2d.k180deg) + ) + } + + fun getAlgaePoseAtOffset(face: Int, x: Double): Pose2d { + val poseDirection = + Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble())) + val adjustX: Double = reefFaceOffset + x + val adjustY: Double = 0.0 + + return Pose2d( + Translation2d( + poseDirection + .transformBy(Transform2d(adjustX, adjustY, Rotation2d())) + .x, + poseDirection + .transformBy(Transform2d(adjustX, adjustY, Rotation2d())) + .y + ), + Rotation2d( + poseDirection.rotation.radians + ).rotateBy(Rotation2d.k180deg) + ) + } enum class ReefPositions(var left: Pose2d, var right: Pose2d, var center: Pose2d) { A(Pose2d(), Pose2d(), Pose2d()), @@ -23,7 +76,7 @@ object AutoScoringConstants { val poseDirection = Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble())) val adjustX: Double = reefFaceOffset - val adjustY: Double = Units.inchesToMeters(6.469)// + 1.5) + val adjustY: Double = branchOffset val rightPose = Pose2d( diff --git a/src/main/java/frc/robot/constants/CANConstants.kt b/src/main/java/frc/robot/constants/CANConstants.kt index 9d45762..20d4bb6 100644 --- a/src/main/java/frc/robot/constants/CANConstants.kt +++ b/src/main/java/frc/robot/constants/CANConstants.kt @@ -19,6 +19,13 @@ object CANConstants { const val algaeLaserCANId = 42 } - // Climb + object Climb { + const val motorId = 45 + const val encoderId = 46 + } + object Funnel { + const val motorId = 47 + const val encoderId = 48 + } } diff --git a/src/main/java/frc/robot/constants/FieldConstants.kt b/src/main/java/frc/robot/constants/FieldConstants.kt index 5e90246..fa32b8e 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.kt +++ b/src/main/java/frc/robot/constants/FieldConstants.kt @@ -5,7 +5,9 @@ import com.fasterxml.jackson.databind.ObjectMapper import edu.wpi.first.apriltag.AprilTagFieldLayout import edu.wpi.first.math.geometry.* import edu.wpi.first.math.util.Units +import edu.wpi.first.wpilibj.DriverStation.Alliance import edu.wpi.first.wpilibj.Filesystem +import frc.robot.util.AllianceFlipUtil import java.io.IOException import java.nio.file.Path import java.util.* @@ -193,4 +195,11 @@ object FieldConstants { ANDYMARK("andymark"), WELDED("welded") } + + //red processor = 11.63, 7.35, 90 + var processorPosition = Pose2d(6.02, .71, Rotation2d.fromDegrees(270.0)) + + var leftFeederPosition = Pose2d(1.546, 7.352, Rotation2d.fromDegrees(-55.0)) + var rightFeederPosition = Pose2d(1.546, 8.052 - 7.352, Rotation2d.fromDegrees(55.0)) + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt index 881152e..ea7cf5c 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt @@ -8,10 +8,16 @@ import frc.robot.RobotContainer import frc.robot.RobotState import frc.robot.RobotType import frc.robot.constants.FieldConstants +import frc.robot.subsystems.superstructure.climb.ClimbSystem +import frc.robot.subsystems.superstructure.climb.implementation.ClimbIOEmpty +import frc.robot.subsystems.superstructure.climb.implementation.ClimbIOReal import frc.robot.subsystems.superstructure.elevator.ElevatorSystem import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOEmpty import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOPID import frc.robot.subsystems.superstructure.elevator.implementation.ElevatorIOSim +import frc.robot.subsystems.superstructure.funnel.FunnelSystem +import frc.robot.subsystems.superstructure.funnel.implementation.FunnelIOEmpty +import frc.robot.subsystems.superstructure.funnel.implementation.FunnelIOReal import frc.robot.subsystems.superstructure.intake.GamePieceState import frc.robot.subsystems.superstructure.intake.IntakeSystem import frc.robot.subsystems.superstructure.intake.implementation.IntakeIOEmpty @@ -27,8 +33,8 @@ object Superstructure : SubsystemBase() { val pivotSystem = PivotSystem( when (RobotConfiguration.robotType) { - RobotType.Real -> PivotIOPID() // RobotType.Real -> PivotIOEmpty() + RobotType.Real -> PivotIOPID() RobotType.Simulated -> PivotIOSim() RobotType.Empty -> PivotIOEmpty() } @@ -37,7 +43,6 @@ object Superstructure : SubsystemBase() { ElevatorSystem( when (RobotConfiguration.robotType) { RobotType.Real -> ElevatorIOPID() -// RobotType.Real -> ElevatorIOEmpty() RobotType.Simulated -> ElevatorIOSim() RobotType.Empty -> ElevatorIOEmpty() } @@ -51,6 +56,20 @@ object Superstructure : SubsystemBase() { RobotType.Empty -> IntakeIOEmpty() } ) + val climbSystem = ClimbSystem( + when (RobotConfiguration.robotType) { + RobotType.Real -> ClimbIOReal() + RobotType.Simulated -> ClimbIOEmpty() + RobotType.Empty -> ClimbIOEmpty() + } + ) + val funnelSystem = FunnelSystem( + when (RobotConfiguration.robotType) { + RobotType.Real -> FunnelIOReal() + RobotType.Simulated -> FunnelIOEmpty() + RobotType.Empty -> FunnelIOEmpty() + } + ) // Requests system private var activeRequest: Optional = Optional.empty() @@ -116,11 +135,14 @@ object Superstructure : SubsystemBase() { fun awaitAtDesiredPosition() = ParallelRequest(elevatorSystem.awaitDesiredPosition(), pivotSystem.awaitDesiredAngle()) + const val safeRetractReefDistance = 1.6 + fun safeToRetract(): Boolean { val swervePose = RobotContainer.drivetrain.getSwervePose() - val blueReefDistance = FieldConstants.Reef.center.getDistance(swervePose.translation) > 1.7 + val blueReefDistance = FieldConstants.Reef.center.getDistance(swervePose.translation) > safeRetractReefDistance val redReefDistance = - AllianceFlipUtil.apply(FieldConstants.Reef.center).getDistance(swervePose.translation) > 1.7 + AllianceFlipUtil.apply(FieldConstants.Reef.center) + .getDistance(swervePose.translation) > safeRetractReefDistance val clearOfBarge = swervePose.x < 7.0 || swervePose.x > 10.5 return blueReefDistance && redReefDistance && clearOfBarge @@ -139,6 +161,11 @@ object Superstructure : SubsystemBase() { ) ) + private fun unHalfSpitRequest() = IfRequest( + { !RobotState.actionConfirmed }, + SequentialRequest(WaitRequest(.25), intakeSystem.coralIntake(), WaitRequest(0.25), intakeSystem.idle()) + ) + fun stow() = requestSuperstructureAction(stowRequest()) fun intakeFeeder() = @@ -146,15 +173,34 @@ object Superstructure : SubsystemBase() { SuperstructureAction.create( ParallelRequest( elevatorSystem.feederPosition(), - pivotSystem.feederAngle(), + pivotSystem.feederAngle().withPrerequisite(elevatorSystem.safeIntakePosition()), intakeSystem.coralIntake() ), EmptyRequest(), - ParallelRequest(stowRequest(), intakeSystem.idle()), + ParallelRequest( + stowRequest(), + IfRequest( + { RobotState.gamePieceState == GamePieceState.Coral }, + intakeSystem.coralHolding(), + intakeSystem.idle() + ) + ), confirmed = { RobotState.gamePieceState == GamePieceState.Coral } ) ) + fun correctIntake() = requestSuperstructureAction( + SuperstructureAction.create( + intakeSystem.coralIntake(), + EmptyRequest(), + SequentialRequest( + WaitRequest(0.1), + intakeSystem.idle() + ), + confirmed = { RobotState.gamePieceState == GamePieceState.Coral } + ) + ) + fun scoreL2() = requestSuperstructureAction( SuperstructureAction.create( @@ -168,7 +214,7 @@ object Superstructure : SubsystemBase() { fun scoreL3() = requestSuperstructureAction( SuperstructureAction.create( - ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position()), + ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit()), intakeSystem.coralScore(), stowRequest(), safeRetract = true @@ -191,7 +237,15 @@ object Superstructure : SubsystemBase() { SuperstructureAction.create( l4PrepRequest(), intakeSystem.coralScore(), - safeRetractRequest(), +// safeRetractRequest(), + SequentialRequest( + ParallelRequest(pivotSystem.travelAngle(), elevatorSystem.safeDownPosition()), +// WaitRequest(0.1), + ParallelRequest( + elevatorSystem.stowPosition().withPrerequisite(pivotSystem.safeTravelDown()), + pivotSystem.stowAngle().withPrerequisite(elevatorSystem.belowSafeUpPosition()) + ) + ), safeRetract = true ) ) @@ -207,9 +261,14 @@ object Superstructure : SubsystemBase() { intakeSystem.algaeIntake() ), EmptyRequest(), - SequentialRequest( - ParallelRequest(pivotSystem.stowAngle(), elevatorSystem.stowPosition()), - intakeSystem.idle() + ParallelRequest( + pivotSystem.stowAngle(), + elevatorSystem.stowPosition(), + IfRequest( + { RobotState.gamePieceState == GamePieceState.Empty }, + intakeSystem.idle(), + intakeSystem.algaeHolding() + ) ), { RobotState.gamePieceState == GamePieceState.Algae }, safeRetract = true @@ -230,9 +289,13 @@ object Superstructure : SubsystemBase() { intakeSystem.algaeIntake() ), EmptyRequest(), - SequentialRequest( + ParallelRequest( safeRetractRequest(), - intakeSystem.idle() + IfRequest( + { RobotState.gamePieceState == GamePieceState.Empty }, + intakeSystem.idle(), + intakeSystem.algaeHolding() + ) ), { RobotState.gamePieceState == GamePieceState.Algae }, safeRetract = true @@ -241,9 +304,9 @@ object Superstructure : SubsystemBase() { fun scoreProcessor() = requestSuperstructureAction( SuperstructureAction.create( - ParallelRequest(elevatorSystem.processorPosition(), pivotSystem.processorAngle()), + ParallelRequest(elevatorSystem.processorPosition(), pivotSystem.processorAngle(), funnelSystem.deploy()), intakeSystem.algaeScore(), - stowRequest() + ParallelRequest(stowRequest(), funnelSystem.stow()) ) ) @@ -265,18 +328,59 @@ object Superstructure : SubsystemBase() { ) ) - object Auto { - fun prepL4() = requestSuperstructureAction( - l4PrepRequest() + fun climb() = requestSuperstructureAction( + SuperstructureAction.create( + ParallelRequest( + pivotSystem.oldClimbAngle(), + climbSystem.deploy(), + funnelSystem.deploy() + ), + climbSystem.climb(), + EmptyRequest()//IfRequest({RobotState.actionCancelled}, SequentialRequest(ParallelRequest(funnelSystem.stow(), climbSystem.stow())), pivotSystem.stowAngle()) ) + ) - fun justScoreL4() = requestSuperstructureAction( - SuperstructureAction.create( - EmptyRequest(), - intakeSystem.coralScore(), - safeRetractRequest(), - safeRetract = true - ) + + fun unclimb() = requestSuperstructureAction( + SequentialRequest( + ParallelRequest( + climbSystem.deploy(), + funnelSystem.deploy(), + pivotSystem.climbStowAngle() + ), +// stowRequest().withPrerequisite(Prerequisite.withCondition { climbSystem.isStow() && funnelSystem.isStow() }) ) - } + ) + + fun climbStowThenStow() = requestSuperstructureAction( + SequentialRequest( + ParallelRequest(climbSystem.stow(), pivotSystem.climbStowAngle()), + funnelSystem.stow().withPrerequisite(Prerequisite.withCondition{climbSystem.isStow()}), + WaitRequest(2.5), + stowRequest().withPrerequisite(Prerequisite.withCondition { climbSystem.isStow() && funnelSystem.isStow() }) + ) + ) + + +// object Auto { +// fun prepL4() = requestSuperstructureAction( +// SuperstructureAction.create( +// l4PrepRequest(), +// EmptyRequest(), +// EmptyRequest(), +// { true }, +// { true } +// ) +// ) +// +// fun justScoreL4() = requestSuperstructureAction( +// SuperstructureAction.create( +// EmptyRequest(), +// intakeSystem.coralScore(), +// safeRetractRequest(), +// { true }, +// safeRetract = true +// ) +// ) +// } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt new file mode 100644 index 0000000..fa6e8f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt @@ -0,0 +1,9 @@ +package frc.robot.subsystems.superstructure.climb + +object ClimbConstants { + val stowAngle = 223.0 + val deployAngle = 80.0 +// val climbAngle = 145.0 + var climbAngle = 125.0 + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt new file mode 100644 index 0000000..2244f64 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt @@ -0,0 +1,10 @@ +package frc.robot.subsystems.superstructure.climb + +interface ClimbIO { + fun getAngle(): Double + fun angleDegrees(angleDegrees: Double) + fun periodic() + + var climbing: Boolean + var disable: Boolean +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt new file mode 100644 index 0000000..a91a869 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt @@ -0,0 +1,29 @@ +package frc.robot.subsystems.superstructure.climb + +import cshcyberhawks.lib.math.MiscCalculations +import cshcyberhawks.lib.requests.Request +import cshcyberhawks.lib.requests.SequentialRequest +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.SubsystemBase + +class ClimbSystem(private val io: ClimbIO): SubsystemBase() { + private fun setAngle(angleDegrees: Double) = Request.withAction { io.angleDegrees(angleDegrees) } + private fun setClimbing(climbing: Boolean) = Request.withAction { io.climbing = climbing } + private fun setDisable(disable: Boolean) = Request.withAction {io.disable = disable} + + fun deploy() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.deployAngle)) + fun climb() = SequentialRequest(setClimbing(true), setDisable(false), setAngle(ClimbConstants.climbAngle)) + + fun stow() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.stowAngle)) + + fun isStow(): Boolean { + return MiscCalculations.calculateDeadzone(io.getAngle() - ClimbConstants.stowAngle, 5.0) == 0.0 + } + + + override fun periodic() { + io.periodic() + + SmartDashboard.putNumber("Climb Angle", io.getAngle()) + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt new file mode 100644 index 0000000..aa758e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt @@ -0,0 +1,18 @@ +package frc.robot.subsystems.superstructure.climb.implementation + +import frc.robot.subsystems.superstructure.climb.ClimbIO + +class ClimbIOEmpty(): ClimbIO { + + override var disable = false; + override var climbing = false; + override fun getAngle(): Double { + return 0.0 + } + + override fun angleDegrees(angleDegrees: Double) { + } + + override fun periodic() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt new file mode 100644 index 0000000..962ce30 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt @@ -0,0 +1,119 @@ +package frc.robot.subsystems.superstructure.climb.implementation + +import com.ctre.phoenix6.configs.CANcoderConfiguration +import com.ctre.phoenix6.configs.MagnetSensorConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.TorqueCurrentFOC +import com.ctre.phoenix6.hardware.CANcoder +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue +import com.ctre.phoenix6.signals.SensorDirectionValue +import cshcyberhawks.lib.math.MiscCalculations +import edu.wpi.first.math.controller.ProfiledPIDController +import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import frc.robot.constants.CANConstants +import frc.robot.subsystems.superstructure.climb.ClimbConstants +import frc.robot.subsystems.superstructure.climb.ClimbIO +import frc.robot.subsystems.superstructure.funnel.FunnelConstants +import kotlin.math.sin + +class ClimbIOReal() : ClimbIO { + private val motor = TalonFX(CANConstants.Climb.motorId) + private val encoder = CANcoder(CANConstants.Climb.encoderId) + + override var climbing = false + + override var disable = false + + private var neutralCoast = false + + + private val pidController = ProfiledPIDController( + 0.2315, + 0.0, + 0.0315, + TrapezoidProfile.Constraints( + 180.0, + 250.0, + ) + ) + + private var motorConfig = TalonFXConfiguration() + + private var encoderConfig = CANcoderConfiguration() + private var encoderMagnetSensorConfig = MagnetSensorConfigs() + + private val torqueRequest = TorqueCurrentFOC(0.0) + + private var desiredAngle = ClimbConstants.stowAngle + + private val tbOffset = -120.2 + + init { + val slot0Configs = motorConfig.Slot0 + + slot0Configs.kP = 0.0 + slot0Configs.kI = 0.0 + slot0Configs.kD = 0.0 + + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + + val currentConfigs = motorConfig.CurrentLimits + currentConfigs.StatorCurrentLimitEnable = true + currentConfigs.StatorCurrentLimit = 60.0 + + motor.configurator.apply(motorConfig) + + encoderMagnetSensorConfig.SensorDirection = SensorDirectionValue.Clockwise_Positive + + encoderConfig.withMagnetSensor(encoderMagnetSensorConfig) + encoder.configurator.apply(encoderConfig) + + motor.setNeutralMode(NeutralModeValue.Brake) + + pidController.goal = TrapezoidProfile.State(desiredAngle, 0.0) + + SmartDashboard.putBoolean("Climb coast", false) + } + + override fun getAngle(): Double { + return MiscCalculations.wrapAroundAngles((MiscCalculations.wrapAroundAngles(encoder + .absolutePosition.valueAsDouble * 360.0) - tbOffset)) + } + + override fun angleDegrees(angleDegrees: Double) { + desiredAngle = angleDegrees + pidController.goal = TrapezoidProfile.State(angleDegrees, 0.0) + } + + override fun periodic() { + + + val positionPIDOut = pidController.calculate(getAngle()) + +// val gravityFF = 3.15 * sin(Math.toRadians(getAngle())) + + val motorOut = if (climbing && (ClimbConstants.climbAngle - getAngle()) > 10.0) {positionPIDOut + 30.0} else if (climbing) {positionPIDOut + 15.0} else positionPIDOut + + + if (!disable) { +// println(motorOut) + motor.setControl(torqueRequest.withOutput(motorOut)) + + } + + + val sdCoast = SmartDashboard.getBoolean("Climb coast", false) +// println(sdCoast) + if (sdCoast != neutralCoast) { + neutralCoast = sdCoast + motor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + } + + SmartDashboard.putNumber("Climb Raw TB Angle", MiscCalculations.wrapAroundAngles(encoder.absolutePosition.valueAsDouble * 360.0)) + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt index 77e9779..38e937d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt @@ -25,9 +25,12 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { fun awaitDesiredPosition() = AwaitRequest { atDesiredPosition() } fun belowSafeUpPosition() = Prerequisite.withCondition { getPosition() < safeUpPosition } + fun safeIntakePosition() = Prerequisite.withCondition { getPosition() < 7.0 } + fun safeToFlipPivot() = Prerequisite.withCondition { getPosition() > 4.5 } private fun setPosition(positionInches: Double) = Request.withAction { io.setPosition(positionInches) + println("requested position: $positionInches") } override fun periodic() { @@ -47,6 +50,8 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { fun l3Position() = setPosition(4.0) fun l4Position() = setPosition(29.95) // Should be 30 eventually but not safe right now + fun climbPosition() = setPosition(6.0) + fun algaeRemoveLowPosition() = setPosition(1.0) fun algaeRemoveHighPosition() = setPosition(18.0) diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt new file mode 100644 index 0000000..c613f2c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt @@ -0,0 +1,12 @@ +package frc.robot.subsystems.superstructure.funnel + +object FunnelConstants { + const val velocityDegrees = 180.0 // Guessed values for now + const val accelerationDegrees = 250.0 + +// const val stowAngle = 215.0 + const val stowAngle = 212.0 +// const val deployAngle = 50.5 + const val deployAngle = 65.5 + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelIO.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelIO.kt new file mode 100644 index 0000000..9dc199e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelIO.kt @@ -0,0 +1,7 @@ +package frc.robot.subsystems.superstructure.funnel + +interface FunnelIO { + fun getAngle(): Double + fun setAngle(angleDegrees: Double) + fun periodic() +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelSystem.kt new file mode 100644 index 0000000..b653e6c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelSystem.kt @@ -0,0 +1,25 @@ +package frc.robot.subsystems.superstructure.funnel + +import cshcyberhawks.lib.math.MiscCalculations +import cshcyberhawks.lib.requests.EmptyRequest +import cshcyberhawks.lib.requests.Request +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.subsystems.superstructure.climb.ClimbConstants + +class FunnelSystem(private val io: FunnelIO) : SubsystemBase() { + private fun setAngle(angle: Double) = Request.withAction { io.setAngle(angle) } + + fun stow() = setAngle(FunnelConstants.stowAngle) + fun deploy() = setAngle(FunnelConstants.deployAngle) + + fun isStow(): Boolean { + return MiscCalculations.calculateDeadzone(io.getAngle() - FunnelConstants.stowAngle, 5.0) == 0.0 + } + + override fun periodic() { + SmartDashboard.putNumber("Funnel Angle", io.getAngle()) + + io.periodic() + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOEmpty.kt new file mode 100644 index 0000000..b3dfad6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOEmpty.kt @@ -0,0 +1,15 @@ +package frc.robot.subsystems.superstructure.funnel.implementation + +import frc.robot.subsystems.superstructure.funnel.FunnelIO + +class FunnelIOEmpty(): FunnelIO { + override fun getAngle(): Double { + return 0.0 + } + + override fun setAngle(angleDegrees: Double) { + } + + override fun periodic() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt new file mode 100644 index 0000000..eae5ab1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt @@ -0,0 +1,97 @@ +package frc.robot.subsystems.superstructure.funnel.implementation + +import com.ctre.phoenix6.configs.CANcoderConfiguration +import com.ctre.phoenix6.configs.MagnetSensorConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.TorqueCurrentFOC +import com.ctre.phoenix6.hardware.CANcoder +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue +import com.ctre.phoenix6.signals.SensorDirectionValue +import cshcyberhawks.lib.math.MiscCalculations +import edu.wpi.first.math.controller.ProfiledPIDController +import edu.wpi.first.math.trajectory.TrapezoidProfile +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import frc.robot.constants.CANConstants +import frc.robot.subsystems.superstructure.funnel.FunnelConstants +import frc.robot.subsystems.superstructure.funnel.FunnelIO +import kotlin.math.sin + +class FunnelIOReal(): FunnelIO { + private val motor = TalonFX(CANConstants.Funnel.motorId) + private val encoder = CANcoder(CANConstants.Funnel.encoderId) + + private val pidController = ProfiledPIDController( + 0.145, + 0.0, + 0.0315, + TrapezoidProfile.Constraints( + FunnelConstants.velocityDegrees, + FunnelConstants.accelerationDegrees + ) + ) + + private var motorConfig = TalonFXConfiguration() + + private var encoderConfig = CANcoderConfiguration() + private var encoderMagnetSensorConfig = MagnetSensorConfigs() + + private val torqueRequest = TorqueCurrentFOC(0.0) + + private var desiredAngle = FunnelConstants.stowAngle + + private val tbOffset = 29.0 + + init { + val slot0Configs = motorConfig.Slot0 + + slot0Configs.kP = 0.0 + slot0Configs.kI = 0.0 + slot0Configs.kD = 0.0 + + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + + val currentConfigs = motorConfig.CurrentLimits + currentConfigs.StatorCurrentLimitEnable = true + currentConfigs.StatorCurrentLimit = 60.0 + + motor.configurator.apply(motorConfig) + + encoderMagnetSensorConfig.SensorDirection = SensorDirectionValue.Clockwise_Positive + + encoderConfig.withMagnetSensor(encoderMagnetSensorConfig) + encoder.configurator.apply(encoderConfig) + + motor.setNeutralMode(NeutralModeValue.Brake) + + pidController.goal = TrapezoidProfile.State(desiredAngle, 0.0) + } + + override fun getAngle(): Double { + return MiscCalculations.wrapAroundAngles((MiscCalculations.wrapAroundAngles(encoder + .absolutePosition.valueAsDouble * 360.0) - tbOffset)) + } + + override fun setAngle(angleDegrees: Double) { + desiredAngle = angleDegrees + pidController.goal = TrapezoidProfile.State(angleDegrees, 0.0) + } + + override fun periodic() { + val positionPIDOut = pidController.calculate(getAngle()) + + +// println("position pid out: " + positionPIDOut) + + val pureGrav = 3.15 * sin(Math.toRadians(getAngle())) + val gravityFF = pureGrav +// val gravityFF: Double = if (pureGrav < 0) .85 else pureGrav + +// println(positionPIDOut + gravityFF) + + motor.setControl(torqueRequest.withOutput(positionPIDOut + gravityFF)) + + SmartDashboard.putNumber("Funnel Raw TB Angle", MiscCalculations.wrapAroundAngles(encoder.absolutePosition.valueAsDouble * 360.0)) + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt index 9912680..798d36e 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeIO.kt @@ -6,7 +6,5 @@ interface IntakeIO { fun hasCoral():Boolean fun hasAlgae():Boolean - fun watchForIntake() - fun periodic() } diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt index 72186b7..91f5fc9 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeState.kt @@ -2,14 +2,28 @@ package frc.robot.subsystems.superstructure.intake // Setting the motor positive will intake both coral and algae enum class IntakeState(val current: Double) { +// Idle(0.0), +// CoralIntake(-40.0), +// CoralHalfSpit(15.0), +// CoralHolding(-1.0), +//// CoralScore(200.0), +// CoralScore(40.0), +// +// AlgaeIntake(40.0), +// AlgaeScore(-40.0), +// AlgaeHolding(1.0) + + Idle(0.0), - CoralIntake(-40.0), - CoralHalfSpit(15.0), - CoralHolding(-1.0), - CoralScore(200.0), - AlgaeIntake(40.0), - AlgaeScore(-40.0), - AlgaeHolding(1.0) + CoralIntake(40.0), + CoralHalfSpit(-14.0), + CoralHolding(1.0), +// CoralScore(200.0), + CoralScore(-200.0), + + AlgaeIntake(-40.0), + AlgaeScore(40.0), + AlgaeHolding(-0.5) // Right now this is always 2 percent output, see IntakeIOReal.kt } enum class GamePieceState { diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt index 52785de..8b5668c 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeSystem.kt @@ -12,19 +12,17 @@ import frc.robot.RobotState // It also reduces the need to have reference variables for the subsystems to be passed around. class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { private fun setIntakeState(state: IntakeState) = Request.withAction { io.setIntakeState(state) } - private fun watchForIntake() = Request.withAction { io.watchForIntake() } fun idle() = setIntakeState(IntakeState.Idle) fun coralHalfSpit() = SequentialRequest( setIntakeState(IntakeState.CoralHalfSpit), - WaitRequest(IntakeConstants.coralScoreTimeoutSeconds), + WaitRequest(IntakeConstants.coralHalfSpitTimeSeconds), setIntakeState(IntakeState.Idle) ) fun coralIntake() = SequentialRequest( setIntakeState(IntakeState.CoralIntake), - watchForIntake() ) fun coralScore() = SequentialRequest( @@ -35,7 +33,6 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { fun algaeIntake() = SequentialRequest( setIntakeState(IntakeState.AlgaeIntake), - watchForIntake() ) fun algaeScore() = SequentialRequest( @@ -44,6 +41,9 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { setIntakeState(IntakeState.Idle) ) + fun coralHolding() = setIntakeState(IntakeState.CoralHolding) + + fun algaeHolding() = setIntakeState(IntakeState.AlgaeHolding) override fun periodic() { io.periodic() @@ -53,6 +53,7 @@ class IntakeSystem(private val io: IntakeIO) : SubsystemBase() { SmartDashboard.putBoolean("Has coral", io.hasCoral()) SmartDashboard.putBoolean("Has algae", io.hasAlgae()) + SmartDashboard.putString("Game Piece State", RobotState.gamePieceState.name) } override fun simulationPeriodic() {} diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt index a324221..ba72208 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOEmpty.kt @@ -7,10 +7,6 @@ import frc.robot.subsystems.superstructure.intake.IntakeState class IntakeIOEmpty() : IntakeIO { override fun setIntakeState(state: IntakeState) {} - override fun watchForIntake() { - - } - override fun hasAlgae(): Boolean { return false } diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt index dbf7d8d..7d9dc6d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt @@ -3,6 +3,7 @@ package frc.robot.subsystems.superstructure.intake.implementation import au.grapplerobotics.LaserCan import au.grapplerobotics.interfaces.LaserCanInterface import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC import com.ctre.phoenix6.controls.TorqueCurrentFOC import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.InvertedValue @@ -20,16 +21,19 @@ class IntakeIOReal() : IntakeIO { private val torqueRequest = TorqueCurrentFOC(0.0) - private var watchingForIntake = false + private var currentIntakeState = IntakeState.Idle init { val coralIntakeMotorConfiguration = TalonFXConfiguration() - coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive +// coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive + println("WHY") +// coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + val currentConfigs = coralIntakeMotorConfiguration.CurrentLimits currentConfigs.StatorCurrentLimitEnable = true currentConfigs.StatorCurrentLimit = 60.0 - intakeMotor.configurator.apply(coralIntakeMotorConfiguration) +// intakeMotor.configurator.apply(coralIntakeMotorConfiguration) coralLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) //coralLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) @@ -41,11 +45,14 @@ class IntakeIOReal() : IntakeIO { } override fun setIntakeState(state: IntakeState) { - intakeMotor.setControl(torqueRequest.withOutput(state.current)) - } - - override fun watchForIntake() { - watchingForIntake = true + println("Intake State: ${state.name}") + println("Intake state current: ${state.current}") + if (state == IntakeState.AlgaeHolding) { + intakeMotor.set(-0.02) + } else { + intakeMotor.setControl(torqueRequest.withOutput(state.current)) + } + currentIntakeState = state } override fun hasAlgae(): Boolean { @@ -53,8 +60,10 @@ class IntakeIOReal() : IntakeIO { SmartDashboard.putNumber("Algae Measurement", measurement.distance_mm.toDouble()) @Suppress("SENSELESS_COMPARISON") return (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm < Units.inchesToMeters( - 12.0 // Other side is ~15.5in from sensor - ) * 1000.0 +// 12.0 // Other side is ~15.5in from sensor + 8.0 // Other side is ~15.5in from sensor + + ) * 1000.0 ) } @@ -65,21 +74,10 @@ class IntakeIOReal() : IntakeIO { return (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm < 40.0) } - private val coralIntakeTimer = Timer() override fun periodic() { - if (watchingForIntake) { - if (hasCoral()) { - intakeMotor.set(IntakeState.CoralHolding.current) - watchingForIntake = false - } else if (hasAlgae()) { - intakeMotor.set(IntakeState.AlgaeHolding.current) - watchingForIntake = false - } - } - - SmartDashboard.putNumber("Intake position", intakeMotor.position.valueAsDouble) - SmartDashboard.putNumber("Intake velocity", intakeMotor.velocity.valueAsDouble) - SmartDashboard.putNumber("Intake acceleration", intakeMotor.acceleration.valueAsDouble) +// SmartDashboard.putNumber("Intake position", intakeMotor.position.valueAsDouble) +// SmartDashboard.putNumber("Intake velocity", intakeMotor.velocity.valueAsDouble) +// SmartDashboard.putNumber("Intake acceleration", intakeMotor.acceleration.valueAsDouble) } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotIO.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotIO.kt index 13aaddc..90819e9 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotIO.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotIO.kt @@ -8,5 +8,6 @@ interface PivotIO { fun getDesiredAngle(): Double fun atDesiredAngle(): Boolean fun setAngle(angleDegrees: Double) + fun killPID() fun periodic() } diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt index 9a27ae5..e01c2a9 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt @@ -3,8 +3,8 @@ package frc.robot.subsystems.superstructure.pivot import cshcyberhawks.lib.requests.AwaitRequest import cshcyberhawks.lib.requests.Prerequisite import cshcyberhawks.lib.requests.Request +import cshcyberhawks.lib.requests.SequentialRequest import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -37,28 +37,46 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() { fun safeTravelUp() = Prerequisite.withCondition { getAngle() < 245.0 } fun safeTravelDown() = Prerequisite.withCondition { getAngle() > 180.0 } + fun safeToClimbDisable() = Prerequisite.withCondition { getAngle() < 20.0 } - fun stowAngle() = setAngle(290.0) + fun stowAngle() = setAngle(295.0) fun travelAngle() = setAngle(220.0) + fun travelDownAngle() = setAngle(180.0) fun feederAngle() = setAngle(310.0) - fun l2Angle() = setAngle(155.0) + fun climbAngle() = SequentialRequest( + setAngle(50.0) +// Request.withAction { io.killPID() }.withPrerequisite(safeToClimbDisable()) + ) + + fun oldClimbAngleWithPID() = setAngle(345.0) + + fun oldClimbAngle() = SequentialRequest( + setAngle(360.0), + Request.withAction { io.killPID() }.withPrerequisite(safeToClimbDisable()) + ) + + fun climbStowAngle() = setAngle(325.0) + + fun l2Angle() = setAngle(150.0) // fun l3Angle() = setAngle(SmartDashboard.getNumber("L3 Angle", 125.0)) - fun l3Angle() = setAngle(123.0) + fun l3Angle() = setAngle(120.0) // fun l4Angle() = setAngle(SmartDashboard.getNumber("L4 Angle", 133.0)) - fun l4Angle() = setAngle(134.0) + fun l4Angle() = setAngle(131.0) - fun algaeRemoveAngle() = setAngle(225.0) + fun algaeRemoveAngle() = setAngle(228.0) fun bargeAngle() = setAngle(130.0) - fun processorAngle() = setAngle(295.0) + fun processorAngle() = setAngle(288.0) override fun periodic() { io.periodic() + SmartDashboard.putBoolean("Safe travel up", safeTravelUp().met()) + SmartDashboard.putNumber("Pivot Angle", getAngle()) } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOEmpty.kt index 60ca06e..48a8cc3 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOEmpty.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOEmpty.kt @@ -9,6 +9,8 @@ class PivotIOEmpty() : PivotIO { override fun atDesiredAngle(): Boolean = false override fun setAngle(angleDegrees: Double) {} + override fun killPID() { + } override fun periodic() {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt index 9c8d191..0fde92e 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt @@ -63,10 +63,17 @@ class PivotIOPID() : PivotIO { //138.6 = 180 private val tbOffset = -43.2 +// private val tbOffset = -23.2//-43.2 // private fun getTBDegrees() = private var neutralCoast = false + private var pidDistabled = false + + override fun killPID() { + pidDistabled = true + } + init { val feedBackConfigs = motorConfig.Feedback @@ -105,6 +112,7 @@ class PivotIOPID() : PivotIO { // motor.setPosition((currentPosition - 45.0)) pivotPIDController.goal = TrapezoidProfile.State(290.0, 0.0) + pivotPIDController.reset(getAngle()) SmartDashboard.putBoolean("Pivot coast", false) @@ -138,6 +146,19 @@ class PivotIOPID() : PivotIO { SmartDashboard.putNumber("Pivot Raw TB Angle", MiscCalculations.wrapAroundAngles(encoder.absolutePosition.valueAsDouble * 360.0)) SmartDashboard.putNumber("Pivot Desired Angle", desiredAngle) + val sdCoast = SmartDashboard.getBoolean("Pivot coast", false) + if (sdCoast != neutralCoast) { + neutralCoast = sdCoast + motor.setNeutralMode( + if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake + ) + } + + if (pidDistabled) { + motor.set(0.0) + return + } + // val kG = if (getAngle() > 270) 8.25 else 10.5 val kG = if (getAngle() > 270) 7.25 else 7.5 val gravityFF = kG * sin(Math.toRadians(getAngle() + 90)) @@ -155,13 +176,5 @@ class PivotIOPID() : PivotIO { SmartDashboard.putNumber("Pivot Output", motorSet) motor.setControl(torqueRequest.withOutput(motorSet)) - - val sdCoast = SmartDashboard.getBoolean("Pivot coast", false) - if (sdCoast != neutralCoast) { - neutralCoast = sdCoast - motor.setNeutralMode( - if (neutralCoast) NeutralModeValue.Coast else NeutralModeValue.Brake - ) - } } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt index 8639bd2..2f6310c 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOReal.kt @@ -1,79 +1,79 @@ -package frc.robot.subsystems.superstructure.pivot.implementation - -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC -import com.ctre.phoenix6.hardware.TalonFX -import com.ctre.phoenix6.signals.InvertedValue -import edu.wpi.first.wpilibj.DutyCycleEncoder -import frc.robot.constants.CANConstants -import cshcyberhawks.lib.math.MiscCalculations -import frc.robot.subsystems.superstructure.pivot.PivotConstants -import frc.robot.subsystems.superstructure.pivot.PivotIO - -class PivotIOReal() : PivotIO { - private val motor = TalonFX(CANConstants.Pivot.motorId) - private val encoder = DutyCycleEncoder(CANConstants.Pivot.encoderId) - - private var motorConfiguration = TalonFXConfiguration() - - private val motionMagic = MotionMagicTorqueCurrentFOC(0.0) - - private var desiredAngle = 290.0 - - init { - val feedBackConfigs = motorConfiguration.Feedback - - // Converts 1 rotation to 1 rotation of the pivot - feedBackConfigs.SensorToMechanismRatio = - PivotConstants.conversionFactor - - val slot0Configs = motorConfiguration.Slot0; - - // TODO: These values need to be changed - slot0Configs.kS = 0.25; // Add 0.25 V output to overcome static friction - slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output - slot0Configs.kA = 0.01; // An acceleration of 1 rps/s requires 0.01 V output - slot0Configs.kP = 4.8; // A position error of 2.5 rotations results in 12 V output - slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.1; // A velocity error of 1 rps results in 0.1 V output - - // set Motion Magic settings - val motionMagicConfigs = motorConfiguration.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = - PivotConstants.velocityDegrees / 360.0 // Target cruise velocity in rps - motionMagicConfigs.MotionMagicAcceleration = - PivotConstants.accelerationDegrees / 360.0 // Target acceleration in rps/s -// motionMagicConfigs.MotionMagicJerk = 1600; Optional // Target jerk of 1600 rps/s/s (0.1 seconds) - motorConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive - - val currentConfigs = motorConfiguration.CurrentLimits - currentConfigs.StatorCurrentLimitEnable = true - currentConfigs.StatorCurrentLimit = 60.0 - - motor.configurator.apply(motorConfiguration); - - // TODO: Also need to mess with this on bringup - val currentPosition = - encoder.get() * (24 / 32) * 360.0 // Clockwise should be positive and zero should probably be 45 degrees up from motor zero (position we likely won't start at but allowing for full rotation with the ratio) - - motor.setPosition((currentPosition - 45.0) / 360.0) - } - - override fun getAngle(): Double { - return motor.position.valueAsDouble * 360.0 - } - - override fun getDesiredAngle(): Double { - return desiredAngle - } - - override fun atDesiredAngle() = - MiscCalculations.appxEqual(getAngle(), desiredAngle, PivotConstants.accelerationDegrees) - - override fun setAngle(angleDegrees: Double) { - desiredAngle = angleDegrees - motor.setControl(motionMagic.withPosition(angleDegrees / 360.0)) - } - - override fun periodic() {} -} +//package frc.robot.subsystems.superstructure.pivot.implementation +// +//import com.ctre.phoenix6.configs.TalonFXConfiguration +//import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC +//import com.ctre.phoenix6.hardware.TalonFX +//import com.ctre.phoenix6.signals.InvertedValue +//import edu.wpi.first.wpilibj.DutyCycleEncoder +//import frc.robot.constants.CANConstants +//import cshcyberhawks.lib.math.MiscCalculations +//import frc.robot.subsystems.superstructure.pivot.PivotConstants +//import frc.robot.subsystems.superstructure.pivot.PivotIO +// +//class PivotIOReal() : PivotIO { +// private val motor = TalonFX(CANConstants.Pivot.motorId) +// private val encoder = DutyCycleEncoder(CANConstants.Pivot.encoderId) +// +// private var motorConfiguration = TalonFXConfiguration() +// +// private val motionMagic = MotionMagicTorqueCurrentFOC(0.0) +// +// private var desiredAngle = 290.0 +// +// init { +// val feedBackConfigs = motorConfiguration.Feedback +// +// // Converts 1 rotation to 1 rotation of the pivot +// feedBackConfigs.SensorToMechanismRatio = +// PivotConstants.conversionFactor +// +// val slot0Configs = motorConfiguration.Slot0; +// +// // TODO: These values need to be changed +// slot0Configs.kS = 0.25; // Add 0.25 V output to overcome static friction +// slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output +// slot0Configs.kA = 0.01; // An acceleration of 1 rps/s requires 0.01 V output +// slot0Configs.kP = 4.8; // A position error of 2.5 rotations results in 12 V output +// slot0Configs.kI = 0.0; // no output for integrated error +// slot0Configs.kD = 0.1; // A velocity error of 1 rps results in 0.1 V output +// +// // set Motion Magic settings +// val motionMagicConfigs = motorConfiguration.MotionMagic; +// motionMagicConfigs.MotionMagicCruiseVelocity = +// PivotConstants.velocityDegrees / 360.0 // Target cruise velocity in rps +// motionMagicConfigs.MotionMagicAcceleration = +// PivotConstants.accelerationDegrees / 360.0 // Target acceleration in rps/s +//// motionMagicConfigs.MotionMagicJerk = 1600; Optional // Target jerk of 1600 rps/s/s (0.1 seconds) +// motorConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive +// +// val currentConfigs = motorConfiguration.CurrentLimits +// currentConfigs.StatorCurrentLimitEnable = true +// currentConfigs.StatorCurrentLimit = 60.0 +// +// motor.configurator.apply(motorConfiguration); +// +// // TODO: Also need to mess with this on bringup +// val currentPosition = +// encoder.get() * (24 / 32) * 360.0 // Clockwise should be positive and zero should probably be 45 degrees up from motor zero (position we likely won't start at but allowing for full rotation with the ratio) +// +// motor.setPosition((currentPosition - 45.0) / 360.0) +// } +// +// override fun getAngle(): Double { +// return motor.position.valueAsDouble * 360.0 +// } +// +// override fun getDesiredAngle(): Double { +// return desiredAngle +// } +// +// override fun atDesiredAngle() = +// MiscCalculations.appxEqual(getAngle(), desiredAngle, PivotConstants.accelerationDegrees) +// +// override fun setAngle(angleDegrees: Double) { +// desiredAngle = angleDegrees +// motor.setControl(motionMagic.withPosition(angleDegrees / 360.0)) +// } +// +// override fun periodic() {} +//} diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt index b19a669..2d34a86 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOSim.kt @@ -27,6 +27,10 @@ class PivotIOSim : PivotIO { desiredState = TrapezoidProfile.State(angleDegrees, 0.0) } + override fun killPID() { + + } + override fun periodic() { val currentTime = Timer.getFPGATimestamp() val dt = currentTime - lastTime diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt index 8ff506e..cb7aaa6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.kt @@ -2,7 +2,6 @@ package frc.robot.subsystems.swerve import com.pathplanner.lib.config.PIDConstants import edu.wpi.first.units.Units.* -import frc.robot.subsystems.swerve.TunerConstants object SwerveConstants { val MaxSpeedConst = TunerConstants.kSpeedAt12Volts.`in`(MetersPerSecond) // kSpeedAt12Volts desired top @@ -10,19 +9,25 @@ object SwerveConstants { // speed var ControlledSpeed = MaxSpeedConst val MaxAngularRateConst = - RotationsPerSecond.of(0.75).`in`(RadiansPerSecond) // 3/4 of a rotation per second max angular velocity + RotationsPerSecond.of(1.1).`in`(RadiansPerSecond) // 3/4 of a rotation per second max angular velocity var ControlledAngularRate = MaxAngularRateConst - val maxAutoSpeed = 2.0 - val maxAutoAccel = 3.0 +// val maxAutoSpeed = 2.8 +// val maxAutoAccel = 2.5 - val maxAutoTwist = 350.0 - val maxAutoTwistAccel = 450.0 - val translationPIDConstants = PIDConstants(16.0, 0.0, 0.0) - val rotationPIDConstants = PIDConstants(1.5, 0.0, 0.15) + val maxAutoSpeed = 2.25 + val maxAutoAccel = 1.575 + + val maxAutoTwistDegrees = 180.0 + val maxAutoTwistAccelDegrees = 180.0 + +// val translationPIDConstants = PIDConstants(16.0, 0.0, 0.0) + val translationPIDConstants = PIDConstants(9.0, 0.0, 0.005) + + val rotationPIDConstants = PIDConstants(1.9, 0.0, 0.05) val positionDeadzone = 0.01 val rotationDeadzone = 1.5 diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt index 8abf3f8..379606e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOReal.kt @@ -66,11 +66,6 @@ class SwerveIOReal() : SwerveIOBase() { override fun getSpeeds() = drivetrain.state.Speeds - override fun getAutoPath(name: String): Command { - return PathPlannerAuto(name) - } - - override fun seedFieldCentric() { drivetrain.seedFieldCentric() } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt b/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt index 965475a..9067c70 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIOSim.kt @@ -1,11 +1,11 @@ package frc.robot.subsystems.swerve -import com.ctre.phoenix6.swerve.SwerveDrivetrain import com.pathplanner.lib.auto.AutoBuilder -import com.pathplanner.lib.config.RobotConfig +import com.pathplanner.lib.commands.PathPlannerAuto import com.pathplanner.lib.config.PIDConstants +import com.pathplanner.lib.config.RobotConfig import com.pathplanner.lib.controllers.PPHolonomicDriveController -import com.pathplanner.lib.commands.PathPlannerAuto +import edu.wpi.first.math.MathUtil import edu.wpi.first.math.Matrix import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d @@ -14,24 +14,14 @@ import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.numbers.N1 import edu.wpi.first.math.numbers.N3 -import edu.wpi.first.math.util.Units import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.robot.constants.FieldConstants -import frc.robot.subsystems.swerve.SwerveIOBase class SwerveIOSim() : SwerveIOBase() { // private var robotPose = Pose2d() private var robotPose = - FieldConstants.Reef.centerFaces[0]!!.plus( - Transform2d( - Translation2d(Units.inchesToMeters(33.5 / 2 + 10.0), 0.0), - Rotation2d.fromDegrees(180.0) - ) - ) + Pose2d(Translation2d(7.6, 6.3), Rotation2d()) private var currentSpeed = ChassisSpeeds() @@ -71,7 +61,7 @@ class SwerveIOSim() : SwerveIOBase() { } init { - configurePathPlanner() +// configurePathPlanner() } override fun seedFieldCentric() { @@ -90,6 +80,8 @@ class SwerveIOSim() : SwerveIOBase() { override fun applyDriveRequest(x: Double, y: Double, twistRadians: Double) { // TODO Actually implement this +// val x = MathUtil.clamp(x, -SwerveConstants.MaxSpeedConst, SwerveConstants.MaxSpeedConst) +// val y = MathUtil.clamp(x, -SwerveConstants.MaxSpeedConst, SwerveConstants.MaxSpeedConst) currentSpeed = ChassisSpeeds(x, y, twistRadians) } @@ -99,18 +91,31 @@ class SwerveIOSim() : SwerveIOBase() { y: Double, twistRadians: Double ) { +// val x = MathUtil.clamp(x, -SwerveConstants.MaxSpeedConst, SwerveConstants.MaxSpeedConst) +// val y = MathUtil.clamp(x, -SwerveConstants.MaxSpeedConst, SwerveConstants.MaxSpeedConst) currentSpeed = ChassisSpeeds(x, y, twistRadians) } override fun periodic() { - val currentTime = Timer.getFPGATimestamp() + val currentTime = cshcyberhawks.lib.math.Timer.getFPGATimestamp() val dtSeconds = currentTime - lastLoopTime + val vx = MathUtil.clamp( + currentSpeed.vxMetersPerSecond, + -SwerveConstants.MaxSpeedConst, + SwerveConstants.MaxSpeedConst + ) + val vy = MathUtil.clamp( + currentSpeed.vyMetersPerSecond, + -SwerveConstants.MaxSpeedConst, + SwerveConstants.MaxSpeedConst + ) + robotPose = robotPose.plus( Transform2d( - currentSpeed.vxMetersPerSecond * dtSeconds, - currentSpeed.vyMetersPerSecond * dtSeconds, + vx * dtSeconds, + vy * dtSeconds, Rotation2d(currentSpeed.omegaRadiansPerSecond * dtSeconds) ) ) diff --git a/src/main/java/frc/robot/util/Vision.kt b/src/main/java/frc/robot/util/Vision.kt index 16fd03c..bc591de 100644 --- a/src/main/java/frc/robot/util/Vision.kt +++ b/src/main/java/frc/robot/util/Vision.kt @@ -16,38 +16,72 @@ import frc.robot.* class VisionSystem { val max_distance_m = 4.0 + val max_pose_diff = 2.0 + val limelightNames: Array = when (RobotConfiguration.robotType) { // RobotType.Real -> arrayOf("limelight-tright", "limelight-btfront") - RobotType.Real -> arrayOf("limelight-tleft", "limelight-tright", "limelight-btfront") + RobotType.Real -> arrayOf("limelight-tleft", "limelight-tright", "limelight-bfrontl", "limelight-bfrontr") else -> emptyArray() } + var hasInitM2 = false; + fun updateOdometryFromDisabled() { var namesToSearch: Array; - var fullReset = SmartDashboard.getBoolean("full reset with vision", false) + var fullReset = SmartDashboard.getBoolean("Disabled vision mode", false) + + if (!fullReset) { + return + } namesToSearch = limelightNames val headingDeg: Double = RobotContainer.drivetrain.getSwervePose().rotation.degrees + val bottomM2Reset = SmartDashboard.getBoolean("LL Bottom M2 Reset", false) + + if (bottomM2Reset && !hasInitM2) { + hasInitM2 = true + } + + + for (llName in namesToSearch) { if (DriverStation.getAlliance().isEmpty) { println("DS alliance is empty; skipping vision") return } + if (!bottomM2Reset) { + if (llName == "limelight-tright" || llName == "limelight-tleft") { + continue; + } + } + // LimelightHelpers.SetRobotOrientation(llName, headingDeg, 0.0, 0.0, 0.0, 0.0, 0.0); if (LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) == null) { // println(llName + " is null") SmartDashboard.putBoolean("ll " + llName + " is valid", false) - return; + continue; } SmartDashboard.putBoolean("ll " + llName + " is valid", true) - var llMeasure: LimelightHelpers.PoseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) + + if (hasInitM2 && bottomM2Reset) { + LimelightHelpers.SetRobotOrientation(llName, headingDeg, 0.0, 0.0, 0.0, 0.0, 0.0); + } + + var llMeasureRead: LimelightHelpers.PoseEstimate? = if(hasInitM2 && bottomM2Reset) LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llName) else LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) + + var llMeasure = LimelightHelpers.PoseEstimate() + if (llMeasureRead != null) { + llMeasure = llMeasureRead + } else { + continue + } // println("ll measure x: " + llMeasure.pose.x) // println("ll measure y: " + llMeasure.pose.y) @@ -64,28 +98,28 @@ class VisionSystem { // SmartDashboard.putNumber("ll update pose y: ", llMeasure.pose.y) // SmartDashboard.putNumber("ll timestampt: ", llMeasure.timestampSeconds) - if (fullReset) { - RobotContainer.drivetrain.resetPose(llMeasure.pose) - return; - } - if (distanceToTag < max_distance_m) { // println("doing the thingy") var xyStds: Double var degStds: Double if (llMeasure.tagCount >= 2) { - xyStds = 0.1 - degStds = 6.0 - } else if (llMeasure.avgTagArea > 0.8 && poseDifference < 0.5) { - xyStds = .3 - degStds = 8.0 - } else if (llMeasure.avgTagArea > 0.1 && poseDifference < 0.3) { - xyStds = .5 - degStds = 13.0 + xyStds = .75 + degStds = 110.0 + } else if (llMeasure.avgTagArea > .8) { + xyStds = 9.0 + degStds = 260.0 + } else if (llMeasure.avgTagArea > 0.2) { + xyStds = 10.0 + degStds = 280.0 } else { - xyStds = .8 - degStds = 25.0 + xyStds = 11.0 + degStds = 300.0 + } + + if (llName == "limelight-tright" || llName == "limelight-tleft") { + xyStds *= 4.0 + degStds *= 4.0 } RobotContainer.drivetrain.setVisionMeasurementStdDevs( @@ -113,47 +147,77 @@ class VisionSystem { for (llName in namesToSearch) { if (DriverStation.getAlliance().isEmpty) { // println("DS alliance is empty; skipping vision") - return + continue } -// LimelightHelpers.SetRobotOrientation(llName, headingDeg, 0.0, 0.0, 0.0, 0.0, 0.0); + LimelightHelpers.SetRobotOrientation(llName, headingDeg, 0.0, 0.0, 0.0, 0.0, 0.0); if (LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) == null) { // println(llName + " is null") SmartDashboard.putBoolean("ll " + llName + " is valid", false) - return; + continue; } SmartDashboard.putBoolean("ll " + llName + " is valid", true) - var llMeasure: LimelightHelpers.PoseEstimate = - LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) + var llMeasureRead: LimelightHelpers.PoseEstimate? = +// LimelightHelpers.getBotPoseEstimate_wpiBlue(llName) + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llName) + + + var llMeasure = LimelightHelpers.PoseEstimate() + if (llMeasureRead != null) { + llMeasure = llMeasureRead + } else { + continue + } if (llMeasure.tagCount >= tagCount && llMeasure.pose.x != 0.0 && llMeasure.pose.y != 0.0) { val poseDifference = llMeasure.pose.translation.getDistance(RobotContainer.drivetrain.getSwervePose().translation) - if (!poseDifferenceCheck || poseDifference < max_distance_m) { + if (!poseDifferenceCheck || poseDifference < max_pose_diff) { val distanceToTag = llMeasure.avgTagDist if (distanceToTag < max_distance_m) { var xyStds: Double var degStds: Double - if (llMeasure.tagCount >= 2) { - xyStds = 0.5 - degStds = 250.0 - } else if (llMeasure.avgTagArea > 0.8 && poseDifference < 0.5) { - xyStds = 1.0 - degStds = 800.0 - } else if (llMeasure.avgTagArea > 0.1 && poseDifference < 0.3) { - xyStds = 2.0 - degStds = 1300.0 - } else { - xyStds = 4.0 - degStds = 25000.0 - } + + degStds = 1800.0; + xyStds = Math.sqrt((1.0/(2.0 * (llMeasure.avgTagArea + .25)))) - .15; + + if (llMeasure.tagCount >= 2) { + xyStds *= .4; + } + +// if (llMeasure.tagCount >= 2) { +// xyStds = 0.05 +// degStds = 1250.0 +//// } else if (llMeasure.avgTagArea > 0.55 && poseDifference < 0.6) { +// } else if (llMeasure.avgTagArea > 1.75 && llName == "limelight-btfront") { +// xyStds = if (llMeasure.avgTagArea > 3.0) .1 else .2 +// degStds = 1800.0 +// +// } else if (llMeasure.avgTagArea > 0.4 && poseDifference < 0.3) { +// xyStds = 0.5 +// degStds = 1600.0 +// } +// else if (llMeasure.avgTagArea > .8) { +// xyStds = 1.05 +// degStds = 1900.0 +// } +// else { +// xyStds = 1.25 +// degStds = 25000.0 +// } + + + if (llName == "limelight-tright" || llName == "limelight-tleft") { + xyStds = 14.0 + degStds = 300.0 + } // var headingDeg = RobotContainer.drivetrain.getSwervePose().rotation @@ -165,8 +229,6 @@ class VisionSystem { // println("updating odometry with ll") // println("Updating with LL ${llName}: X = " + llMeasure.pose.x + " Y = " + llMeasure.pose.y) -// val finalPose = Pose2d(llMeasure.pose.x, llMeasure.pose.y, Rotation2d.fromDegrees(headingDeg)) - RobotContainer.drivetrain.addVisionMeasurement( llMeasure.pose, Utils.fpgaToCurrentTime(llMeasure.timestampSeconds) diff --git a/src/main/java/frc/robot/util/Visualizer.kt b/src/main/java/frc/robot/util/Visualizer.kt index 30e8a7b..01e2c89 100644 --- a/src/main/java/frc/robot/util/Visualizer.kt +++ b/src/main/java/frc/robot/util/Visualizer.kt @@ -11,6 +11,7 @@ import frc.robot.subsystems.superstructure.Superstructure object Visualizer { val robotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish(); + val targetPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Target Pose", Pose2d.struct).publish(); val componentPosePublisher = NetworkTableInstance.getDefault().getStructArrayTopic("Component Poses", Pose3d.struct).publish() diff --git a/src/main/java/frc/robot/util/input/ManualDriverInput.kt b/src/main/java/frc/robot/util/input/ManualDriverInput.kt index eff25fa..70f5d2c 100644 --- a/src/main/java/frc/robot/util/input/ManualDriverInput.kt +++ b/src/main/java/frc/robot/util/input/ManualDriverInput.kt @@ -3,9 +3,11 @@ package frc.robot.util.input import edu.wpi.first.wpilibj2.command.Commands import frc.robot.RobotContainer import frc.robot.RobotState +import frc.robot.subsystems.superstructure.Superstructure import java.util.* object ManualDriverInput { + var alignCommand = Commands.none() val cancel = 4; fun configureBindings() { @@ -22,11 +24,14 @@ object ManualDriverInput { })) RobotContainer.rightJoystick.button(3).onTrue(Commands.runOnce({ - OperatorControls.action.alignCommand.schedule() + OperatorControls.noWalk = false + alignCommand.cancel() + alignCommand = OperatorControls.action.alignCommand + alignCommand.schedule() })) RobotContainer.rightJoystick.button(3).onFalse(Commands.runOnce({ - OperatorControls.action.alignCommand.cancel() + alignCommand.cancel() })) RobotContainer.leftJoystick.button(3).onTrue(Commands.runOnce({ @@ -46,6 +51,11 @@ object ManualDriverInput { OperatorControls.action.superStructureCommand.schedule() println("running superstructure command") })) + + RobotContainer.leftJoystick.button(1).onTrue(Commands.runOnce({ +// Superstructure.correctIntake() + Superstructure.intakeFeeder() + })) } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/input/ManualOperatorInput.kt b/src/main/java/frc/robot/util/input/ManualOperatorInput.kt index e834223..ed86f47 100644 --- a/src/main/java/frc/robot/util/input/ManualOperatorInput.kt +++ b/src/main/java/frc/robot/util/input/ManualOperatorInput.kt @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.Commands import frc.robot.RobotContainer import frc.robot.RobotState import frc.robot.subsystems.superstructure.Superstructure -import frc.robot.subsystems.superstructure.pivot.PivotSystem +import frc.robot.subsystems.superstructure.climb.ClimbConstants object ManualOperatorInput { fun configureBindings() { @@ -41,7 +41,8 @@ object ManualOperatorInput { })) RobotContainer.xbox.b().onTrue(Commands.runOnce({ // Superstructure.request(Superstructure.pivotSystem.algaeRemoveAngle()) - Superstructure.scoreL3() +// Superstructure.scoreL3() + Superstructure.climbStowThenStow() })) RobotContainer.xbox.x().onTrue(Commands.runOnce({ Superstructure.intakeFeeder() @@ -49,13 +50,26 @@ object ManualOperatorInput { // Superstructure.request(Superstructure.pivotSystem.l3Angle()) })) RobotContainer.xbox.leftBumper().onTrue(Commands.runOnce({ - Superstructure.removeAlgaeLow() +// Superstructure.requestSuperstructureAction(Superstructure.climbSystem.stow())/\\ + + ClimbConstants.climbAngle -= 2.5; + Superstructure.requestSuperstructureAction(Superstructure.climbSystem.climb()) +// Superstructure.requestSuperstructureAction(Superstructure.funnelSystem.stow()) +// Superstructure.removeAlgaeLow() })) RobotContainer.xbox.rightBumper().onTrue(Commands.runOnce({ - Superstructure.removeAlgaeHigh() + + ClimbConstants.climbAngle += 2.5; + Superstructure.requestSuperstructureAction(Superstructure.climbSystem.climb()) + +// println("doing the dpl") +// Superstructure.requestSuperstructureAction(Superstructure.climbSystem.deploy()) +// Superstructure.requestSuperstructureAction(Superstructure.funnelSystem.deploy()) +// Superstructure.removeAlgaeHigh() })) RobotContainer.xbox.a().onTrue(Commands.runOnce({ - Superstructure.scoreBarge() + Superstructure.climb() +// Superstructure.scoreBarge() // Superstructure.request(Superstructure.pivotSystem.stowAngle()) })) RobotContainer.xbox.povUp().onTrue(Commands.runOnce({ diff --git a/src/main/java/frc/robot/util/input/OperatorControls.kt b/src/main/java/frc/robot/util/input/OperatorControls.kt index a7a2178..833fd97 100644 --- a/src/main/java/frc/robot/util/input/OperatorControls.kt +++ b/src/main/java/frc/robot/util/input/OperatorControls.kt @@ -3,20 +3,21 @@ package frc.robot.util.input import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands -import frc.robot.commands.AutoAlign +import frc.robot.commands.TeleopAutoAlign import frc.robot.constants.AutoScoringConstants import frc.robot.subsystems.superstructure.Superstructure enum class DriverAction(val superStructureCommand: Command, val alignCommand: Command = Commands.runOnce({})) { -// Stow(Commands.runOnce({ Superstructure.stow() })), - IntakeFeeder(Commands.runOnce({ Superstructure.intakeFeeder() })), - ScoreL2(Commands.runOnce({ Superstructure.scoreL2() }), AutoAlign.coralReefAlign()), - ScoreL3(Commands.runOnce({ Superstructure.scoreL3() }), AutoAlign.coralReefAlign()), - ScoreL4(Commands.runOnce({ Superstructure.scoreL4() }), AutoAlign.coralReefAlign()), - RemoveAlgaeLow(Commands.runOnce({ Superstructure.removeAlgaeLow() }), AutoAlign.algaeReefAlign()), - RemoveAlgaeHigh(Commands.runOnce({ Superstructure.removeAlgaeHigh() }), AutoAlign.algaeReefAlign()), + // Stow(Commands.runOnce({ Superstructure.stow() })), + IntakeFeeder(Commands.runOnce({ Superstructure.intakeFeeder() }), TeleopAutoAlign.feederAlign()), + ScoreL2(Commands.runOnce({ Superstructure.scoreL2() }), TeleopAutoAlign.coralReefAlign()), + ScoreL3(Commands.runOnce({ Superstructure.scoreL3() }), TeleopAutoAlign.coralReefAlign()), + ScoreL4(Commands.runOnce({ Superstructure.scoreL4() }), TeleopAutoAlign.coralReefAlign()), + RemoveAlgaeLow(Commands.runOnce({ Superstructure.removeAlgaeLow() }), TeleopAutoAlign.algaeReefAlign()), + RemoveAlgaeHigh(Commands.runOnce({ Superstructure.removeAlgaeHigh() }), TeleopAutoAlign.algaeReefAlign()), ScoreBarge(Commands.runOnce({ Superstructure.scoreBarge() })), - ScoreProcessor(Commands.runOnce({ Superstructure.scoreProcessor() })), + ScoreProcessor(Commands.runOnce({ Superstructure.scoreProcessor() }), TeleopAutoAlign.processorAlign()), + Climb(Commands.runOnce({ Superstructure.climb() })), None(Commands.runOnce({})) } @@ -26,6 +27,8 @@ enum class CoralSide { } object OperatorControls { + var noWalk = false + val action get() = when (SmartDashboard.getString("ConsoleDriverAction", "")) { "Feeder" -> DriverAction.IntakeFeeder @@ -36,8 +39,9 @@ object OperatorControls { "Processor" -> DriverAction.ScoreProcessor "Algae High" -> DriverAction.RemoveAlgaeHigh "Algae Low" -> DriverAction.RemoveAlgaeLow + "Cage" -> DriverAction.Climb else -> DriverAction.None - } + } val coralSide get() = when (SmartDashboard.getString("ConsoleCoralSide", "")) { diff --git a/vendordeps/PathplannerLib-2025.2.3.json b/vendordeps/PathplannerLib-2025.2.6.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.3.json rename to vendordeps/PathplannerLib-2025.2.6.json index 9151ce4..95ba203 100644 --- a/vendordeps/PathplannerLib-2025.2.3.json +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.3.json", + "fileName": "PathplannerLib-2025.2.6.json", "name": "PathplannerLib", - "version": "2025.2.3", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.3" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.3", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,