diff --git a/AdvantageScopeSwerve.json b/AdvantageScopeSwerve.json index 30463e8..4880e03 100644 --- a/AdvantageScopeSwerve.json +++ b/AdvantageScopeSwerve.json @@ -1,28 +1,78 @@ { "hubs": [ { - "x": -8, - "y": -8, - "width": 1680, - "height": 1077, + "x": -1080, + "y": 609, + "width": 1080, + "height": 936, "state": { "sidebar": { - "width": 408, + "width": 300, "expanded": [ "/SmartDashboard/MapleSim/MatchData/Breakdown/blue Alliance", "/SmartDashboard/swerve/advantagescope/currentStates/1/angle", "/SmartDashboard/swerve/advantagescope/currentStates/0/angle", "/SmartDashboard/swerve/advantagescope/currentStates/3", "/SmartDashboard/swerve/advantagescope/currentStates/3/angle", - "/AdvantageKit", - "/AdvantageKit/RealOutputs/FieldSimulation", - "/AdvantageKit/RealOutputs", - "/SmartDashboard", "/SmartDashboard/swerve/modules", "/SmartDashboard/swerve/modules/frontright", "/SmartDashboard/swerve/modules/frontleft", "/SmartDashboard/swerve/modules/backright", - "/SmartDashboard/swerve/modules/backleft" + "/SmartDashboard/swerve/modules/backleft", + "/Mechanisms/Intake/IntakeMotor/rotor", + "/Mechanisms/Intake/IntakeMotor/current", + "/Mechanisms/Intake/IntakeMotor/motor", + "/Mechanisms/Hopper/HopperMotor/mechanism", + "/Mechanisms/Hopper/HopperMotor/rotor", + "/Mechanisms/Hopper/HopperMotor/current", + "/Mechanisms/IntakePivot/IntakePivotMotor", + "/Mechanisms/IntakePivot/IntakePivotMotor/rotor", + "/Mechanisms/IntakePivot/IntakePivotMotor/mechanism", + "/Mechanisms/Hopper/HopperMotor", + "/Mechanisms/Hopper/HopperMotor/motor", + "/Mechanisms/IntakePivot/IntakePivotMotor/closedloop", + "/Mechanisms/IntakePivot/IntakePivotMotor/closedloop/setpoint", + "/TheThriftyBot/CAN_15", + "/TheThriftyBot/CAN_16", + "/AdvantageKit/RealOutputs/DriverControls", + "/Mechanisms/Shooter/ShooterMotor", + "/Mechanisms/Shooter/ShooterMotor/closedloop", + "/Mechanisms/Shooter/ShooterMotor/closedloop/setpoint", + "/Mechanisms/Shooter/ShooterMotor/current", + "/Mechanisms/Shooter/ShooterMotor/mechanism", + "/Mechanisms/Shooter/ShooterMotor/motor", + "/Mechanisms/Shooter/ShooterMotor/rotor", + "/Tuning/Shooter/ShooterMotor/closedloop/feedback", + "/Tuning/Shooter/ShooterMotor/closedloop/feedforward", + "/Tuning/Shooter/ShooterMotor/closedloop/motionprofile", + "/Tuning/Shooter/ShooterMotor/closedloop/setpoint", + "/Tuning/Shooter/ShooterMotor/mechanism", + "/Tuning/Shooter/ShooterMotor/mechanism/limit", + "/Tuning/Shooter/ShooterMotor/ramprate", + "/Tuning/Turret/TurretMotor", + "/Tuning/Turret/TurretMotor/closedloop", + "/Tuning/Turret/TurretMotor/current", + "/Tuning/Turret/TurretMotor/current/limit", + "/Tuning/Turret/TurretMotor/closedloop/setpoint", + "/Tuning/Turret/TurretMotor/closedloop/motionprofile", + "/Tuning/Turret/TurretMotor/closedloop/feedforward", + "/Tuning/Turret/TurretMotor/closedloop/feedback", + "/Mechanisms/Turret", + "/Mechanisms/Turret/TurretMotor", + "/Mechanisms/Turret/TurretMotor/closedloop", + "/Mechanisms/Turret/TurretMotor/closedloop/setpoint", + "/Mechanisms/Turret/TurretMotor/current", + "/Mechanisms/Turret/TurretMotor/mechanism", + "/Mechanisms/Turret/TurretMotor/motor", + "/Mechanisms/Turret/TurretMotor/rotor", + "/SmartDashboard/Mechanisms/Commands", + "/Tuning/Turret", + "/AdvantageKit", + "/AdvantageKit/RealOutputs", + "/AdvantageKit/RealOutputs/ShootOnTheMove/RobotHeading", + "/AdvantageKit/RealOutputs/ASCalibration", + "/AdvantageKit/RealOutputs/FieldSimulation", + "/AdvantageKit/RealOutputs/FieldSimulation/Shooter" ] }, "tabs": { @@ -47,9 +97,16 @@ "logType": "Pose2d", "visible": true, "options": { - "model": "Crab Bot" + "model": "2026 CA26" } }, + { + "type": "component", + "logKey": "NT:/AdvantageKit/RealOutputs/ASCalibration/FinalComponentPoses", + "logType": "Pose3d[]", + "visible": true, + "options": {} + }, { "type": "ghost", "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/TargetPose", @@ -69,27 +126,160 @@ "model": "Crab Bot", "color": "#00ff00" } + }, + { + "type": "ghost", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/AimTarget", + "logType": "Pose3d", + "visible": true, + "options": { + "model": "Duck Bot", + "color": "#00ffff" + } + }, + { + "type": "ghost", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/AimTargetCorrected", + "logType": "Pose3d", + "visible": true, + "options": { + "model": "Duck Bot", + "color": "#ff00ff" + } + }, + { + "type": "trajectory", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Shooter/ProjectileSuccessfulShot", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "trajectory", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/Shooter/ProjectileUnsuccessfulShot", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#ff0000", + "size": "normal" + } + }, + { + "type": "gamePiece", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/FuelPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "variant": "Fuel" + } } ], - "game": "2025 Field (Welded)", - "origin": "blue" + "game": "2026 Field", + "origin": "auto" }, "controllerUUID": "56hy5kyvd0kjxfyigyn0v5dx374uh50y", "renderer": { "cameraIndex": -1, "orbitFov": 50, "cameraPosition": [ - -8.213793838348083, - 9.984019446636196, - -2.8847015884021476 + 10.378995714785386, + 9.101397756194608, + -0.10752311567010889 ], "cameraTarget": [ - -4.333498230620374, - -1.8078313603407625, - 0.2364458308149924 + 4.495231078701326, + -1.3207821976493825, + -0.22073703807687897 ] }, - "controlsHeight": 329 + "controlsHeight": -570 + }, + { + "type": 1, + "title": "SOTM", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/ShootOnTheMove/RobotHeading/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/ShootOnTheMove/CalculatedHeading", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "r4osgrvtuob9ky26vtvjubxgpml80oc1", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Commands", + "controller": { + "leftSources": [], + "rightSources": [], + "discreteSources": [ + { + "type": "alerts", + "logKey": "NT:/AdvantageKit/RealOutputs/CommandScheduler/Running", + "logType": "Alerts", + "visible": true, + "options": {} + }, + { + "type": "alerts", + "logKey": "NT:/AdvantageKit/RealOutputs/CommandScheduler/Subsystems", + "logType": "Alerts", + "visible": true, + "options": {} + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "pigoam74jesfb4nbj26dep7viktz08f8", + "renderer": null, + "controlsHeight": 200 }, { "type": 9, @@ -133,28 +323,131 @@ "renderer": null, "controlsHeight": 200 }, + { + "type": 5, + "title": "Console", + "controller": "NT:/AdvantageKit/RealOutputs/Console", + "controllerUUID": "uv5up5g3ebsg6srxe6xpjd0wk37924t2", + "renderer": { + "highlight": false + }, + "controlsHeight": 0 + }, { "type": 1, - "title": "Commands", + "title": "Shooter", "controller": { - "leftSources": [], - "rightSources": [], - "discreteSources": [ + "leftSources": [ { - "type": "alerts", - "logKey": "NT:/AdvantageKit/RealOutputs/CommandScheduler/Running", - "logType": "Alerts", + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/FieldSimulation/hubDiff", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Shooter/FollowerVelocity", + "logType": "Number", "visible": true, - "options": {} + "options": { + "color": "#e5b31b", + "size": "normal" + } }, { - "type": "alerts", - "logKey": "NT:/AdvantageKit/RealOutputs/CommandScheduler/Subsystems", - "logType": "Alerts", + "type": "stepped", + "logKey": "NT:/AdvantageKit/RealOutputs/Shooter/LeaderVelocity", + "logType": "Number", + "visible": false, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "43xa5zkag3om7orp2mzuhjxyyleru2bp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turret", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Turret/TurretMotor/mechanism/velocity", + "logType": "Number", + "visible": false, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Turret/TurretMotor/closedloop/setpoint/velocity", + "logType": "Number", + "visible": false, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Turret/TurretMotor/motor/outputVoltage", + "logType": "Number", + "visible": false, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Turret/TurretMotor/mechanism/position", + "logType": "Number", "visible": true, - "options": {} + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Turret/TurretMotor/closedloop/setpoint/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } } ], + "discreteSources": [], "leftLockedRange": null, "rightLockedRange": null, "leftUnitConversion": { @@ -168,19 +461,177 @@ "leftFilter": 0, "rightFilter": 0 }, - "controllerUUID": "pigoam74jesfb4nbj26dep7viktz08f8", + "controllerUUID": "y8fk3c1rybbz1fz244jf954wsqjto7ys", "renderer": null, "controlsHeight": 200 }, { - "type": 5, - "title": "Console", - "controller": "NT:/AdvantageKit/RealOutputs/Console", - "controllerUUID": "uv5up5g3ebsg6srxe6xpjd0wk37924t2", - "renderer": { - "highlight": false + "type": 1, + "title": "IntakeRollers", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Intake/IntakeMotor/mechanism/velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Intake/IntakeMotor/motor/outputVoltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Intake/IntakeMotor/current/stator", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "NT:/FMSInfo/FMSControlData", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 }, - "controlsHeight": 0 + "controllerUUID": "3w53yo8dirw5hm0kew03f0eufaery3ix", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Hopper", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Hopper/HopperMotor/mechanism/velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/Hopper/HopperMotor/rotor/velocity", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "7be9mz4ju6no9ykvx2ciao2zj05bezzx", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "IntakePivot", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/IntakePivot/IntakePivotMotor/rotor/position", + "logType": "Number", + "visible": false, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "NT:/Mechanisms/IntakePivot/IntakePivotMotor/mechanism/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "NT:/Mechanisms/IntakePivot/IntakePivotMotor/closedloop/setpoint/position", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "wrkalnnws4h9aaj7fgcj2lmnl42eibne", + "renderer": null, + "controlsHeight": 200 } ] } diff --git a/assets/Robot_CA26/config.json b/assets/Robot_CA26/config.json new file mode 100644 index 0000000..cdcbbcf --- /dev/null +++ b/assets/Robot_CA26/config.json @@ -0,0 +1,38 @@ +{ + "name": "2026 CA26", + "rotations": [ + { + "axis": "x", + "degrees": 90.0 + }, + { + "axis": "y", + "degrees": 0.0 + }, + { + "axis": "z", + "degrees": 90.0 + } + ], + "position": [0.0, 0.0, 0.0], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90.0 + }, + { + "axis": "y", + "degrees": 0.0 + }, + { + "axis": "z", + "degrees": 90.0 + } + ], + "zeroedPosition": [0.205, 0.0, -0.375] + } + ] +} diff --git a/assets/Robot_CA26/model.glb b/assets/Robot_CA26/model.glb new file mode 100644 index 0000000..578d7d9 Binary files /dev/null and b/assets/Robot_CA26/model.glb differ diff --git a/assets/Robot_CA26/model_0.glb b/assets/Robot_CA26/model_0.glb new file mode 100644 index 0000000..eae739c Binary files /dev/null and b/assets/Robot_CA26/model_0.glb differ diff --git a/simgui-ds.json b/simgui-ds.json index 464a7db..d22e3cd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -94,10 +94,6 @@ "guid": "78696e70757401000000000000000000", "useGamepad": true }, - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - }, { "guid": "78696e70757401000000000000000000", "useGamepad": true diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 348f7b7..20ca3b1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,16 +1,16 @@ package frc.robot; import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnField; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.util.CommandsLogging; +import frc.robot.util.maplesim.Arena2026Rebuilt; public class Robot extends LoggedRobot { private Command m_autonomousCommand; @@ -52,16 +52,15 @@ public void robotPeriodic() { CommandsLogging.logRequiredSubsystems(); if (Robot.isSimulation()) { - Pose3d[] algaePoses = arena.getGamePiecesArrayByType("Algae"); - Logger.recordOutput("FieldSimulation/AlgaePoses", algaePoses); - - Pose3d[] coralPoses = arena.getGamePiecesArrayByType("Coral"); - Logger.recordOutput("FieldSimulation/CoralPoses", coralPoses); + Pose3d[] fuelPoses = arena.getGamePiecesArrayByType("Fuel"); + Logger.recordOutput("FieldSimulation/FuelPoses", fuelPoses); } Logger.recordOutput("FieldSimulation/RobotPose", m_robotContainer.getRobotPose()); Logger.recordOutput("FieldSimulation/TargetPose", m_robotContainer.getSwerveDrive().field.getObject("targetPose").getPose()); + Logger.recordOutput("FieldSimulation/AimDirection", m_robotContainer.getAimDirection()); + Logger.recordOutput("FieldSimulation/AimTarget", new Pose3d(m_robotContainer.getAimPoint(), Rotation3d.kZero)); } @Override @@ -108,9 +107,15 @@ public void testPeriodic() { @Override public void simulationInit() { + // Shut down the old arena instance first to release ownership of all bodies + // (including the drivetrain) so they can be added to a new physics world + SimulatedArena.getInstance().shutDown(); + + SimulatedArena.overrideInstance(new Arena2026Rebuilt()); + arena = SimulatedArena.getInstance(); - arena.addGamePiece(new ReefscapeAlgaeOnField(new Translation2d(14, 1))); + arena.addDriveTrainSimulation(m_robotContainer.getSwerveDrive().getMapleSimDrive().get()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 629eeea..d87564a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,9 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -16,24 +19,26 @@ import frc.robot.Constants.ControllerConstants; import frc.robot.controls.DriverControls; import frc.robot.controls.OperatorControls; -import frc.robot.controls.PoseControls; +import frc.robot.subsystems.HoodSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.KickerSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.TurretSubsystem; import swervelib.SwerveDrive; public class RobotContainer { private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); - + private final TurretSubsystem turret = new TurretSubsystem(); + private final ShooterSubsystem shooter = new ShooterSubsystem(); private final IntakeSubsystem intake = new IntakeSubsystem(); private final HopperSubsystem hopper = new HopperSubsystem(); private final KickerSubsystem kicker = new KickerSubsystem(); - private final ShooterSubsystem shooter = new ShooterSubsystem(); + private final HoodSubsystem hood = new HoodSubsystem(); - private final Superstructure superstructure = new Superstructure(shooter, null, null, intake, hopper, kicker); + private final Superstructure superstructure = new Superstructure(shooter, turret, hood, intake, hopper, kicker); private final SendableChooser autoChooser; @@ -45,7 +50,7 @@ public RobotContainer() { configureBindings(); buildNamedAutoCommands(); - if (!Robot.isReal()) { + if (!Robot.isReal() || true) { DriverStation.silenceJoystickConnectionWarning(true); } @@ -102,4 +107,18 @@ public SwerveDrive getSwerveDrive() { public Pose2d getRobotPose() { return drivebase.getPose(); } + + public Pose3d getAimDirection() { + // Apply robot heading first, then turret/hood rotation on top + Pose3d shooterPose = superstructure.getShooterPose(); + + var pose = drivebase.getPose3d().plus(new Transform3d( + shooterPose.getTranslation(), shooterPose.getRotation())); + + return pose; + } + + public Translation3d getAimPoint() { + return superstructure.getAimPoint(); + } } diff --git a/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java b/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java new file mode 100644 index 0000000..8fd7849 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootOnTheMoveCommand.java @@ -0,0 +1,169 @@ +package frc.robot.commands; + +import java.util.Map; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.SwerveSubsystem; + +public class ShootOnTheMoveCommand extends Command { + private final SwerveSubsystem drivetrain; + private final Superstructure superstructure; + + private Supplier aimPointSupplier; // The point to aim at + private AngularVelocity latestShootSpeed; + private Angle latestHoodAngle; + private Angle latestTurretAngle; + + public ShootOnTheMoveCommand(SwerveSubsystem drivetrain, Superstructure superstructure, + Supplier aimPointSupplier) { + this.drivetrain = drivetrain; + this.superstructure = superstructure; + this.aimPointSupplier = aimPointSupplier; + + // We use the drivetrain to determine linear velocity, but don't require it for + // control. We + // will be using the superstructure to control the shooting mechanism so it's a + // requirement. + // addRequirements(superstructure); + + // TODO: figure out if the above is actually required. Right now, when you start + // some other command, the auto aim can't start back up again + } + + @Override + public void initialize() { + super.initialize(); + + latestHoodAngle = superstructure.getHoodAngle(); + latestTurretAngle = superstructure.getTurretAngle(); + latestShootSpeed = superstructure.getShooterSpeed(); + + // TODO: when this current command ends, we should probably cancel the dynamic + // aim command + superstructure.aimDynamicCommand( + () -> { + return this.latestShootSpeed; + }, + () -> { + return this.latestTurretAngle; + }, + () -> { + return this.latestHoodAngle; + }).schedule(); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void execute() { + // Calculate trajectory to aimPoint + var target = aimPointSupplier.get(); + + var shooterLocation = drivetrain.getPose3d().getTranslation() + .plus(superstructure.getShooterPose().getTranslation()); + + // Ignore this parameter for now, the range tables will account for it :/ + // var deltaH = target.getMeasureZ().minus(shooterLocation.getMeasureZ()); + var shooterOnGround = new Translation2d(shooterLocation.getX(), shooterLocation.getY()); + var targetOnGround = new Translation2d(target.getX(), target.getY()); + + var distanceToTarget = Meters.of(shooterOnGround.getDistance(targetOnGround)); + + // Get time of flight. We could try to do this analytically but for now it's + // easier and more realistic + // to use a simple linear approximation based on empirical data. + double timeOfFlight = getFlightTime(distanceToTarget); + + // Calculate corrective vector based on our current velocity multiplied by time + // of flight. + // If we're stationary, this should be zero. If we're backing up, this will be + // "ahead" of the target, etc. + var updatedPosition = drivetrain.getFieldVelocity().times(timeOfFlight); + var correctiveVector = new Translation2d(updatedPosition.vxMetersPerSecond, updatedPosition.vyMetersPerSecond) + .unaryMinus(); + var correctiveVector3d = new Translation3d(correctiveVector.getX(), correctiveVector.getY(), 0); + + Logger.recordOutput("FieldSimulation/AimTargetCorrected", + new Pose3d(target.plus(correctiveVector3d), Rotation3d.kZero)); + + var correctedTarget = targetOnGround.plus(correctiveVector); + + var vectorToTarget = drivetrain.getPose().getTranslation().minus(correctedTarget); + + var correctedDistance = Meters.of(vectorToTarget.getNorm()); + var calculatedHeading = vectorToTarget.getAngle() + .rotateBy(drivetrain.getHeading().unaryMinus()) + .getMeasure(); + + Logger.recordOutput("ShootOnTheMove/RobotHeading", drivetrain.getHeading()); + Logger.recordOutput("ShootOnTheMove/CalculatedHeading", calculatedHeading); + Logger.recordOutput("ShootOnTheMove/distanceToTarget", distanceToTarget); + + latestTurretAngle = calculatedHeading; + latestShootSpeed = calculateRequiredShooterSpeed(correctedDistance); + + // TODO: add this back if/when we have a real hood, for now, just set it to the + // current angle + // latestHoodAngle = calculateRequiredHoodAngle(correctedDistance); + latestHoodAngle = superstructure.getHoodAngle(); + + superstructure.setShooterSetpoints( + latestShootSpeed, + latestTurretAngle, + latestHoodAngle); + + // System.out.println("Shooting at distance: " + correctedDistance + " requires + // speed: " + latestShootSpeed + // + ", hood angle: " + latestHoodAngle + ", turret angle: " + + // latestTurretAngle); + } + + private double getFlightTime(Distance distanceToTarget) { + // Simple linear approximation based on empirical data. + return TIME_OF_FLIGHT_BY_DISTANCE.get(distanceToTarget.in(Meters)); + } + + private AngularVelocity calculateRequiredShooterSpeed(Distance distanceToTarget) { + return RPM.of(SHOOTING_SPEED_BY_DISTANCE.get(distanceToTarget.in(Meters))); + } + + private Angle calculateRequiredHoodAngle(Distance distanceToTarget) { + return Degrees.of(HOOD_ANGLE_BY_DISTANCE.get(distanceToTarget.in(Meters))); + } + + // meters, seconds + private static final InterpolatingDoubleTreeMap TIME_OF_FLIGHT_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries( + Map.entry(1.0, 1.0), + Map.entry(4.86, 1.5)); + + // meters, RPS + private static final InterpolatingDoubleTreeMap SHOOTING_SPEED_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries( + Map.entry(2.0, 2700.0), + Map.entry(3.0, 3000.0), + Map.entry(4.0, 3300.0), + Map.entry(4.86, 3750.0)); + + // meters, degrees + private static final InterpolatingDoubleTreeMap HOOD_ANGLE_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries( + Map.entry(1.0, 15.0), + Map.entry(2.0, 30.0), + Map.entry(3.0, 45.0)); +} diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index ef701d8..0764568 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -1,21 +1,15 @@ package frc.robot.controls; import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; +import org.ironmaple.simulation.gamepieces.GamePieceProjectile; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Feet; -import static edu.wpi.first.units.Units.FeetPerSecond; import static edu.wpi.first.units.Units.Meter; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -24,6 +18,7 @@ import frc.robot.Robot; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.util.maplesim.RebuiltFuelOnFly; import swervelib.SwerveInputStream; public class DriverControls { @@ -48,15 +43,15 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur .withControllerRotationAxis(() -> controller.getRightX() * -1) .robotRelative(false) .allianceRelativeControl(true) - // .scaleTranslation(0.25) // TODO: Tune speed scaling + .scaleTranslation(0.25) // TODO: Tune speed scaling .deadband(ControllerConstants.DEADBAND); - controller.rightBumper().whileTrue(Commands.run( - () -> { - driveInputStream - .aim(getTargetPose()) - .aimWhile(true); - }).finallyDo(() -> driveInputStream.aimWhile(false))); + // controller.rightBumper().whileTrue(Commands.run( + // () -> { + // driveInputStream + // .aim(getTargetPose()) + // .aimWhile(true); + // }).finallyDo(() -> driveInputStream.aimWhile(false))); drivetrain.setDefaultCommand( drivetrain.driveFieldOriented(driveInputStream).withName("Drive" + ".test")); @@ -85,48 +80,54 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur // Overrides drive command above! // Might be useful for robot-oriented controls in testing - controller.a().onTrue((Commands.runOnce(drivetrain::zeroGyro))); controller.b().whileTrue(drivetrain.centerModulesCommand()); controller.x().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); controller.y().onTrue((Commands.runOnce(drivetrain::zeroGyro))); controller.start().whileTrue(drivetrain.sysIdAngleMotorCommand()); controller.back().whileTrue(drivetrain.sysIdDriveMotorCommand()); - - controller.leftBumper().onTrue(Commands.none()); } else if (Robot.isSimulation()) { - controller.back().whileTrue(fireAlgae(drivetrain)); + // Fire fuel 10 times per second while button is held + controller.back().whileTrue( + Commands.repeatingSequence( + fireFuel(drivetrain, superstructure), + Commands.waitSeconds(0.1))); } else { controller.start().onTrue((Commands.runOnce(drivetrain::zeroGyro))); - controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); + + controller.leftBumper().whileTrue( + superstructure.feedAllCommand() + .finallyDo(() -> superstructure.stopFeedingAllCommand().schedule())); + + controller.rightBumper() + .whileTrue(superstructure.setIntakeDeployAndRoll().withName("OperatorControls.intakeDeployed")); + } } - public static Command fireAlgae(SwerveSubsystem drivetrain) { + public static Command fireFuel(SwerveSubsystem drivetrain, Superstructure superstructure) { return Commands.runOnce(() -> { - System.err.println("FIRE!"); - SimulatedArena arena = SimulatedArena.getInstance(); - // Translation2d robotPosition, - // Translation2d shooterPositionOnRobot, - // ChassisSpeeds chassisSpeeds, - // Rotation2d shooterFacing, - // Distance initialHeight, - // LinearVelocity launchingSpeed, - // Angle shooterAngle + System.out.println("FIRE!"); - ReefscapeAlgaeOnFly algae = new ReefscapeAlgaeOnFly( + GamePieceProjectile fuel = new RebuiltFuelOnFly( drivetrain.getPose().getTranslation(), - new Translation2d(), - drivetrain.getSwerveDrive().getRobotVelocity().times(-1), - drivetrain.getSwerveDrive().getOdometryHeading(), - Distance.ofBaseUnits(1, Feet), - LinearVelocity.ofBaseUnits(5, FeetPerSecond), - Angle.ofBaseUnits(45, Degrees)); + new Translation2d( + superstructure.turret.turretTranslation.getX() * -1, + superstructure.turret.turretTranslation.getY()), + drivetrain.getSwerveDrive().getRobotVelocity(), + superstructure.getAimRotation3d().toRotation2d(), + Feet.of(superstructure.turret.turretTranslation.getZ()), + + // based on numbers from https://www.reca.lc/flywheel + // Adjust for simulation tuning + // 0.5 times because we're applying spin to the fuel as we shoot it + superstructure.getTangentialVelocity().times(0.5), + superstructure.getHoodAngle()); // Configure callbacks to visualize the flight trajectory of the projectile - algae.withProjectileTrajectoryDisplayCallBack( + fuel.withProjectileTrajectoryDisplayCallBack( // Callback for when the note will eventually hit the target (if configured) (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot", pose3ds.toArray(Pose3d[]::new)), @@ -135,7 +136,7 @@ public static Command fireAlgae(SwerveSubsystem drivetrain) { (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot", pose3ds.toArray(Pose3d[]::new))); - arena.addGamePieceProjectile(algae); - }).withName("Fire.Algae"); + arena.addGamePieceProjectile(fuel); + }); } } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index a8580d1..61c239a 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -1,34 +1,65 @@ package frc.robot.controls; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.gamepieces.GamePieceProjectile; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import static edu.wpi.first.units.Units.Feet; +import static edu.wpi.first.units.Units.FeetPerSecond; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.ShootOnTheMoveCommand; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.util.maplesim.RebuiltFuelOnFly; public class OperatorControls { + public static final boolean MACOS_WEIRD_CONTROLLER = true; public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { CommandXboxController controller = new CommandXboxController(port); - // Intake controls - A to intake, B to eject - // controller.a().whileTrue(superstructure.intakeCommand()); - // controller.b().whileTrue(superstructure.ejectCommand()); + // if (Robot.isSimulation()) { + // controller.leftBumper().whileTrue(aimCommand(drivetrain, superstructure)); + // controller.start().whileTrue(fireAlgae(drivetrain, superstructure)); + + // Commands.run(() -> { + // double leftX = controller.getLeftX(); + // double leftY = controller.getLeftY(); + // double rightY = controller.getRightY(); + + // if (MACOS_WEIRD_CONTROLLER) { + // rightY = controller.getRightTriggerAxis() - controller.getLeftTriggerAxis(); + // } + + // // Apply deadband + // if (Math.abs(leftX) < Constants.ControllerConstants.DEADBAND) + // leftX = 0; + // if (Math.abs(leftY) < Constants.ControllerConstants.DEADBAND) + // leftY = 0; + // if (Math.abs(rightY) < Constants.ControllerConstants.DEADBAND) + // rightY = 0; - // Hopper controls - X to run hopper forward, Y to run backward - // controller.x().whileTrue(superstructure.hopperFeedCommand()); - // controller.y().whileTrue(superstructure.hopperReverseCommand()); + // Translation3d translation = new Translation3d(leftX, leftY, rightY); - // Shooter controls - Right bumper to shoot - // controller.rightBumper().whileTrue(superstructure.shootCommand()); - // controller.leftBumper().whileTrue(superstructure.stopShootingCommand()); + // if (MACOS_WEIRD_CONTROLLER) { + // // MacOS Xbox controller mapping is weird - swap X and Y + // translation = new Translation3d(leftY, leftX, rightY); + // } - // Kicker controls - // controller.back().whileTrue(superstructure.kickerFeedCommand()); - // controller.start().whileTrue(superstructure.kickerStopCommand()); + // // System.out.println("Adjusting pose by: " + translation.toString()); - // Intake pivot controls - // controller.povUp().onTrue(superstructure.setIntakeStow()); - // controller.povRight().onTrue(superstructure.setIntakeHold()); - // controller.povRight().onTrue(superstructure.setIntakeDeployed()); + // var newAimPoint = superstructure.getAimPoint().plus(translation.times(0.05)); + // // new Transform3d(leftX * 0.05, leftY * 0.05, rightY * 0.05)); + + // superstructure.setAimPoint(newAimPoint); + // }).ignoringDisable(true).schedule(); + // } // REAL CONTROLS controller.start().onTrue(superstructure.rezeroIntakePivotCommand().ignoringDisable(true)); @@ -36,13 +67,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur controller.rightBumper() .whileTrue(superstructure.setIntakeDeployAndRoll().withName("OperatorControls.intakeDeployed")); - // WIP on shooter power LERP - // controller.y() - // .whileTrue(Commands.defer( - // () -> superstructure.shootWithDistanceCommand(drivetrain.getDistanceToHub()), - // java.util.Set.of())); - - controller.y().whileTrue(superstructure.shootCommand()); + controller.y().onTrue(superstructure.shootCommand()); controller.x().whileTrue(superstructure.stopShootingCommand()); controller.a().whileTrue( @@ -52,5 +77,55 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur controller.b().whileTrue( superstructure.backFeedAllCommand() .finallyDo(() -> superstructure.stopFeedingAllCommand().schedule())); + + controller.povUp().onTrue(superstructure.setTurretForward().withName("OperatorControls.setTurretForward")); + controller.povLeft().onTrue(superstructure.setTurretLeft().withName("OperatorControls.setTurretLeft")); + controller.povRight().onTrue(superstructure.setTurretRight().withName("OperatorControls.setTurretRight")); + + controller.leftBumper().toggleOnTrue( + new ShootOnTheMoveCommand(drivetrain, superstructure, () -> superstructure.getAimPoint()) + .ignoringDisable(true) + .withName("OperatorControls.aimCommand")); + } + + public static Command fireAlgae(SwerveSubsystem drivetrain, Superstructure superstructure) { + return Commands.runOnce(() -> { + System.err.println("FIRE!"); + + SimulatedArena arena = SimulatedArena.getInstance(); + + // Translation2d robotPosition, + // Translation2d shooterPositionOnRobot, + // ChassisSpeeds chassisSpeeds, + // Rotation2d shooterFacing, + // Distance initialHeight, + // LinearVelocity launchingSpeed, + // Angle shooterAngle + + GamePieceProjectile fuel = new RebuiltFuelOnFly( + drivetrain.getPose().getTranslation(), + new Translation2d(), + drivetrain.getSwerveDrive().getRobotVelocity().times(-1), + superstructure.getAimRotation3d().toRotation2d(), + Distance.ofBaseUnits(1, Feet), + + // based on numbers from https://www.reca.lc/flywheel + // superstructure.getTangentialVelocity().times(0.5), // adjust for simulation + // tuning + LinearVelocity.ofBaseUnits(5, FeetPerSecond), + superstructure.getHoodAngle()); + + // Configure callbacks to visualize the flight trajectory of the projectile + fuel.withProjectileTrajectoryDisplayCallBack( + // Callback for when the note will eventually hit the target (if configured) + (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot", + pose3ds.toArray(Pose3d[]::new)), + // Callback for when the note will eventually miss the target, or if no target + // is configured + (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot", + pose3ds.toArray(Pose3d[]::new))); + + arena.addGamePieceProjectile(fuel); + }).withName("Fire.Fuel"); } } diff --git a/src/main/java/frc/robot/subsystems/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/HoodSubsystem.java index 0361997..dffed02 100644 --- a/src/main/java/frc/robot/subsystems/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HoodSubsystem.java @@ -1,94 +1,92 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; +import java.util.function.Supplier; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import yams.gearing.GearBox; -import yams.gearing.MechanismGearing; -import yams.mechanisms.config.PivotConfig; -import yams.mechanisms.positional.Pivot; -import yams.motorcontrollers.SmartMotorController; -import yams.motorcontrollers.SmartMotorControllerConfig; -import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; -import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; -import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.SparkWrapper; public class HoodSubsystem extends SubsystemBase { // 1 Neo, 0-90 degree variability, 50:1 reduction - private SparkMax spark = new SparkMax(Constants.HoodConstants.kMotorId, MotorType.kBrushless); - - private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withControlMode(ControlMode.CLOSED_LOOP) - .withClosedLoopController(100, 0, 0, DegreesPerSecond.of(90), DegreesPerSecondPerSecond.of(90)) - .withFeedforward(new ArmFeedforward(0, 0.3, 0.1)) - .withTelemetry("HoodMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(50))) - .withMotorInverted(false) - .withIdleMode(MotorMode.BRAKE) - .withSoftLimit(Degrees.of(0), Degrees.of(90)) - .withStatorCurrentLimit(Amps.of(40)) - .withClosedLoopRampRate(Seconds.of(0.1)) - .withOpenLoopRampRate(Seconds.of(0.1)); - - private SmartMotorController smc = new SparkWrapper(spark, DCMotor.getNEO(1), smcConfig); - - private PivotConfig hoodConfig = new PivotConfig(smc) - .withHardLimit(Degrees.of(-5), Degrees.of(95)) - .withStartingPosition(Degrees.of(0)) - .withMOI(0.001) - .withTelemetry("Hood", TelemetryVerbosity.HIGH); - - private Pivot hood = new Pivot(hoodConfig); + // private SparkMax spark = new SparkMax(Constants.HoodConstants.kMotorId, + // MotorType.kBrushless); + + // private SmartMotorControllerConfig smcConfig = new + // SmartMotorControllerConfig(this) + // .withControlMode(ControlMode.CLOSED_LOOP) + // .withClosedLoopController(100, 0, 0, DegreesPerSecond.of(90), + // DegreesPerSecondPerSecond.of(90)) + // .withFeedforward(new ArmFeedforward(0, 0.3, 0.1)) + // .withTelemetry("HoodMotor", TelemetryVerbosity.HIGH) + // .withGearing(new MechanismGearing(GearBox.fromReductionStages(50))) + // .withMotorInverted(false) + // .withIdleMode(MotorMode.BRAKE) + // .withSoftLimit(Degrees.of(0), Degrees.of(90)) + // .withStatorCurrentLimit(Amps.of(40)) + // .withClosedLoopRampRate(Seconds.of(0.1)) + // .withOpenLoopRampRate(Seconds.of(0.1)); + + // private SmartMotorController smc = new SparkWrapper(spark, DCMotor.getNEO(1), + // smcConfig); + + // private PivotConfig hoodConfig = new PivotConfig(smc) + // .withHardLimit(Degrees.of(-5), Degrees.of(95)) + // .withStartingPosition(Degrees.of(0)) + // .withMOI(0.001) + // .withTelemetry("Hood", TelemetryVerbosity.HIGH); + + // private Pivot hood = new Pivot(hoodConfig); public HoodSubsystem() { } public Command setAngle(Angle angle) { - return hood.setAngle(angle); + // return hood.setAngle(angle); + return Commands.runOnce(() -> { + }); + } + + public Command setAngleDynamic(Supplier hoodAngleSupplier) { + // TODO: Uncomment when hood is enabled + // return hood.setAngle(hoodAngleSupplier); + return Commands.run(() -> { + }); } public Command stow() { - return hood.setAngle(Degrees.of(0)); + return setAngle(Degrees.of(0)); } public Command max() { - return hood.setAngle(Degrees.of(90)); + return setAngle(Degrees.of(90)); } public Angle getAngle() { - return hood.getAngle(); + return Degrees.of(75); + // return hood.getAngle(); } public Command set(double dutyCycle) { - return hood.set(dutyCycle); + return Commands.runOnce(() -> { + }); } public Command sysId() { - return hood.sysId(Volts.of(7), Volts.of(2).per(Second), Seconds.of(10)); + // return hood.sysId(Volts.of(7), Volts.of(2).per(Second), Seconds.of(10)); + return Commands.runOnce(() -> { + }); } @Override public void periodic() { - hood.updateTelemetry(); + // hood.updateTelemetry(); } @Override public void simulationPeriodic() { - hood.simIterate(); + // hood.simIterate(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 6bf64c2..13aeae9 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -127,7 +127,7 @@ public Command deployAndRollCommand() { public Command backFeedAndRollCommand() { return Commands.run(() -> { setIntakeDeployed(); - smc.setDutyCycle(-INTAKE_SPEED); + // smc.setDutyCycle(-INTAKE_SPEED); }, this).finallyDo(() -> { smc.setDutyCycle(0); setIntakeHold(); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 691b6c6..3f50039 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,129 +1,165 @@ package frc.robot.subsystems; -import java.util.Map; -import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; -import com.thethriftybot.ThriftyNova; - -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.SparkWrapper; public class ShooterSubsystem extends SubsystemBase { - - private double SHOOTER_SPEED = 0.65; - private double temp_distance = 0.0; - // 2 Neos, 4in shooter wheels - private final ThriftyNova leaderNova = new ThriftyNova(Constants.ShooterConstants.kLeaderMotorId); - private final ThriftyNova followerNova = new ThriftyNova(Constants.ShooterConstants.kFollowerMotorId); - - // private final SmartMotorControllerConfig smcConfig = new - // SmartMotorControllerConfig(this) - // .withFollowers(Pair.of(followerNova, false)) - // .withControlMode(ControlMode.CLOSED_LOOP) - // .withClosedLoopController(0.1, 0, 0) - // .withFeedforward(new SimpleMotorFeedforward(0, 0.5, 0)) - // .withTelemetry("ShooterMotor", TelemetryVerbosity.HIGH) - // .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) - // .withMotorInverted(false) - // .withIdleMode(MotorMode.COAST) - // .withStatorCurrentLimit(Amps.of(40)); - - // private final SmartMotorController smc = new NovaWrapper(leaderNova, - // DCMotor.getNEO(2), smcConfig); - - // private final FlyWheelConfig shooterConfig = new FlyWheelConfig(smc) - // .withDiameter(Inches.of(4)) - // .withMass(Pounds.of(1)) - // .withUpperSoftLimit(RotationsPerSecond.of(6000)) - // .withLowerSoftLimit(RotationsPerSecond.of(0)) - // .withTelemetry("Shooter", TelemetryVerbosity.HIGH); - - // private final FlyWheel shooter = new FlyWheel(shooterConfig); + // private final ThriftyNova leaderNova = new ThriftyNova( + // Constants.ShooterConstants.kLeaderMotorId, + // ThriftyNova.MotorType.NEO); + + // private final ThriftyNova followerNova = new ThriftyNova( + // Constants.ShooterConstants.kFollowerMotorId, + // ThriftyNova.MotorType.NEO); + + private final SparkMax leaderSpark = new SparkMax(Constants.ShooterConstants.kLeaderMotorId, + MotorType.kBrushless); + + private final SparkMax followerSpark = new SparkMax(Constants.ShooterConstants.kFollowerMotorId, + MotorType.kBrushless); + + private final SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withFollowers(Pair.of(followerSpark, true)) + .withControlMode(ControlMode.CLOSED_LOOP) + .withClosedLoopController(0.00936, 0, 0) + .withFeedforward(new SimpleMotorFeedforward(0.191, 0.11858, 0.0)) + .withTelemetry("ShooterMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) + .withMotorInverted(false) + .withIdleMode(MotorMode.COAST) + .withStatorCurrentLimit(Amps.of(40)); + + private final SmartMotorController smc = new SparkWrapper(leaderSpark, DCMotor.getNEO(2), smcConfig); + + private final FlyWheelConfig shooterConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(1)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(0)) + .withTelemetry("Shooter", TelemetryVerbosity.HIGH); + + private final FlyWheel shooter = new FlyWheel(shooterConfig); public ShooterSubsystem() { - SmartDashboard.putNumber("ShooterSpeed", SHOOTER_SPEED); - - leaderNova.factoryReset(); - followerNova.factoryReset(); + // leaderNova.factoryReset(); + // followerNova.factoryReset(); // leaderNova.setVoltageCompensation(12); // followerNova.setVoltageCompensation(12); - leaderNova.setInverted(false); - followerNova.setInverted(true); + // leaderNova.setInverted(false); + // followerNova.setInverted(true); + + // followerNova + // .setInversion(true) + // .follow(leaderNova.getID()); } - // public Command setSpeed(AngularVelocity speed) { - // return shooter.setSpeed(speed); - // } + public Command setSpeed(AngularVelocity speed) { + return shooter.setSpeed(speed); + } + + public Command setSpeedDynamic(Supplier speedSupplier) { + return shooter.setSpeed(speedSupplier); + } public Command spinUp() { - return run(() -> { - leaderNova.setPercent(SHOOTER_SPEED); - followerNova.setPercent(SHOOTER_SPEED); + return setSpeed(RPM.of(5500)); - // followerNova.follow(leaderNova.getID()); - // followerNova.setPercent(0.5); - }); + // return setSpeed(RotationsPerSecond.of(50)); - // return shooter.set(0.5); - // return shooter.setSpeed(RotationsPerSecond.of(500)); - } + // return run(() -> { + // // followerNova.follow(leaderNova.getID()); + // // followerNova.setInverted(true); + + // // leaderNova.setPercent(SHOOTER_SPEED); + // // followerNova.setPercent(SHOOTER_SPEED); - public Command shootAtDistance(double distanceMeters) { - return run(() -> { - temp_distance = SHOOTING_SPEED_BY_DISTANCE.get(distanceMeters); + // // followerNova.setPercent(0.5); + // }); - leaderNova.setPercent(temp_distance); - followerNova.setPercent(temp_distance); - }); + // return shooter.set(0.5); + // return shooter.setSpeed(RotationsPerSecond.of(500)); } public Command stop() { - return run(() -> { - leaderNova.setPercent(0); - followerNova.setPercent(0); - // followerNova.setPercent(0.5); - }); + return setSpeed(RPM.of(0)); + // return run(() -> { + + // // leaderNova.setPercent(0); + // // followerNova.setPercent(0); + // // followerNova.setPercent(0.5); + // }); // return shooter.set(0); } - // public AngularVelocity getSpeed() { - // return shooter.getSpeed(); - // } + public AngularVelocity getSpeed() { + return shooter.getSpeed(); + } // public Command set(double dutyCycle) { // return shooter.set(dutyCycle); // } - // public Command sysId() { - // return shooter.sysId(Volts.of(10), Volts.of(2).per(Second), Seconds.of(10)); - // } + public Command sysId() { + return shooter.sysId(Volts.of(12), Volts.of(3).per(Second), Seconds.of(7)); + } @Override public void periodic() { - // shooter.updateTelemetry(); - SHOOTER_SPEED = SmartDashboard.getNumber("ShooterSpeed", SHOOTER_SPEED); - - Logger.recordOutput("Shooter/Setpoint", temp_distance); - Logger.recordOutput("Shooter/LeaderVelocity", leaderNova.getVelocity()); - Logger.recordOutput("Shooter/FollowerVelocity", followerNova.getVelocity()); + Logger.recordOutput("Shooter/LeaderVelocity", leaderSpark.getEncoder().getVelocity()); + Logger.recordOutput("Shooter/FollowerVelocity", followerSpark.getEncoder().getVelocity()); } @Override public void simulationPeriodic() { - // shooter.simIterate(); + shooter.simIterate(); + } + + private Distance wheelRadius() { + return Inches.of(4).div(2); } - // meters, RPS - private static final InterpolatingDoubleTreeMap SHOOTING_SPEED_BY_DISTANCE = InterpolatingDoubleTreeMap.ofEntries( - Map.entry(2.63, 0.55), - Map.entry(3.4, 0.6), - Map.entry(4.83, 0.69)); + public LinearVelocity getTangentialVelocity() { + // Calculate tangential velocity at the edge of the wheel and convert to + // LinearVelocity + + return MetersPerSecond.of(getSpeed().in(RadiansPerSecond) + * wheelRadius().in(Meters)); + } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9258e1d..83c5b9d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,12 +1,16 @@ package frc.robot.subsystems; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meter; import static edu.wpi.first.units.Units.RPM; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,21 +22,16 @@ */ public class Superstructure extends SubsystemBase { - private final ShooterSubsystem shooter; - private final TurretSubsystem turret; - private final HoodSubsystem hood; - private final IntakeSubsystem intake; - private final HopperSubsystem hopper; - private final KickerSubsystem kicker; - - // Default values for "ready" state - private static final AngularVelocity DEFAULT_SHOOTER_SPEED = RPM.of(4000); - private static final Angle DEFAULT_TURRET_ANGLE = Degrees.of(0); - private static final Angle DEFAULT_HOOD_ANGLE = Degrees.of(45); + public final ShooterSubsystem shooter; + public final TurretSubsystem turret; + public final HoodSubsystem hood; + public final IntakeSubsystem intake; + public final HopperSubsystem hopper; + public final KickerSubsystem kicker; // Tolerance for "at setpoint" checks private static final AngularVelocity SHOOTER_TOLERANCE = RPM.of(100); - private static final Angle TURRET_TOLERANCE = Degrees.of(2); + private static final Angle TURRET_TOLERANCE = Degrees.of(1); private static final Angle HOOD_TOLERANCE = Degrees.of(2); // Triggers for readiness checks @@ -45,6 +44,9 @@ public class Superstructure extends SubsystemBase { private Angle targetTurretAngle = Degrees.of(0); private Angle targetHoodAngle = Degrees.of(0); + // Hard coded red hub aim point + private Translation3d aimPoint = new Translation3d(Meter.of(11.902), Meter.of(4.031), Meter.of(0)); + public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, HopperSubsystem hopper, KickerSubsystem kicker) { this.shooter = shooter; @@ -55,20 +57,17 @@ public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubs this.kicker = kicker; // Create triggers for checking if mechanisms are at their targets - this.isShooterAtSpeed = new Trigger(() -> false); - // this.isShooterAtSpeed = new Trigger( - // () -> Math.abs(shooter.getSpeed().in(RPM) - targetShooterSpeed.in(RPM)) < - // SHOOTER_TOLERANCE.in(RPM)); + this.isShooterAtSpeed = new Trigger( + () -> Math.abs(shooter.getSpeed().in(RPM) - targetShooterSpeed.in(RPM)) < SHOOTER_TOLERANCE.in(RPM)); this.isTurretOnTarget = new Trigger( - () -> Math.abs(turret.getAngle().in(Degrees) - targetTurretAngle.in(Degrees)) < TURRET_TOLERANCE.in(Degrees)); + () -> Math.abs(turret.getRawAngle().in(Degrees) - targetTurretAngle.in(Degrees)) < TURRET_TOLERANCE + .in(Degrees)); this.isHoodOnTarget = new Trigger( () -> Math.abs(hood.getAngle().in(Degrees) - targetHoodAngle.in(Degrees)) < HOOD_TOLERANCE.in(Degrees)); - this.isReadyToShoot = new Trigger(() -> false); - // this.isReadyToShoot = - // isShooterAtSpeed.and(isTurretOnTarget).and(isHoodOnTarget); + this.isReadyToShoot = isShooterAtSpeed.and(isTurretOnTarget).and(isHoodOnTarget); } /** @@ -81,41 +80,6 @@ public Command stopAllCommand() { hood.set(0).asProxy()).withName("Superstructure.stopAll"); } - /** - * Moves all mechanisms to a default "ready" state: - * - Shooter spun up to default speed - * - Turret centered - * - Hood at 45 degrees - */ - public Command readyCommand() { - return Commands.runOnce(() -> { - targetShooterSpeed = DEFAULT_SHOOTER_SPEED; - targetTurretAngle = DEFAULT_TURRET_ANGLE; - targetHoodAngle = DEFAULT_HOOD_ANGLE; - }).andThen( - Commands.parallel( - // shooter.setSpeed(DEFAULT_SHOOTER_SPEED).asProxy(), - turret.center().asProxy(), - hood.setAngle(DEFAULT_HOOD_ANGLE).asProxy())) - .withName("Superstructure.ready"); - } - - /** - * Stows the superstructure - stops shooter, centers turret, stows hood. - */ - public Command stowCommand() { - return Commands.runOnce(() -> { - targetShooterSpeed = RPM.of(0); - targetTurretAngle = Degrees.of(0); - targetHoodAngle = Degrees.of(0); - }).andThen( - Commands.parallel( - // shooter.stop().asProxy(), - turret.center().asProxy(), - hood.stow().asProxy())) - .withName("Superstructure.stow"); - } - /** * Aims the superstructure to specific targets - used for auto-targeting. * @@ -136,6 +100,12 @@ public Command aimCommand(AngularVelocity shooterSpeed, Angle turretAngle, Angle .withName("Superstructure.aim"); } + public void setShooterSetpoints(AngularVelocity shooterSpeed, Angle turretAngle, Angle hoodAngle) { + targetShooterSpeed = shooterSpeed; + targetTurretAngle = turretAngle; + targetHoodAngle = hoodAngle; + } + /** * Aims the superstructure using suppliers - useful for dynamic targeting. * @@ -147,15 +117,10 @@ public Command aimDynamicCommand( Supplier shooterSpeedSupplier, Supplier turretAngleSupplier, Supplier hoodAngleSupplier) { - return Commands.run(() -> { - targetShooterSpeed = shooterSpeedSupplier.get(); - targetTurretAngle = turretAngleSupplier.get(); - targetHoodAngle = hoodAngleSupplier.get(); - }).alongWith( - Commands.parallel( - // shooter.setSpeed(shooterSpeedSupplier.get()).asProxy(), - turret.setAngle(turretAngleSupplier.get()).asProxy(), - hood.setAngle(hoodAngleSupplier.get()).asProxy())) + return Commands.parallel( + shooter.setSpeedDynamic(shooterSpeedSupplier).asProxy(), + turret.setAngleDynamic(turretAngleSupplier).asProxy(), + hood.setAngleDynamic(hoodAngleSupplier).asProxy()) .withName("Superstructure.aimDynamic"); } @@ -170,18 +135,30 @@ public Command waitUntilReadyCommand() { * Aims and waits until ready - combines aim and wait. */ public Command aimAndWaitCommand(AngularVelocity shooterSpeed, Angle turretAngle, Angle hoodAngle) { - return aimCommand(shooterSpeed, turretAngle, hoodAngle) + return aimDynamicCommand(() -> shooterSpeed, () -> turretAngle, () -> hoodAngle) .andThen(waitUntilReadyCommand()) .withName("Superstructure.aimAndWait"); } + public Command setTurretForward() { + return turret.setAngle(Degrees.of(0)).withName("Superstructure.setTurretForward"); + } + + public Command setTurretLeft() { + return turret.setAngle(Degrees.of(45)).withName("Superstructure.setTurretLeft"); + } + + public Command setTurretRight() { + return turret.setAngle(Degrees.of(-45)).withName("Superstructure.setTurretRight"); + } + // Getters for current state - // public AngularVelocity getShooterSpeed() { - // return shooter.getSpeed(); - // } + public AngularVelocity getShooterSpeed() { + return shooter.getSpeed(); + } public Angle getTurretAngle() { - return turret.getAngle(); + return turret.getRawAngle(); } public Angle getHoodAngle() { @@ -200,6 +177,23 @@ public Angle getTargetHoodAngle() { return targetHoodAngle; } + public Translation3d getAimPoint() { + return aimPoint; + } + + public void setAimPoint(Translation3d newAimPoint) { + this.aimPoint = newAimPoint; + } + + public Rotation3d getAimRotation3d() { + // See + // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html + return new Rotation3d( + Degrees.of(0), // no roll 🤞 + hood.getAngle().unaryMinus(), // pitch is negative hood angle + turret.getRobotAdjustedAngle()); + } + /** * Command to run the intake while held. */ @@ -288,16 +282,10 @@ public Command setIntakeDeployAndRoll() { * Command to shoot - spins up shooter. */ public Command shootCommand() { + // return shooter.sysId().withName("Superstructure.shoot"); return shooter.spinUp().withName("Superstructure.shoot"); } - /** - * Command to shoot - spins up shooter. - */ - public Command shootWithDistanceCommand(double distanceMeters) { - return shooter.shootAtDistance(distanceMeters).withName("Superstructure.shootWithDistance"); - } - /** * Command to stop shooting - stops shooter. */ @@ -313,5 +301,38 @@ public Command rezeroIntakePivotCommand() { @Override public void periodic() { // Superstructure doesn't need periodic updates - subsystems handle their own + + String shooterOut = "S:" + isShooterAtSpeed.getAsBoolean() + "(" + Math.round(shooter.getSpeed().in(RPM)) + "/" + + Math.round(targetShooterSpeed.in(RPM)) + ")"; + + String turretOut = "T:" + isTurretOnTarget.getAsBoolean() + "(" + Math.round(turret.getRawAngle().in(Degrees)) + "/" + + Math.round(targetTurretAngle.in(Degrees)) + ")"; + + String hoodOut = "H:" + isHoodOnTarget.getAsBoolean() + "(" + Math.round(hood.getAngle().in(Degrees)) + "/" + + Math.round(targetHoodAngle.in(Degrees)) + ")"; + + String readyOut = "R:" + isReadyToShoot.getAsBoolean(); + + // System.out.println(shooterOut + " " + turretOut + " " + hoodOut + " " + + // readyOut); + } + + public Command useRequirement() { + return runOnce(() -> { + }); + } + + public Pose3d getShooterPose() { + // Position of the shooter relative to the "front" of the robot. Rotation + // element is based on hood and turret angles + return new Pose3d(new Translation3d( + Meter.of(-0.3), + Meter.of(0), + Meter.of(0.6)), + getAimRotation3d()); + } + + public LinearVelocity getTangentialVelocity() { + return shooter.getTangentialVelocity(); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index b5622a1..dc3bf8e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -37,11 +37,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meter; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -96,12 +96,12 @@ public double getDistanceToHub() { */ public SwerveSubsystem(File directory) { boolean blueAlliance = false; - Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1), + Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(2.75), Meter.of(4)), - Rotation2d.fromDegrees(0)) - : new Pose2d(new Translation2d(Meter.of(16), + Rotation2d.fromDegrees(180)) + : new Pose2d(new Translation2d(Meter.of(14.25), Meter.of(4)), - Rotation2d.fromDegrees(180)); + Rotation2d.fromDegrees(0)); // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary // objects being created. @@ -620,6 +620,16 @@ public Pose2d getPose() { return swerveDrive.getPose(); } + /** + * Gets the current 3d pose (position and rotation) of the robot, as reported by + * odometry. Transforms into a 3d pose assuming on the XY plane. + * + * @return + */ + public Pose3d getPose3d() { + return new Pose3d(swerveDrive.getPose()); + } + /** * Set chassis speeds with closed-loop velocity control. * diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index c393aec..afc355a 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,13 +1,22 @@ package frc.robot.subsystems; -import com.thethriftybot.ThriftyNova; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; @@ -17,6 +26,8 @@ import frc.robot.Constants; import yams.gearing.GearBox; import yams.gearing.MechanismGearing; +import yams.mechanisms.config.MechanismPositionConfig; +import yams.mechanisms.config.MechanismPositionConfig.Plane; import yams.mechanisms.config.PivotConfig; import yams.mechanisms.positional.Pivot; import yams.motorcontrollers.SmartMotorController; @@ -24,37 +35,44 @@ import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.NovaWrapper; +import yams.motorcontrollers.local.SparkWrapper; public class TurretSubsystem extends SubsystemBase { - private final double MAX_ONE_DIR_FOV = 45; // degrees + private final double MAX_ONE_DIR_FOV = 90; // degrees + public final Translation3d turretTranslation = new Translation3d(-0.205, 0.0, 0.375); // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous // 360 deg // Total reduction: 4 * 10 = 40:1 - private ThriftyNova nova = new ThriftyNova(Constants.TurretConstants.kMotorId); + + // private ThriftyNova nova = new + // ThriftyNova(Constants.TurretConstants.kMotorId); + + private SparkMax spark = new SparkMax(Constants.TurretConstants.kMotorId, MotorType.kBrushless); private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) .withControlMode(ControlMode.CLOSED_LOOP) - .withClosedLoopController(100, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(180)) - .withFeedforward(new ArmFeedforward(0, 0, 0.1)) + .withClosedLoopController(60.0, 0, 0, DegreesPerSecond.of(10800), DegreesPerSecondPerSecond.of(10800)) + .withFeedforward(new ArmFeedforward(0, 0, 0)) .withTelemetry("TurretMotor", TelemetryVerbosity.HIGH) .withGearing(new MechanismGearing(GearBox.fromReductionStages(4, 10))) - .withMotorInverted(false) - .withIdleMode(MotorMode.BRAKE) + .withMotorInverted(true) + .withIdleMode(MotorMode.COAST) .withSoftLimit(Degrees.of(-MAX_ONE_DIR_FOV), Degrees.of(MAX_ONE_DIR_FOV)) - .withStatorCurrentLimit(Amps.of(40)) + .withStatorCurrentLimit(Amps.of(10)) .withClosedLoopRampRate(Seconds.of(0.1)) .withOpenLoopRampRate(Seconds.of(0.1)); - private SmartMotorController smc = new NovaWrapper(nova, DCMotor.getNEO(1), smcConfig); + private SmartMotorController smc = new SparkWrapper(spark, DCMotor.getNEO(1), smcConfig); private final PivotConfig turretConfig = new PivotConfig(smc) .withHardLimit(Degrees.of(-MAX_ONE_DIR_FOV - 5), Degrees.of(MAX_ONE_DIR_FOV + 5)) .withStartingPosition(Degrees.of(0)) .withMOI(0.05) - .withTelemetry("Turret", TelemetryVerbosity.HIGH); + .withTelemetry("Turret", TelemetryVerbosity.HIGH) + .withMechanismPositionConfig( + new MechanismPositionConfig().withMovementPlane(Plane.XY).withRelativePosition(turretTranslation)); private Pivot turret = new Pivot(turretConfig); @@ -65,11 +83,21 @@ public Command setAngle(Angle angle) { return turret.setAngle(angle); } + public Command setAngleDynamic(Supplier turretAngleSupplier) { + return turret.setAngle(turretAngleSupplier); + } + public Command center() { return turret.setAngle(Degrees.of(0)); } - public Angle getAngle() { + public Angle getRobotAdjustedAngle() { + // Returns the turret angle in the robot's coordinate frame + // since the turret is mounted backwards, we need to add 180 degrees + return turret.getAngle().plus(Degrees.of(180)); + } + + public Angle getRawAngle() { return turret.getAngle(); } @@ -84,6 +112,12 @@ public Command sysId() { @Override public void periodic() { turret.updateTelemetry(); + + Logger.recordOutput("ASCalibration/FinalComponentPoses", new Pose3d[] { + new Pose3d( + turretTranslation, + new Rotation3d(0, 0, turret.getAngle().in(Radians))) + }); } @Override diff --git a/src/main/java/frc/robot/util/CommandsLogging.java b/src/main/java/frc/robot/util/CommandsLogging.java index 71319d7..934ee3e 100644 --- a/src/main/java/frc/robot/util/CommandsLogging.java +++ b/src/main/java/frc/robot/util/CommandsLogging.java @@ -367,6 +367,10 @@ private static void addCommand( var subCommands = (List) sequenceSubCommandsField.get(sequence); var index = (Integer) sequenceCommandIndexField.get(sequence); + // Handle case where index is -1 (before sequence starts) or out of bounds + if (index < 0 || index >= subCommands.size()) { + return; + } var subCommand = subCommands.get(index); addCommand(subCommand, parentCommandName + ": " + getCommandName(subCommand), commandsList); } catch (IllegalAccessException e) { diff --git a/src/main/java/frc/robot/util/maplesim/Arena2026Rebuilt.java b/src/main/java/frc/robot/util/maplesim/Arena2026Rebuilt.java new file mode 100644 index 0000000..373d256 --- /dev/null +++ b/src/main/java/frc/robot/util/maplesim/Arena2026Rebuilt.java @@ -0,0 +1,469 @@ +package frc.robot.util.maplesim; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.List; +import org.dyn4j.dynamics.Settings; +import org.ironmaple.simulation.SimulatedArena; + +public class Arena2026Rebuilt extends SimulatedArena { + + protected boolean shouldClock = true; + + protected double clock = 0; + protected boolean blueIsOnClock = Math.random() < 0.5; + + protected DoublePublisher phaseClockPublisher = genericInfoTable.getDoubleTopic("Time left in current phase") + .publish(); + + protected BooleanPublisher redActivePublisher = redTable.getBooleanTopic("Red is active").publish(); + protected BooleanPublisher blueActivePublisher = blueTable.getBooleanTopic("Blue is active").publish(); + + protected RebuiltHub blueHub; + protected RebuiltHub redHub; + + protected RebuiltOutpost blueOutpost; + protected RebuiltOutpost redOutpost; + + protected boolean isInEfficiencyMode = true; + + protected static Translation2d centerPieceBottomRightCorner = new Translation2d(7.35737, 1.724406); + protected static Translation2d redDepotBottomRightCorner = new Translation2d(0.02, 5.53); + protected static Translation2d blueDepotBottomRightCorner = new Translation2d(16.0274, 1.646936); + + /** the obstacles on the 2026 competition field */ + public static final class RebuiltFieldObstaclesMap extends FieldMap { + private static final double FIELD_WIDTH = 16.54; + private static final double FIELD_HEIGHT = 8.052; + + public RebuiltFieldObstaclesMap(boolean AddRampCollider) { + super.addBorderLine(new Translation2d(0, 0), new Translation2d(0, 8.052)); + + // red wall + super.addBorderLine(new Translation2d(16.540988, 0), new Translation2d(16.540988, 8.052)); + + // upper walls + super.addBorderLine(new Translation2d(16.540988, 8.052), new Translation2d(0, 8.052)); + + // lower walls + super.addBorderLine(new Translation2d(0, 0), new Translation2d(16.540988, 0)); + + // Trench Walls (47 inch height, 12 inch width) + double trenchWallDistX = Inches.of(120.0).in(Meters) + Inches.of(47.0 / 2).in(Meters); + + double trenchWallDistY = Inches.of(73.0).in(Meters) + + Inches.of(47.0 / 2).in(Meters) + + Inches.of(6).in(Meters); + + super.addRectangularObstacle( + Inches.of(53).in(Meters), + Inches.of(12).in(Meters), + new Pose2d(8.27 - trenchWallDistX, 4.035 - trenchWallDistY, Rotation2d.kZero)); + super.addRectangularObstacle( + Inches.of(53).in(Meters), + Inches.of(12).in(Meters), + new Pose2d(8.27 + trenchWallDistX, 4.035 - trenchWallDistY, Rotation2d.kZero)); + super.addRectangularObstacle( + Inches.of(53).in(Meters), + Inches.of(12).in(Meters), + new Pose2d(8.27 - trenchWallDistX, 4.035 + trenchWallDistY, Rotation2d.kZero)); + super.addRectangularObstacle( + Inches.of(53).in(Meters), + Inches.of(12).in(Meters), + new Pose2d(8.27 - trenchWallDistX, 4.035 - trenchWallDistY, Rotation2d.kZero)); + + // poles of the tower + super.addRectangularObstacle( + Inches.of(2).in(Meters), + Inches.of(47).in(Meters), + new Pose2d(new Translation2d(Inches.of(42), Inches.of(159)), new Rotation2d())); + + super.addRectangularObstacle( + Inches.of(2).in(Meters), + Inches.of(47).in(Meters), + new Pose2d(new Translation2d(Inches.of(651 - 42), Inches.of(170)), new Rotation2d())); + + // Colliders to describe the hub plus ramps + if (AddRampCollider) { + super.addRectangularObstacle( + Inches.of(47).in(Meters), + Inches.of(217).in(Meters), + new Pose2d(RebuiltHub.blueHubPose.toTranslation2d(), new Rotation2d())); + + super.addRectangularObstacle( + Inches.of(47).in(Meters), + Inches.of(217).in(Meters), + new Pose2d(RebuiltHub.redHubPose.toTranslation2d(), new Rotation2d())); + } + + // Colliders to describe just the hub + else { + super.addRectangularObstacle( + Inches.of(47).in(Meters), + Inches.of(47).in(Meters), + new Pose2d(RebuiltHub.blueHubPose.toTranslation2d(), new Rotation2d())); + + super.addRectangularObstacle( + Inches.of(47).in(Meters), + Inches.of(47).in(Meters), + new Pose2d(RebuiltHub.redHubPose.toTranslation2d(), new Rotation2d())); + } + } + } + + /** + * + * + *

Creates an Arena for the 2026 FRC game rebuilt

+ * + *

+ * This will create an Arena with the ramp areas marked as inaccessible. If you + * would like to change that use + * {@link #Arena2026Rebuilt(boolean)}. Additionally due to performance issues + * the arena will not spawn all fuel by + * default. If you would like to change this use + * {@link #setEfficiencyMode(boolean)} + */ + public Arena2026Rebuilt() { + this(true); + } + + /** + * + * + *

Creates an Arena for the 2026 FRC game rebuilt

+ * + *

+ * Due to the nature of maple sim they can not be fully simulated and so either + * must be non existent or treated + * as full colliders. This behavior can be changed with the AddRampCollider + * variable. Additionally due to + * performance issues the arena will not spawn all fuel by default. If you would + * like to change this use + * {@link #setEfficiencyMode(boolean)} + * + * @param AddRampCollider Whether or not the ramps should be added as colliders. + */ + public Arena2026Rebuilt(boolean AddRampCollider) { + super(new RebuiltFieldObstaclesMap(AddRampCollider)); + + Settings settings = physicsWorld.getSettings(); + + // settings.setVelocityConstraintSolverIterations(3); + // settings.setPositionConstraintSolverIterations(2); + settings.setMinimumAtRestTime(0.02); + + physicsWorld.setSettings(settings); + + blueHub = new RebuiltHub(this, true); + super.addCustomSimulation(blueHub); + + redHub = new RebuiltHub(this, false); + super.addCustomSimulation(redHub); + + blueOutpost = new RebuiltOutpost(this, true); + super.addCustomSimulation(blueOutpost); + + redOutpost = new RebuiltOutpost(this, false); + super.addCustomSimulation(redOutpost); + } + + public static Pose3d flip(Pose3d toFlip) { + return new Pose3d( + new Translation3d(RebuiltFieldObstaclesMap.FIELD_WIDTH - toFlip.getX(), + RebuiltFieldObstaclesMap.FIELD_HEIGHT - toFlip.getY(), + toFlip.getZ()), + toFlip.getRotation()); + } + + /** + * + * + *

Generates a random number within a range centered on 0 with a variance + * set by the parameter.

+ * + * @param variance the length of range used to generate the random number. + * @return A random number in range. + */ + public static double randomInRange(double variance) { + return (Math.random() - 0.5) * variance; + } + + /** + * + * + *

Adds a game piece too the arena with a certain random variance.

+ * + * This method is useful for certain spawners like the return cutes on the hub + * to prevent the game pieces from being + * returned to the exact same position every time. + * + * @param info the info of the game piece + * @param robotPosition the position of the robot (not the shooter) + * at the time of launching the game piece + * @param shooterPositionOnRobot the translation from the shooter's position + * to the robot's center, in the robot's + * frame of reference + * @param chassisSpeedsFieldRelative the field-relative velocity of the robot + * chassis when launching the game piece, + * influencing the initial velocity of the + * game piece + * @param shooterFacing the direction in which the shooter is + * facing at launch + * @param initialHeight the initial height of the game piece when + * launched, i.e., the height of the shooter + * from the + * ground + * @param launchingSpeed the speed at which the game piece is launch + * @param shooterAngle the pitch angle of the shooter when + * launching + * @param xVariance The max amount of variance that should be + * added too the x coordinate of the game + * piece. + * @param yVariance The max amount of variance that should be + * added too the y coordinate of the game + * piece. + * @param yawVariance The max amount of variance that should be + * added too the yaw of the game piece. + * @param speedVariance The max amount of variance that should be + * added too the speed of the game piece. + * @param pitchVariance The max amount of variance that should be + * added too the pitch of the game piece. + */ + public void addPieceWithVariance( + Translation2d piecePose, + Rotation2d yaw, + Distance height, + LinearVelocity speed, + Angle pitch, + double xVariance, + double yVariance, + double yawVariance, + double speedVariance, + double pitchVariance) { + addGamePieceProjectile(new RebuiltFuelOnFly( + piecePose.plus(new Translation2d(randomInRange(xVariance), randomInRange(yVariance))), + new Translation2d(), + new ChassisSpeeds(), + yaw.plus(Rotation2d.fromDegrees(randomInRange(yawVariance))), + height, + speed.plus(MetersPerSecond.of(randomInRange(speedVariance))), + Degrees.of(pitch.in(Degrees) + randomInRange(pitchVariance)))); + } + + @Override + public void placeGamePiecesOnField() { + blueOutpost.reset(); + redOutpost.reset(); + + for (int x = 0; x < 12; x += 1) { + for (int y = 0; y < 30; y += isInEfficiencyMode ? 3 : 1) { + addGamePiece(new RebuiltFuelOnField(centerPieceBottomRightCorner.plus( + new Translation2d(Inches.of(5.991 * x), Inches.of(5.95 * y))))); + } + } + + boolean isOnBlue = !DriverStation.getAlliance().isEmpty() + && DriverStation.getAlliance().get() == Alliance.Blue; + + if (isOnBlue || !isInEfficiencyMode) { + for (int x = 0; x < 4; x++) { + for (int y = 0; y < 6; y++) { + addGamePiece(new RebuiltFuelOnField(blueDepotBottomRightCorner.plus( + new Translation2d(Inches.of(5.991 * x), Inches.of(5.95 * y))))); + } + } + } + + if (!isOnBlue || !isInEfficiencyMode) { + for (int x = 0; x < 4; x++) { + for (int y = 0; y < 6; y++) { + addGamePiece(new RebuiltFuelOnField(redDepotBottomRightCorner.plus( + new Translation2d(Inches.of(5.991 * x), Inches.of(5.95 * y))))); + } + } + } + + setupValueForMatchBreakdown("CurrentFuelInOutpost"); + setupValueForMatchBreakdown("TotalFuelInOutpost"); + setupValueForMatchBreakdown("TotalFuelInHub"); + setupValueForMatchBreakdown("WastedFuel"); + } + + @Override + public synchronized List getGamePiecesPosesByType(String type) { + List poses = super.getGamePiecesPosesByType(type); + + blueOutpost.draw(poses); + redOutpost.draw(poses); + + return poses; + } + + @Override + public void simulationSubTick(int tickNum) { + if (shouldClock && !DriverStation.isAutonomous() && DriverStation.isEnabled()) { + clock -= getSimulationDt().in(Units.Seconds); + + if (clock <= 0) { + clock = 25; + blueIsOnClock = !blueIsOnClock; + } + } else { + clock = 25; + } + + phaseClockPublisher.set((clock)); + + super.simulationSubTick(tickNum); + + blueActivePublisher.set(isActive(true)); + redActivePublisher.set(isActive(false)); + } + + /** + * + * + *

Returns wether the specified team currently has an active HUB

+ * + * This function returns true during autonomous or when shouldClock (set by + * {@link #setShouldRunClock(boolean)}) is + * false. + * + * @param isBlue Wether to check the blue or red alliance. + * @return Wether the specified alliance's HUB is currently active + */ + public boolean isActive(boolean isBlue) { + if (isBlue) { + return blueIsOnClock || DriverStation.isAutonomous() || !shouldClock; + } else { + return !blueIsOnClock || DriverStation.isAutonomous() || !shouldClock; + } + } + + /** + * + * + *

Used to determine wether the arena should time which goal is active.

+ * + * When this is set too false both goals will always be set to active. Ths can + * be useful for testing or too simulate + * endgame. + * + * @param shouldRunClock + */ + public void setShouldRunClock(boolean shouldRunClock) { + shouldClock = shouldRunClock; + } + + /** + * + * + *

Dumps game pieces from the specified outpost.

+ * + * This function will dump up to 24 game pieces, dependent on how many game + * pieces are currently stored in the + * outpost. For more manual control of the game pieces in the outpost use + * {@link #outpostThrow(boolean, Rotation2d, + * Angle, LinearVelocity)}. To have a human player attempt to throw a game piece + * into the hub use + * {@link #outpostThrowForGoal(boolean)}. + * + * @param isBlue wether to dump the blue or red outpost + */ + public void outpostDump(boolean isBlue) { + (isBlue ? blueOutpost : redOutpost).dump(); + } + + /** + * + * + *

Attempts too throw a game piece at the specified goal.

+ * + *

+ * This method comes with variance built in (to simulate human inconsistency) + * and will therefore only hit about + * half the time. Additionally if the hub does not have game pieces stored this + * method will not do anything. If you + * would like to manually control how the human player throws game pieces use + * {@link #outpostThrow(boolean, + * Rotation2d, Angle, LinearVelocity)} + * + * @param isBlue whether too throw for the blue or red HUB. + */ + public void outpostThrowForGoal(boolean isBlue) { + (isBlue ? blueOutpost : redOutpost).throwForGoal(); + } + + /** + * + * + *

Throws a game piece from the outpost at the specified angle and + * speed.

+ * + *

+ * This method comes with variance built in (to simulate human inconsistency). + * Additionally if the hub does not + * have game pieces stored this method will not do anything. If you would like + * to have the human player throw at the + * hub use {@link #outpostThrowForGoal(boolean)} + * + * @param isBlue Wether too throw from the blue or red OUTPOST. + * @param throwYaw The yaw at which too throw the ball. + * @param throwPitch The pitch at which too throw the ball. + * @param speed The speed at which too throw the ball. + */ + public void outpostThrow(boolean isBlue, Rotation2d throwYaw, Angle throwPitch, LinearVelocity speed) { + (isBlue ? blueOutpost : redOutpost).throwFuel(throwYaw, throwPitch, speed); + } + + /** + * + * + *

Determines wether the arena is in efficiency mode

+ * + *

For changes too take effect call {@link #resetFieldForAuto()}.

+ * + *

+ * Efficiency mode reduces the amount of game pieces on the field too increase + * performance. MapleSim was not + * designed with 400 game pieces in mind and so can struggle with the large + * number of game pieces present in + * rebuilt. + * + * @param efficiencyMode Wether efficiency mode should be on or off. + */ + public void setEfficiencyMode(boolean efficiencyMode) { + isInEfficiencyMode = efficiencyMode; + } + + /** + * + * + *

Returns wether or not the arena is in Efficiency mode

+ * + * For more information see {@link #setEfficiencyMode(boolean)} + * + * @return Wether or not the Arena is in efficiency mode. + */ + public boolean getEfficiencyMode() { + return isInEfficiencyMode; + } +} diff --git a/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnField.java b/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnField.java new file mode 100644 index 0000000..3189c4f --- /dev/null +++ b/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnField.java @@ -0,0 +1,26 @@ +package frc.robot.util.maplesim; + +import org.dyn4j.geometry.Circle; +import org.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import static edu.wpi.first.units.Units.Centimeter; +import static edu.wpi.first.units.Units.Centimeters; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Pounds; + +/** + * + * + *

Represents an fuel in the 2026 Rebuilt game.

+ */ +public class RebuiltFuelOnField extends GamePieceOnFieldSimulation { + public static final GamePieceInfo REBUILT_FUEL_INFO = new GamePieceInfo( + "Fuel", new Circle(Centimeters.of(7.5).in(Meters)), Centimeter.of(15), Pounds.of(0.5), 1.8, 5, 0.8); + + public RebuiltFuelOnField(Translation2d initialPosition) { + super(REBUILT_FUEL_INFO, new Pose2d(initialPosition, new Rotation2d())); + } +} diff --git a/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java b/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java new file mode 100644 index 0000000..7c9d58d --- /dev/null +++ b/src/main/java/frc/robot/util/maplesim/RebuiltFuelOnFly.java @@ -0,0 +1,53 @@ +package frc.robot.util.maplesim; + +import org.ironmaple.simulation.gamepieces.GamePieceProjectile; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; + +/** + * + * + *

Represents an FUEL launched into the air.

+ * + *

+ * This class models a {@link RebuiltFuelOnField} launched into the air. + * + *

+ * The simulation will determine if the FUEL hits its target—the HUB. + * + *

+ * The user can specify a callback using {@link #setHitNetCallBack(Runnable)}, + * which will be triggered when the FUEL + * hits the HUB. + */ +public class RebuiltFuelOnFly extends GamePieceProjectile { + + public RebuiltFuelOnFly( + Translation2d robotPosition, + Translation2d shooterPositionOnRobot, + ChassisSpeeds chassisSpeeds, + Rotation2d shooterFacing, + Distance initialHeight, + LinearVelocity launchingSpeed, + Angle shooterAngle) { + super( + RebuiltFuelOnField.REBUILT_FUEL_INFO, + robotPosition, + shooterPositionOnRobot, + chassisSpeeds, + shooterFacing, + initialHeight, + launchingSpeed, + shooterAngle); + + super.withTouchGroundHeight(Inches.of(3).in(Meters)); + super.enableBecomesGamePieceOnFieldAfterTouchGround(); + } +} diff --git a/src/main/java/frc/robot/util/maplesim/RebuiltHub.java b/src/main/java/frc/robot/util/maplesim/RebuiltHub.java new file mode 100644 index 0000000..4a3af45 --- /dev/null +++ b/src/main/java/frc/robot/util/maplesim/RebuiltHub.java @@ -0,0 +1,120 @@ +package frc.robot.util.maplesim; + +import java.util.Arrays; +import java.util.List; +import java.util.Random; + +import org.ironmaple.simulation.Goal; +import org.ironmaple.simulation.gamepieces.GamePiece; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.MetersPerSecond; + +/** + * + * + *

Simulates a HUBs on the field.

+ * + *

+ * This class simulates the HUBs on the field where + * FUELs can be scored. Whether it + * is active can be determined by using + * {@link Arena2026Rebuilt#isActive(boolean)} + */ +public class RebuiltHub extends Goal { + + protected static final Translation3d blueHubPose = new Translation3d(4.5974, 4.034536, 1.5748); + protected static final Translation3d redHubPose = new Translation3d(11.938, 4.034536, 1.5748); + protected static final Pose3d[] blueShootPoses = { + new Pose3d( + blueHubPose.plus(new Translation3d(0.5969, 0.447675, -0.5)), + new Rotation3d(Degrees.of(0), Degrees.of(-15), Degrees.of(33.75))), + new Pose3d( + blueHubPose.plus(new Translation3d(0.5969, 0.149225, -0.5)), + new Rotation3d(Degrees.of(0), Degrees.of(-15), Degrees.of(11.25))), + new Pose3d( + blueHubPose.plus(new Translation3d(0.5969, -0.149225, -0.5)), + new Rotation3d(Degrees.of(0), Degrees.of(-15), Degrees.of(11.25))), + new Pose3d( + blueHubPose.plus(new Translation3d(0.5969, -0.447675, -0.5)), + new Rotation3d(Degrees.of(0), Degrees.of(-15), Degrees.of(-33.75))) + }; + + public static final double GoalRadius = 0.5969; + static final Random rng = new Random(); + + public static final Pose3d[] redShootPoses = Arrays.stream(blueShootPoses) + .map(Arena2026Rebuilt::flip) + .map((Pose3d toRotate) -> { + return toRotate.rotateBy(new Rotation3d(Rotation2d.fromDegrees(180))); + }) + .toArray(Pose3d[]::new); + + StructPublisher posePublisher; + protected final Arena2026Rebuilt arena; + + /** + * + * + *

Creates an HUB of the specified color.

+ * + * @param arena The host arena of this HUB. + * @param isBlue Wether this is the blue HUB or the red one. + */ + public RebuiltHub(Arena2026Rebuilt arena, boolean isBlue) { + super( + arena, + Inches.of(47), + Inches.of(47), + Inches.of(10), + "Fuel", + isBlue ? blueHubPose : redHubPose, + isBlue); + + this.arena = arena; + StructPublisher HubPosePublisher = NetworkTableInstance.getDefault() + .getStructTopic("/SmartDashboard/MapleSim/Goals/" + (isBlue ? "BlueHub" : "RedHub"), Pose3d.struct) + .publish(); + HubPosePublisher.set(new Pose3d(position, new Rotation3d())); + } + + @Override + protected boolean checkCollision(GamePiece GamePiece) { + return Math.pow(GamePiece.getPose3d().getX() - position.getX(), 2) + + Math.pow(GamePiece.getPose3d().getY() - position.getY(), 2) + + Math.pow(GamePiece.getPose3d().getZ() - position.getZ(), 2) < Math.pow(GoalRadius, 2); + } + + @Override + protected void addPoints() { + arena.addValueToMatchBreakdown(isBlue, "TotalFuelInHub", 1); + arena.addValueToMatchBreakdown(isBlue, "WastedFuel", arena.isActive(isBlue) ? 0 : 1); + arena.addToScore(isBlue, arena.isActive(isBlue) ? 1 : 0); + + Pose3d shootPose = isBlue ? blueShootPoses[rng.nextInt(4)] : redShootPoses[rng.nextInt(4)]; + + arena.addPieceWithVariance( + shootPose.getTranslation().toTranslation2d(), + new Rotation2d(shootPose.getRotation().getZ()), + shootPose.getMeasureZ(), + MetersPerSecond.of(2), + shootPose.getRotation().getMeasureY(), + 0, + 0.02, + 15, + 0.2, + 5); + } + + @Override + public void draw(List drawList) { + return; + } +} diff --git a/src/main/java/frc/robot/util/maplesim/RebuiltOutpost.java b/src/main/java/frc/robot/util/maplesim/RebuiltOutpost.java new file mode 100644 index 0000000..a45a4c1 --- /dev/null +++ b/src/main/java/frc/robot/util/maplesim/RebuiltOutpost.java @@ -0,0 +1,219 @@ +package frc.robot.util.maplesim; + +import java.util.List; + +import org.ironmaple.simulation.Goal; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import static edu.wpi.first.units.Units.Centimeters; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.LinearVelocity; + +/** + * + * + *

Simulates a OUTPOSTs on the field.

+ * + *

+ * This class simulates a outpost on the field where fuel can be scored. It has + * a store of fuel which can be added + * too by scoring in it. This fuel can then be dumped via {@link #dump()} or + * {@link Arena2026Rebuilt#outpostDump(boolean)}, thrown at the goal via + * {@link #throwForGoal()} or + * {@link Arena2026Rebuilt#outpostThrowForGoal(boolean)}, or thrown with + * specific parameters via + * {@link #throwFuel(Rotation2d, Angle, LinearVelocity)} or + * {@link Arena2026Rebuilt#outpostThrow(boolean, Rotation2d, + * Angle, LinearVelocity)}, + */ +public class RebuiltOutpost extends Goal { + + protected static final Translation3d redOutpostPose = new Translation3d(16.621, 7.403338, 0); + protected static final Translation3d redLaunchPose = new Translation3d(16, 8.2, 0); + protected static final Translation3d redDumpPose = new Translation3d(16.421, 7.2, 0); + + protected static final Translation3d blueOutpostPose = new Translation3d(0, 0.665988, 0); + protected static final Translation3d blueDumpPose = new Translation3d(0.2, 0.665988, 0); + protected static final Translation3d blueLaunchPose = new Translation3d(0.665988, -0.254, 0); + + protected static final Translation3d redRenderPose = new Translation3d(16.640988, 7.7, 0.844502); + protected static final Translation3d blueRenderPose = new Translation3d(-0.12, 0.325, 0.844502); + + StructPublisher posePublisher; + + protected Arena2026Rebuilt arena; + + /** + * + * + *

Creates an outpost of the specified color.

+ * + * @param arena The host arena of this outpost. + * @param isBlue Wether this is the blue outpost or the red one. + */ + public RebuiltOutpost(Arena2026Rebuilt arena, boolean isBlue) { + super( + arena, + Centimeters.of(3), + Inches.of(21), + Centimeters.of(10), + "Fuel", + isBlue ? blueOutpostPose : redOutpostPose, + isBlue); + + this.arena = arena; + gamePieceCount = 24; + + StructPublisher OutpostPublisher = NetworkTableInstance.getDefault() + .getStructTopic( + "/SmartDashboard/MapleSim/Goals/" + (isBlue ? "BlueOutpost" : "RedOutpost"), Pose3d.struct) + .publish(); + + StructPublisher OutpostThrowPublisher = NetworkTableInstance.getDefault() + .getStructTopic( + "/SmartDashboard/MapleSim/Goals/" + (isBlue ? "BlueOutpostThrow" : "RedOutpostThrow"), + Pose3d.struct) + .publish(); + StructPublisher OutpostDumpPublisher = NetworkTableInstance.getDefault() + .getStructTopic( + "/SmartDashboard/MapleSim/Goals/" + (isBlue ? "BlueOutpostDump" : "RedOutpostDump"), + Pose3d.struct) + .publish(); + + OutpostPublisher.set(new Pose3d(position, new Rotation3d())); + OutpostDumpPublisher.set(new Pose3d(isBlue ? blueDumpPose : redDumpPose, new Rotation3d())); + OutpostThrowPublisher.set(new Pose3d(isBlue ? blueLaunchPose : redLaunchPose, new Rotation3d())); + } + + @Override + protected void addPoints() { + arena.addValueToMatchBreakdown(isBlue, "TotalFuelInOutpost", 1); + this.gamePieceCount++; + } + + @Override + public void simulationSubTick(int subTickNum) { + super.simulationSubTick(subTickNum); + arena.replaceValueInMatchBreakDown(isBlue, "CurrentFuelInOutpost", gamePieceCount); + } + + @Override + public void draw(List drawList) { + + int count = 0; + for (int col = 0; col < 5 && count < gamePieceCount; col++) { + for (int row = 0; row < 5 && count < gamePieceCount; row++) { + count++; + if (isBlue) { + drawList.add(new Pose3d( + blueRenderPose.plus( + new Translation3d(Inches.of(-6 * col), Inches.of(6 * row), Inches.of(1.3 * col))), + new Rotation3d())); + } else { + drawList.add(new Pose3d( + redRenderPose.plus( + new Translation3d(Inches.of(6 * col), Inches.of(-6 * row), Inches.of(1.3 * col))), + new Rotation3d())); + } + } + } + } + + /** Resets the outposts internal fuel counter too 24. */ + public void reset() { + gamePieceCount = 24; + } + + /** + * + * + *

Attempts too throw a game piece at the specified goal.

+ * + *

+ * This method comes with variance built in (to simulate human inconsistency) + * and will therefore only hit about + * half the time. Additionally if the hub does not have game pieces stored this + * method will not do anything. If you + * would like to manually control how the human player throws game pieces use + * {@link #outpostThrow(boolean, + * Rotation2d, Angle, LinearVelocity)} + */ + public void throwForGoal() { + throwFuel( + isBlue ? Rotation2d.fromDegrees(45) : Rotation2d.fromDegrees(220), + Degrees.of(75), + MetersPerSecond.of(11.2)); + } + + /** + * + * + *

Dumps game pieces from the specified outpost.

+ * + * This function will dump up to 24 game pieces, dependent on how many game + * pieces are currently stored in the + * outpost. For more manual control of the game pieces in the outpost use + * {@link #outpostThrow(boolean, Rotation2d, + * Angle, LinearVelocity)}. To have a human player attempt to throw a game piece + * into the hub use + * {@link #outpostThrowForGoal(boolean)}. + */ + public void dump() { + for (int i = 0; i < 24 && gamePieceCount > 0; i++) { + gamePieceCount--; + this.arena.addPieceWithVariance( + isBlue ? blueDumpPose.toTranslation2d() : redDumpPose.toTranslation2d(), + new Rotation2d(), + Meters.of(1.7), + isBlue ? MetersPerSecond.of(2) : MetersPerSecond.of(-2), + Degrees.of(0), + 0, + 0.2, + 5, + 0.2, + 5.0); + } + } + + /** + * + * + *

Throws a game piece from the outpost at the specified angle and speed. + *

+ * + *

+ * This method comes with variance built in (to simulate human inconsistency). + * Additionally if the hub does not + * have game pieces stored this method will not do anything. If you would like + * to have the human player throw at the + * hub use {@link #outpostThrowForGoal(boolean)} + * + * @param throwYaw The yaw at which too throw the ball. + * @param throwPitch The pitch at which too throw the ball. + */ + public void throwFuel(Rotation2d yaw, Angle pitch, LinearVelocity speed) { + if (gamePieceCount > 0) { + gamePieceCount--; + arena.addPieceWithVariance( + isBlue ? blueLaunchPose.toTranslation2d() : redLaunchPose.toTranslation2d(), + yaw, + Meters.of(1.7), + speed, + pitch, + 0, + 0, + 15, + 2, + 5); + } + } +}