From 297b4cdfa1c33b84b90398c624332de7681b9d5b Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Mon, 10 Feb 2025 23:08:26 -0500 Subject: [PATCH 01/31] Mosty done --- .../Robot_401_2025/config.json | 123 ++++++++++++++++++ .../test_drivebase/FeatureFlags.json | 4 +- src/main/java/frc/robot/BuildConstants.java | 12 +- src/main/java/frc/robot/InitSubsystems.java | 1 + src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/robot/RobotContainer.java | 26 ++++ 6 files changed, 161 insertions(+), 8 deletions(-) create mode 100644 advantagekit_configs/Robot_401_2025/config.json diff --git a/advantagekit_configs/Robot_401_2025/config.json b/advantagekit_configs/Robot_401_2025/config.json new file mode 100644 index 00000000..d9a8a0ee --- /dev/null +++ b/advantagekit_configs/Robot_401_2025/config.json @@ -0,0 +1,123 @@ +{ + "name": "Robot 2025", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "position": [ + 0.0, + 0.0, + 0.0 + ], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": -90 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 180 + } + ], + "zeroedPosition": [ + -0.16, + 0.31, + 0.115 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 0 + } + ], + "zeroedPosition": [ + 0, + 0, + 0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + } + ] +} diff --git a/src/main/deploy/constants/test_drivebase/FeatureFlags.json b/src/main/deploy/constants/test_drivebase/FeatureFlags.json index 9991411a..525b1f58 100644 --- a/src/main/deploy/constants/test_drivebase/FeatureFlags.json +++ b/src/main/deploy/constants/test_drivebase/FeatureFlags.json @@ -1,6 +1,6 @@ { - "runElevator": false, + "runElevator": true, "runDrive": true, "runVision": true, - "runScoring": false + "runScoring": true } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 69a5b63d..ac066b43 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 74; - public static final String GIT_SHA = "6d7db8b5d75cfa4b1cf3fe196f9227bf8058dfdc"; - public static final String GIT_DATE = "2025-02-08 16:37:18 EST"; - public static final String GIT_BRANCH = "maplesim"; - public static final String BUILD_DATE = "2025-02-10 10:15:50 EST"; - public static final long BUILD_UNIX_TIME = 1739200550815L; + public static final int GIT_REVISION = 23; + public static final String GIT_SHA = "28e21c564c1d15173b34bb090c3458b14e74aff0"; + public static final String GIT_DATE = "2025-02-10 18:24:07 EST"; + public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; + public static final String BUILD_DATE = "2025-02-10 19:57:53 EST"; + public static final long BUILD_UNIX_TIME = 1739235473531L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitSubsystems.java b/src/main/java/frc/robot/InitSubsystems.java index b2dfbbf2..25540e4b 100644 --- a/src/main/java/frc/robot/InitSubsystems.java +++ b/src/main/java/frc/robot/InitSubsystems.java @@ -50,6 +50,7 @@ public static ScoringSubsystem initScoringSubsystem() { clawMechanism = new ClawMechanism(new ClawIOTalonFX()); } break; + case MAPLESIM: case SIM: if (JsonConstants.scoringFeatureFlags.runElevator) { elevatorMechanism = new ElevatorMechanism(new ElevatorIOSim()); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 996946dc..5f1ad0b5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -87,6 +88,8 @@ public void robotPeriodic() { // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); + Logger.recordOutput("testingPose", new Pose2d()); + robotContainer.robotPeriodic(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f2654f2a..6067952e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,13 @@ import coppercore.vision.VisionLocalizer; 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.Translation3d; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,6 +29,7 @@ import frc.robot.subsystems.scoring.ScoringSubsystem; import java.io.File; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -44,6 +49,27 @@ public class RobotContainer { public static SwerveDriveSimulation driveSim = null; + public void robotPeriodic() { + // double height = 1.05*Math.sin(Timer.getTimestamp()) + 1.05; + double height = scoringSubsystem.getElevatorHeight().magnitude(); + double stage_one_height = Math.max(height - 0.55, 0.0); + double stage_two_height = Math.max(stage_one_height - 0.8, 0.0); + double claw_rotation = 2 * Math.sin(Timer.getTimestamp() * 1.0 / 3.0) + 2.0; + double ramp_rotation = Math.sin(Timer.getTimestamp() * 1.5) + 1.0; + Logger.recordOutput( + "componetPositions", + new Pose3d[] { + new Pose3d(new Translation3d(0.05, 0.01, 0.9), new Rotation3d(0.0, ramp_rotation, 0.0)), + new Pose3d(), + new Pose3d( + new Translation3d(0.34, 0.12, height + 0.35), + new Rotation3d(0.0, claw_rotation, 0.0)), + new Pose3d(new Translation3d(0.0, 0.0, height), new Rotation3d(0.0, 0.0, 0.0)), + new Pose3d(new Translation3d(0.0, 0.0, stage_two_height), new Rotation3d(0.0, 0.0, 0.0)), + new Pose3d(new Translation3d(0.0, 0.0, stage_one_height), new Rotation3d(0.0, 0.0, 0.0)) + }); + } + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { loadConstants(); From 6306d1267ca611dab2c4b907451f75d1c12ddbb7 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Mon, 10 Feb 2025 23:38:52 -0500 Subject: [PATCH 02/31] Elevator works with tempory solution in ScoringSubsystem --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/constants/ModeConstants.java | 2 +- .../frc/robot/subsystems/scoring/ScoringSubsystem.java | 5 ++--- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index ac066b43..c7ba7439 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 23; - public static final String GIT_SHA = "28e21c564c1d15173b34bb090c3458b14e74aff0"; - public static final String GIT_DATE = "2025-02-10 18:24:07 EST"; + public static final int GIT_REVISION = 24; + public static final String GIT_SHA = "297b4cdfa1c33b84b90398c624332de7681b9d5b"; + public static final String GIT_DATE = "2025-02-10 23:08:26 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-10 19:57:53 EST"; - public static final long BUILD_UNIX_TIME = 1739235473531L; + public static final String BUILD_DATE = "2025-02-10 23:31:39 EST"; + public static final long BUILD_UNIX_TIME = 1739248299336L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index f5a15aa0..bac91156 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -15,7 +15,7 @@ public enum Mode { * mode. The robot will automatically detect a real robot, so this value doesn't need to be * changed to Mode.Real when on a real robot. */ - public static final Mode simMode = Mode.MAPLESIM; + public static final Mode simMode = Mode.SIM; public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 5c37c2cf..1fc81370 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -12,7 +12,6 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.ExampleElevatorCommand; import frc.robot.constants.JsonConstants; import frc.robot.subsystems.scoring.states.IdleState; import frc.robot.subsystems.scoring.states.IntakeState; @@ -101,7 +100,7 @@ public ScoringSubsystem(ElevatorMechanism elevatorMechanism, ClawMechanism clawM this.elevatorMechanism = elevatorMechanism; this.clawMechanism = clawMechanism; - setDefaultCommand(new ExampleElevatorCommand(this)); + // setDefaultCommand(new ExampleElevatorCommand(this)); instance = this; @@ -156,7 +155,7 @@ public void fireTrigger(ScoringTrigger trigger) { */ public void setElevatorGoalHeight(Distance goalHeight) { if (JsonConstants.scoringFeatureFlags.runElevator) { - elevatorMechanism.setGoalHeight(goalHeight); + // elevatorMechanism.setGoalHeight(goalHeight); } } From 37171a09e7f5af8174e27509bfb0b60e5420bbb6 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Thu, 13 Feb 2025 17:01:12 -0500 Subject: [PATCH 03/31] Make maplesim work with feature flags --- .../test_drivebase/FeatureFlags.json | 4 +-- src/main/java/frc/robot/BuildConstants.java | 10 +++--- src/main/java/frc/robot/Robot.java | 24 ++++++-------- src/main/java/frc/robot/RobotContainer.java | 31 ++++++++++++++----- .../frc/robot/constants/ModeConstants.java | 2 +- .../subsystems/scoring/ScoringSubsystem.java | 8 ++--- 6 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/constants/test_drivebase/FeatureFlags.json b/src/main/deploy/constants/test_drivebase/FeatureFlags.json index 525b1f58..b103a84f 100644 --- a/src/main/deploy/constants/test_drivebase/FeatureFlags.json +++ b/src/main/deploy/constants/test_drivebase/FeatureFlags.json @@ -1,6 +1,6 @@ { "runElevator": true, - "runDrive": true, + "runDrive": false, "runVision": true, - "runScoring": true + "runScoring": false } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c7ba7439..322448cc 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 24; - public static final String GIT_SHA = "297b4cdfa1c33b84b90398c624332de7681b9d5b"; - public static final String GIT_DATE = "2025-02-10 23:08:26 EST"; + public static final int GIT_REVISION = 25; + public static final String GIT_SHA = "6306d1267ca611dab2c4b907451f75d1c12ddbb7"; + public static final String GIT_DATE = "2025-02-10 23:38:52 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-10 23:31:39 EST"; - public static final long BUILD_UNIX_TIME = 1739248299336L; + public static final String BUILD_DATE = "2025-02-13 16:55:24 EST"; + public static final long BUILD_UNIX_TIME = 1739483724679L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5f1ad0b5..1876b5c3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,10 +1,5 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.constants.ModeConstants; import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -13,6 +8,12 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Threads; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.ModeConstants; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -88,8 +89,6 @@ public void robotPeriodic() { // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); - Logger.recordOutput("testingPose", new Pose2d()); - robotContainer.robotPeriodic(); } /** This function is called once when the robot is disabled. */ @@ -161,15 +160,10 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { + Logger.recordOutput("testingPose", new Pose2d()); + robotContainer.updateRobotModel(); if (ModeConstants.currentMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { - SimulatedArena.getInstance().simulationPeriodic(); - Logger.recordOutput( - "FieldSimulation/RobotPosition", RobotContainer.driveSim.getSimulatedDriveTrainPose()); - - Logger.recordOutput( - "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral")); - Logger.recordOutput( - "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); + robotContainer.updateMapleSim(); } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6067952e..d3038f76 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,10 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import java.io.File; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; import coppercore.vision.VisionLocalizer; import edu.wpi.first.math.geometry.Pose2d; @@ -8,6 +12,8 @@ import edu.wpi.first.math.geometry.Rotation2d; 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.Meters; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Timer; @@ -27,9 +33,6 @@ import frc.robot.constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.scoring.ScoringSubsystem; -import java.io.File; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -49,9 +52,11 @@ public class RobotContainer { public static SwerveDriveSimulation driveSim = null; - public void robotPeriodic() { - // double height = 1.05*Math.sin(Timer.getTimestamp()) + 1.05; - double height = scoringSubsystem.getElevatorHeight().magnitude(); + public void updateRobotModel(){ + double height = 0; + if (scoringSubsystem != null){ + height = scoringSubsystem.getElevatorHeight().magnitude(); + } double stage_one_height = Math.max(height - 0.55, 0.0); double stage_two_height = Math.max(stage_one_height - 0.8, 0.0); double claw_rotation = 2 * Math.sin(Timer.getTimestamp() * 1.0 / 3.0) + 2.0; @@ -212,4 +217,16 @@ public void disabledPeriodic() { public void disabledInit() { CommandScheduler.getInstance().cancelAll(); } + + public void updateMapleSim() { + SimulatedArena.getInstance().simulationPeriodic(); + if (driveSim != null){ + Logger.recordOutput( + "FieldSimulation/RobotPosition", RobotContainer.driveSim.getSimulatedDriveTrainPose()); + } + Logger.recordOutput( + "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral")); + Logger.recordOutput( + "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); + } } diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index bac91156..f5a15aa0 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -15,7 +15,7 @@ public enum Mode { * mode. The robot will automatically detect a real robot, so this value doesn't need to be * changed to Mode.Real when on a real robot. */ - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.MAPLESIM; public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 1fc81370..092c3ada 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -1,12 +1,13 @@ package frc.robot.subsystems.scoring; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Rotations; +import org.littletonrobotics.junction.Logger; import coppercore.controls.state_machine.StateMachine; import coppercore.controls.state_machine.StateMachineConfiguration; import coppercore.controls.state_machine.state.PeriodicStateInterface; import coppercore.controls.state_machine.state.StateContainer; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Rotations; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; @@ -17,7 +18,6 @@ import frc.robot.subsystems.scoring.states.IntakeState; import frc.robot.subsystems.scoring.states.ScoreState; import frc.robot.subsystems.scoring.states.WarmupState; -import org.littletonrobotics.junction.Logger; public class ScoringSubsystem extends SubsystemBase { private ElevatorMechanism elevatorMechanism; @@ -155,7 +155,7 @@ public void fireTrigger(ScoringTrigger trigger) { */ public void setElevatorGoalHeight(Distance goalHeight) { if (JsonConstants.scoringFeatureFlags.runElevator) { - // elevatorMechanism.setGoalHeight(goalHeight); + elevatorMechanism.setGoalHeight(goalHeight); } } From b2ab5538fa9285b09f7aa7ede7298ead93a6b856 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Sat, 15 Feb 2025 12:25:13 -0500 Subject: [PATCH 04/31] . --- .../test_drivebase/FeatureFlags.json | 4 +-- src/main/java/frc/robot/BuildConstants.java | 10 +++--- src/main/java/frc/robot/Robot.java | 13 +++---- src/main/java/frc/robot/RobotContainer.java | 34 ++++++++++++------- .../subsystems/scoring/ScoringSubsystem.java | 6 ++-- 5 files changed, 36 insertions(+), 31 deletions(-) diff --git a/src/main/deploy/constants/test_drivebase/FeatureFlags.json b/src/main/deploy/constants/test_drivebase/FeatureFlags.json index b103a84f..525b1f58 100644 --- a/src/main/deploy/constants/test_drivebase/FeatureFlags.json +++ b/src/main/deploy/constants/test_drivebase/FeatureFlags.json @@ -1,6 +1,6 @@ { "runElevator": true, - "runDrive": false, + "runDrive": true, "runVision": true, - "runScoring": false + "runScoring": true } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 322448cc..f631b108 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 25; - public static final String GIT_SHA = "6306d1267ca611dab2c4b907451f75d1c12ddbb7"; - public static final String GIT_DATE = "2025-02-10 23:38:52 EST"; + public static final int GIT_REVISION = 26; + public static final String GIT_SHA = "37171a09e7f5af8174e27509bfb0b60e5420bbb6"; + public static final String GIT_DATE = "2025-02-13 17:01:12 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-13 16:55:24 EST"; - public static final long BUILD_UNIX_TIME = 1739483724679L; + public static final String BUILD_DATE = "2025-02-13 19:03:53 EST"; + public static final long BUILD_UNIX_TIME = 1739491433194L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1876b5c3..9e2cf584 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,5 +1,9 @@ package frc.robot; +import edu.wpi.first.wpilibj.Threads; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.ModeConstants; import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -8,12 +12,6 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.constants.ModeConstants; - /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -86,6 +84,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + robotContainer.updateRobotModel(); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); @@ -160,8 +159,6 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { - Logger.recordOutput("testingPose", new Pose2d()); - robotContainer.updateRobotModel(); if (ModeConstants.currentMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { robotContainer.updateMapleSim(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d3038f76..039eb582 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,10 +1,7 @@ package frc.robot; -import java.io.File; - -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import coppercore.vision.VisionLocalizer; import edu.wpi.first.math.geometry.Pose2d; @@ -12,8 +9,6 @@ import edu.wpi.first.math.geometry.Rotation2d; 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.Meters; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Timer; @@ -33,6 +28,10 @@ import frc.robot.constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.scoring.ScoringSubsystem; +import java.io.File; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -52,15 +51,22 @@ public class RobotContainer { public static SwerveDriveSimulation driveSim = null; - public void updateRobotModel(){ - double height = 0; - if (scoringSubsystem != null){ + public void updateRobotModel() { + // double height = (1.87 * Math.sin(Timer.getTimestamp()) + 1.87) * 0.5; + double height = 0.0; + if (scoringSubsystem != null) { height = scoringSubsystem.getElevatorHeight().magnitude(); } + double extra = Math.max(height - 1.87, 0.0); + height = Math.min(height, 1.87); double stage_one_height = Math.max(height - 0.55, 0.0); - double stage_two_height = Math.max(stage_one_height - 0.8, 0.0); + double stage_two_height = Math.max(stage_one_height - 0.66, 0.0); double claw_rotation = 2 * Math.sin(Timer.getTimestamp() * 1.0 / 3.0) + 2.0; double ramp_rotation = Math.sin(Timer.getTimestamp() * 1.5) + 1.0; + + Logger.recordOutput( + "testingPose", + new Pose3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, extra, 0.0))); Logger.recordOutput( "componetPositions", new Pose3d[] { @@ -112,6 +118,7 @@ public void configureSubsystems() { if (FeatureFlags.synced.getObject().runScoring) { scoringSubsystem = InitSubsystems.initScoringSubsystem(); } + scoringSubsystem.setOverrideStateMachine(true); if (FeatureFlags.synced.getObject().runDrive) { drive = InitSubsystems.initDriveSubsystem(); if (ModeConstants.simMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { @@ -148,6 +155,7 @@ public Command getAutonomousCommand() { public void periodic() { strategyManager.periodic(); + scoringSubsystem.setOverrideStateMachine(true); } public void autonomousInit() { @@ -220,9 +228,9 @@ public void disabledInit() { public void updateMapleSim() { SimulatedArena.getInstance().simulationPeriodic(); - if (driveSim != null){ + if (driveSim != null) { Logger.recordOutput( - "FieldSimulation/RobotPosition", RobotContainer.driveSim.getSimulatedDriveTrainPose()); + "FieldSimulation/RobotPosition", RobotContainer.driveSim.getSimulatedDriveTrainPose()); } Logger.recordOutput( "FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral")); diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 092c3ada..236a53e6 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -1,13 +1,12 @@ package frc.robot.subsystems.scoring; -import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Rotations; import coppercore.controls.state_machine.StateMachine; import coppercore.controls.state_machine.StateMachineConfiguration; import coppercore.controls.state_machine.state.PeriodicStateInterface; import coppercore.controls.state_machine.state.StateContainer; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Rotations; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; @@ -18,6 +17,7 @@ import frc.robot.subsystems.scoring.states.IntakeState; import frc.robot.subsystems.scoring.states.ScoreState; import frc.robot.subsystems.scoring.states.WarmupState; +import org.littletonrobotics.junction.Logger; public class ScoringSubsystem extends SubsystemBase { private ElevatorMechanism elevatorMechanism; From a231798d14175793887ed7c06d90fba18908698c Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Sat, 15 Feb 2025 14:34:01 -0500 Subject: [PATCH 05/31] . --- .../Robot_401_2025/config.json | 6 ++-- build.gradle | 2 +- .../test_drivebase/FeatureFlags.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 12 +++---- src/main/java/frc/robot/InitSubsystems.java | 1 - src/main/java/frc/robot/RobotContainer.java | 33 +++++++++++-------- .../subsystems/climb/ClimbSubsystem.java | 4 +++ 7 files changed, 34 insertions(+), 26 deletions(-) diff --git a/advantagekit_configs/Robot_401_2025/config.json b/advantagekit_configs/Robot_401_2025/config.json index d9a8a0ee..7b0bac57 100644 --- a/advantagekit_configs/Robot_401_2025/config.json +++ b/advantagekit_configs/Robot_401_2025/config.json @@ -46,9 +46,9 @@ } ], "zeroedPosition": [ - -0.16, - 0.31, - 0.115 + 0.0, + 0.0, + 0.0 ] }, { diff --git a/build.gradle b/build.gradle index 280461ba..a1ae1e0a 100644 --- a/build.gradle +++ b/build.gradle @@ -101,7 +101,7 @@ dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" - def coppercoreVersion = "2025.1.8" // mavenLocal version + def coppercoreVersion = "2025.1.9" // mavenLocal version implementation "io.github.team401.coppercore:controls:$coppercoreVersion" implementation "io.github.team401.coppercore:geometry:$coppercoreVersion" diff --git a/src/main/deploy/constants/test_drivebase/FeatureFlags.json b/src/main/deploy/constants/test_drivebase/FeatureFlags.json index 4c1e126d..b9258c6f 100644 --- a/src/main/deploy/constants/test_drivebase/FeatureFlags.json +++ b/src/main/deploy/constants/test_drivebase/FeatureFlags.json @@ -1,6 +1,6 @@ { "runDrive": true, "runVision": true, - "runScoring": false, + "runScoring": true, "runClimb": true } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bb858363..9b585682 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 28; - public static final String GIT_SHA = "e548533620981bc2bd6a90a74d9df246447bcb03"; - public static final String GIT_DATE = "2025-02-13 16:09:44 EST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2025-02-14 11:15:57 EST"; - public static final long BUILD_UNIX_TIME = 1739549757302L; + public static final int GIT_REVISION = 34; + public static final String GIT_SHA = "d0d378ed5de7af90c77ad067ffc60c221e7517ea"; + public static final String GIT_DATE = "2025-02-15 13:15:30 EST"; + public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; + public static final String BUILD_DATE = "2025-02-15 14:21:30 EST"; + public static final long BUILD_UNIX_TIME = 1739647290837L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitSubsystems.java b/src/main/java/frc/robot/InitSubsystems.java index 665b6948..5c723eaf 100644 --- a/src/main/java/frc/robot/InitSubsystems.java +++ b/src/main/java/frc/robot/InitSubsystems.java @@ -57,7 +57,6 @@ public static ScoringSubsystem initScoringSubsystem() { clawMechanism = new ClawMechanism(new ClawIOTalonFX()); } break; - case MAPLESIM: case SIM: case MAPLESIM: // TODO: Once ground intake is added, make sure this plays nice with it in // maplesim diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 75508c76..ecad1c84 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,7 +1,10 @@ package frc.robot; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; +import java.io.File; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; import coppercore.vision.VisionLocalizer; import edu.wpi.first.math.geometry.Pose2d; @@ -9,6 +12,8 @@ import edu.wpi.first.math.geometry.Rotation2d; 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.Meters; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Timer; @@ -29,10 +34,6 @@ import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.scoring.ScoringSubsystem; -import java.io.File; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -54,26 +55,29 @@ public class RobotContainer { public static SwerveDriveSimulation driveSim = null; public void updateRobotModel() { - // double height = (1.87 * Math.sin(Timer.getTimestamp()) + 1.87) * 0.5; double height = 0.0; + double claw_rotation = 0.0; + double ramp_rotation = Math.sin(Timer.getTimestamp() * 1.5) + 1.0; + double climb_rotation = 0.0; if (scoringSubsystem != null) { height = scoringSubsystem.getElevatorHeight().magnitude(); + claw_rotation = scoringSubsystem.getWristAngle().magnitude(); + } + if (climbSubsystem != null) { + climb_rotation = climbSubsystem.getRotation().magnitude(); } - double extra = Math.max(height - 1.87, 0.0); height = Math.min(height, 1.87); double stage_one_height = Math.max(height - 0.55, 0.0); double stage_two_height = Math.max(stage_one_height - 0.66, 0.0); - double claw_rotation = 2 * Math.sin(Timer.getTimestamp() * 1.0 / 3.0) + 2.0; - double ramp_rotation = Math.sin(Timer.getTimestamp() * 1.5) + 1.0; - Logger.recordOutput( "testingPose", - new Pose3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, extra, 0.0))); + new Pose3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0))); Logger.recordOutput( "componetPositions", new Pose3d[] { new Pose3d(new Translation3d(0.05, 0.01, 0.9), new Rotation3d(0.0, ramp_rotation, 0.0)), - new Pose3d(), + new Pose3d( + new Translation3d(-0.16, 0.31, 0.115), new Rotation3d(climb_rotation, 0.0, 0.0)), new Pose3d( new Translation3d(0.34, 0.12, height + 0.35), new Rotation3d(0.0, claw_rotation, 0.0)), @@ -82,6 +86,7 @@ public void updateRobotModel() { new Pose3d(new Translation3d(0.0, 0.0, stage_one_height), new Rotation3d(0.0, 0.0, 0.0)) }); } + // The robot's subsystems and commands are defined here /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -121,7 +126,7 @@ public void configureSubsystems() { if (FeatureFlags.synced.getObject().runScoring) { scoringSubsystem = InitSubsystems.initScoringSubsystem(); } - scoringSubsystem.setOverrideStateMachine(true); + // scoringSubsystem.setOverrideStateMachine(true); if (FeatureFlags.synced.getObject().runDrive) { drive = InitSubsystems.initDriveSubsystem(); if (ModeConstants.simMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index bce6116b..6b3a6485 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -101,6 +101,10 @@ public boolean getLockedToCage() { return inputs.lockedToCage; } + public Angle getRotation() { + return inputs.motorAngle; + } + @Override public void periodic() { From 8c326eb9cc7a4e9b0c90af53baed5a32182a6e9a Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Sat, 15 Feb 2025 15:21:36 -0500 Subject: [PATCH 06/31] Elevator not working --- simgui-ds.json | 2 -- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/RobotContainer.java | 17 +++++++---------- .../subsystems/scoring/ScoringSubsystem.java | 2 +- 4 files changed, 14 insertions(+), 19 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 4dcea395..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,8 +90,6 @@ } ], "robotJoysticks": [ - {}, - {}, { "guid": "Keyboard0" } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 9b585682..f4fb118e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 34; - public static final String GIT_SHA = "d0d378ed5de7af90c77ad067ffc60c221e7517ea"; - public static final String GIT_DATE = "2025-02-15 13:15:30 EST"; + public static final int GIT_REVISION = 35; + public static final String GIT_SHA = "a231798d14175793887ed7c06d90fba18908698c"; + public static final String GIT_DATE = "2025-02-15 14:34:01 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-15 14:21:30 EST"; - public static final long BUILD_UNIX_TIME = 1739647290837L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2025-02-15 15:08:32 EST"; + public static final long BUILD_UNIX_TIME = 1739650112290L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ecad1c84..c32728de 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,10 +1,7 @@ package frc.robot; -import java.io.File; - -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import coppercore.vision.VisionLocalizer; import edu.wpi.first.math.geometry.Pose2d; @@ -12,8 +9,6 @@ import edu.wpi.first.math.geometry.Rotation2d; 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.Meters; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Timer; @@ -34,6 +29,10 @@ import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.scoring.ScoringSubsystem; +import java.io.File; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -70,8 +69,7 @@ public void updateRobotModel() { double stage_one_height = Math.max(height - 0.55, 0.0); double stage_two_height = Math.max(stage_one_height - 0.66, 0.0); Logger.recordOutput( - "testingPose", - new Pose3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0))); + "testingPose", new Pose3d(new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, 0.0))); Logger.recordOutput( "componetPositions", new Pose3d[] { @@ -126,7 +124,6 @@ public void configureSubsystems() { if (FeatureFlags.synced.getObject().runScoring) { scoringSubsystem = InitSubsystems.initScoringSubsystem(); } - // scoringSubsystem.setOverrideStateMachine(true); if (FeatureFlags.synced.getObject().runDrive) { drive = InitSubsystems.initDriveSubsystem(); if (ModeConstants.simMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index bb832df9..d483158a 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -179,7 +179,7 @@ public ScoringSubsystem( this.clawMechanism = clawMechanism; instance = this; - + stateMachineConfiguration = new StateMachineConfiguration<>(); stateMachineConfiguration From d998e24e91ef962dc912764b46dfce9d964b553a Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Sat, 15 Feb 2025 16:39:10 -0500 Subject: [PATCH 07/31] . --- src/main/deploy/constants/config.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 1 - .../robot/subsystems/scoring/ScoringSubsystem.java | 2 +- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/constants/config.json b/src/main/deploy/constants/config.json index 6352756e..9055af5d 100644 --- a/src/main/deploy/constants/config.json +++ b/src/main/deploy/constants/config.json @@ -1,5 +1,5 @@ { - "environment": "comp", + "environment": "test_drivebase", "defaults": "comp", "environments": [ { diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f4fb118e..6fd320ed 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 35; - public static final String GIT_SHA = "a231798d14175793887ed7c06d90fba18908698c"; - public static final String GIT_DATE = "2025-02-15 14:34:01 EST"; + public static final int GIT_REVISION = 39; + public static final String GIT_SHA = "c77d2b13d57800c55ed12eda0aea95f329e4215d"; + public static final String GIT_DATE = "2025-02-15 15:22:12 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-15 15:08:32 EST"; - public static final long BUILD_UNIX_TIME = 1739650112290L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2025-02-15 15:28:57 EST"; + public static final long BUILD_UNIX_TIME = 1739651337608L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9e2cf584..df7fc3a5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -84,7 +84,6 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - robotContainer.updateRobotModel(); // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); @@ -159,6 +158,8 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { + robotContainer.updateRobotModel(); + if (ModeConstants.currentMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { robotContainer.updateMapleSim(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 744b85ad..4ca58bc7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -182,7 +182,6 @@ public Command getAutonomousCommand() { public void periodic() { strategyManager.periodic(); - scoringSubsystem.setOverrideStateMachine(true); } public void autonomousInit() { diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index d483158a..bb832df9 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -179,7 +179,7 @@ public ScoringSubsystem( this.clawMechanism = clawMechanism; instance = this; - + stateMachineConfiguration = new StateMachineConfiguration<>(); stateMachineConfiguration From ebec31b0010b57eb251b0deaf420ac0a8dd9f74c Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Sat, 15 Feb 2025 18:00:50 -0500 Subject: [PATCH 08/31] It works --- simgui-ds.json | 3 +++ .../deploy/constants/comp/RampConstants.json | 4 ++-- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/InitSubsystems.java | 1 + src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++------ .../frc/robot/subsystems/ramp/RampSubsystem.java | 4 ++++ 6 files changed, 25 insertions(+), 13 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 442546ae..92d025e1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -100,6 +100,9 @@ }, { "guid": "Keyboard1" + }, + { + "guid": "Keyboard2" } ] } diff --git a/src/main/deploy/constants/comp/RampConstants.json b/src/main/deploy/constants/comp/RampConstants.json index 58278a83..71b23a68 100644 --- a/src/main/deploy/constants/comp/RampConstants.json +++ b/src/main/deploy/constants/comp/RampConstants.json @@ -1,6 +1,6 @@ { - "intakePosition": 1.0, - "climbPosition": 2.5, + "intakePosition": 0.0, + "climbPosition": 1.5, "motorId": 1, "positionRange": 0.02, "PID_TalonFX_P": 0.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6fd320ed..c5357022 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 39; - public static final String GIT_SHA = "c77d2b13d57800c55ed12eda0aea95f329e4215d"; - public static final String GIT_DATE = "2025-02-15 15:22:12 EST"; + public static final int GIT_REVISION = 40; + public static final String GIT_SHA = "d998e24e91ef962dc912764b46dfce9d964b553a"; + public static final String GIT_DATE = "2025-02-15 16:39:10 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-15 15:28:57 EST"; - public static final long BUILD_UNIX_TIME = 1739651337608L; + public static final String BUILD_DATE = "2025-02-15 17:40:11 EST"; + public static final long BUILD_UNIX_TIME = 1739659211484L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitSubsystems.java b/src/main/java/frc/robot/InitSubsystems.java index 4befdfd9..9430d431 100644 --- a/src/main/java/frc/robot/InitSubsystems.java +++ b/src/main/java/frc/robot/InitSubsystems.java @@ -91,6 +91,7 @@ public static ClimbSubsystem initClimbSubsystem() { case REAL: throw new UnsupportedOperationException("Climb real functions are not yet implemented."); case SIM: + case MAPLESIM: return new ClimbSubsystem(new ClimbIOSim()); case REPLAY: throw new UnsupportedOperationException("Climb replay is not yet implemented."); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ca58bc7..5e241daa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,11 +30,15 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; + import java.io.File; + import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.littletonrobotics.junction.Logger; +import static edu.wpi.first.units.Units.Radians; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -58,15 +62,18 @@ public class RobotContainer { public void updateRobotModel() { double height = 0.0; double claw_rotation = 0.0; - double ramp_rotation = Math.sin(Timer.getTimestamp() * 1.5) + 1.0; + double ramp_rotation = 0.0; double climb_rotation = 0.0; if (scoringSubsystem != null) { height = scoringSubsystem.getElevatorHeight().magnitude(); - claw_rotation = scoringSubsystem.getWristAngle().magnitude(); + claw_rotation = scoringSubsystem.getWristAngle().in(Radians); } if (climbSubsystem != null) { climb_rotation = climbSubsystem.getRotation().magnitude(); } + if (rampSubsystem != null){ + ramp_rotation = rampSubsystem.getPosition(); + } height = Math.min(height, 1.87); double stage_one_height = Math.max(height - 0.55, 0.0); double stage_two_height = Math.max(stage_one_height - 0.66, 0.0); @@ -80,7 +87,7 @@ public void updateRobotModel() { new Translation3d(-0.16, 0.31, 0.115), new Rotation3d(climb_rotation, 0.0, 0.0)), new Pose3d( new Translation3d(0.34, 0.12, height + 0.35), - new Rotation3d(0.0, claw_rotation, 0.0)), + new Rotation3d(0.0, -claw_rotation, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_two_height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_one_height), new Rotation3d(0.0, 0.0, 0.0)) @@ -123,9 +130,6 @@ public void configureAutos() { } public void configureSubsystems() { - if (FeatureFlags.synced.getObject().runScoring) { - scoringSubsystem = InitSubsystems.initScoringSubsystem(); - } if (FeatureFlags.synced.getObject().runDrive) { drive = InitSubsystems.initDriveSubsystem(); if (ModeConstants.simMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { diff --git a/src/main/java/frc/robot/subsystems/ramp/RampSubsystem.java b/src/main/java/frc/robot/subsystems/ramp/RampSubsystem.java index 6f0c7c30..8c3f5afe 100644 --- a/src/main/java/frc/robot/subsystems/ramp/RampSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ramp/RampSubsystem.java @@ -30,4 +30,8 @@ public void prepareForIntake() { public boolean isInPosition() { return mechanism.inPosition(); } + + public double getPosition() { + return mechanism.inputs.position; + } } From 05518ae9889665c7e51c81aa19dc6d1bd51d2fc0 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Mon, 17 Feb 2025 13:05:26 -0500 Subject: [PATCH 09/31] MapleSim does not work still --- .../deploy/constants/comp/FeatureFlags.json | 2 +- src/main/deploy/constants/config.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 ++++----- src/main/java/frc/robot/RobotContainer.java | 21 ++++++------------- .../frc/robot/constants/ModeConstants.java | 2 +- 5 files changed, 14 insertions(+), 23 deletions(-) diff --git a/src/main/deploy/constants/comp/FeatureFlags.json b/src/main/deploy/constants/comp/FeatureFlags.json index ff6bb68b..99498676 100644 --- a/src/main/deploy/constants/comp/FeatureFlags.json +++ b/src/main/deploy/constants/comp/FeatureFlags.json @@ -1,7 +1,7 @@ { "runDrive": true, "runVision": true, - "runScoring": true, + "runScoring": false, "runClimb": true, "runRamp": true } diff --git a/src/main/deploy/constants/config.json b/src/main/deploy/constants/config.json index 9055af5d..6352756e 100644 --- a/src/main/deploy/constants/config.json +++ b/src/main/deploy/constants/config.json @@ -1,5 +1,5 @@ { - "environment": "test_drivebase", + "environment": "comp", "defaults": "comp", "environments": [ { diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c5357022..9ddf5538 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 40; - public static final String GIT_SHA = "d998e24e91ef962dc912764b46dfce9d964b553a"; - public static final String GIT_DATE = "2025-02-15 16:39:10 EST"; + public static final int GIT_REVISION = 41; + public static final String GIT_SHA = "ebec31b0010b57eb251b0deaf420ac0a8dd9f74c"; + public static final String GIT_DATE = "2025-02-15 18:00:50 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-15 17:40:11 EST"; - public static final long BUILD_UNIX_TIME = 1739659211484L; + public static final String BUILD_DATE = "2025-02-17 13:02:14 EST"; + public static final long BUILD_UNIX_TIME = 1739815334814L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e241daa..1fece201 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,17 +1,13 @@ package frc.robot; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; import coppercore.vision.VisionLocalizer; -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.Translation3d; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -24,21 +20,16 @@ import frc.robot.constants.AutoStrategyContainer; import frc.robot.constants.FeatureFlags; import frc.robot.constants.JsonConstants; -import frc.robot.constants.ModeConstants; import frc.robot.constants.OperatorConstants; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; - import java.io.File; - import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.littletonrobotics.junction.Logger; -import static edu.wpi.first.units.Units.Radians; - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -71,7 +62,7 @@ public void updateRobotModel() { if (climbSubsystem != null) { climb_rotation = climbSubsystem.getRotation().magnitude(); } - if (rampSubsystem != null){ + if (rampSubsystem != null) { ramp_rotation = rampSubsystem.getPosition(); } height = Math.min(height, 1.87); @@ -132,10 +123,10 @@ public void configureAutos() { public void configureSubsystems() { if (FeatureFlags.synced.getObject().runDrive) { drive = InitSubsystems.initDriveSubsystem(); - if (ModeConstants.simMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { - drive.setPose( - new Pose2d(Meters.of(14.350), Meters.of(4.0), new Rotation2d(Degrees.of(180)))); - } + // if (ModeConstants.simMode == frc.robot.constants.ModeConstants.Mode.MAPLESIM) { + // drive.setPose( + // new Pose2d(Meters.of(14.350), Meters.of(4.0), new Rotation2d(Degrees.of(180)))); + // } if (FeatureFlags.synced.getObject().runVision) { vision = InitSubsystems.initVisionSubsystem(drive); diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index bac91156..f5a15aa0 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -15,7 +15,7 @@ public enum Mode { * mode. The robot will automatically detect a real robot, so this value doesn't need to be * changed to Mode.Real when on a real robot. */ - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.MAPLESIM; public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } From a3f36b0c4c12630f6e22893270ab542e2dfc2727 Mon Sep 17 00:00:00 2001 From: avidraccoon Date: Mon, 17 Feb 2025 14:22:41 -0500 Subject: [PATCH 10/31] Elevator works in maple sim --- src/main/deploy/auto/ExampleAutoPath.json | 2 +- src/main/deploy/constants/config.json | 2 +- .../constants/test_drivebase/ClimbConstants.json | 10 +++++----- .../constants/test_drivebase/ElevatorConstants.json | 8 ++++---- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/InitSubsystems.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/constants/ModeConstants.java | 2 +- .../robot/constants/subsystems/ElevatorConstants.java | 4 ++-- 9 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/auto/ExampleAutoPath.json b/src/main/deploy/auto/ExampleAutoPath.json index 54908a25..8b747901 100644 --- a/src/main/deploy/auto/ExampleAutoPath.json +++ b/src/main/deploy/auto/ExampleAutoPath.json @@ -1,6 +1,6 @@ { "scoringLocations": [ - "Reef2", + "Reef5", "Reef5" ], "scoringLevels": [ diff --git a/src/main/deploy/constants/config.json b/src/main/deploy/constants/config.json index 6352756e..9055af5d 100644 --- a/src/main/deploy/constants/config.json +++ b/src/main/deploy/constants/config.json @@ -1,5 +1,5 @@ { - "environment": "comp", + "environment": "test_drivebase", "defaults": "comp", "environments": [ { diff --git a/src/main/deploy/constants/test_drivebase/ClimbConstants.json b/src/main/deploy/constants/test_drivebase/ClimbConstants.json index ed7e85e1..3077218d 100644 --- a/src/main/deploy/constants/test_drivebase/ClimbConstants.json +++ b/src/main/deploy/constants/test_drivebase/ClimbConstants.json @@ -1,18 +1,18 @@ { "restingAngle": { "value": 20.0, - "unit": "Degrees" + "unit": "Degree" }, "searchingAngle": { "value": 60.0, - "unit": "Degrees" + "unit": "Degree" }, "finalHangingAngle": { "value": 0.0, - "unit": "Degrees" + "unit": "Degree" }, - "leadClimbMotorId": 0, - "followerClimbMotorId": 1, + "leadClimbMotorId": 34, + "followerClimbMotorId": 35, "climbP": 5, "climbI": 0, "climbD": 0, diff --git a/src/main/deploy/constants/test_drivebase/ElevatorConstants.json b/src/main/deploy/constants/test_drivebase/ElevatorConstants.json index bee9f05e..7e58c7ea 100644 --- a/src/main/deploy/constants/test_drivebase/ElevatorConstants.json +++ b/src/main/deploy/constants/test_drivebase/ElevatorConstants.json @@ -1,6 +1,6 @@ { - "leadElevatorMotorId": 9, - "followerElevatorMotorId": 10, + "leadElevatorMotorId": 30, + "followerElevatorMotorId": 31, "invertFollowerElevatorMotor": false, "smallCANCoderTeeth": 17, "largeCANCoderTeeth": 19, @@ -19,9 +19,9 @@ "unit": "Inch" }, "elevatorCANCoderDiscontinuityPoint": 1.0, - "elevatorLargeCANCoderID": 11, + "elevatorLargeCANCoderID": 32, "isLargeCANcoderClockwisePositive": true, - "elevatorSmallCANCoderID": 12, + "elevatorSmallCANCoderID": 33, "isSmallCANcoderClockwisePositive": true, "elevatorkP": 40.0, "elevatorkI": 0.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 9ddf5538..e0c5dbdc 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 41; - public static final String GIT_SHA = "ebec31b0010b57eb251b0deaf420ac0a8dd9f74c"; - public static final String GIT_DATE = "2025-02-15 18:00:50 EST"; + public static final int GIT_REVISION = 42; + public static final String GIT_SHA = "05518ae9889665c7e51c81aa19dc6d1bd51d2fc0"; + public static final String GIT_DATE = "2025-02-17 13:05:26 EST"; public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-17 13:02:14 EST"; - public static final long BUILD_UNIX_TIME = 1739815334814L; + public static final String BUILD_DATE = "2025-02-17 14:14:21 EST"; + public static final long BUILD_UNIX_TIME = 1739819661073L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitSubsystems.java b/src/main/java/frc/robot/InitSubsystems.java index 9430d431..6788d6f1 100644 --- a/src/main/java/frc/robot/InitSubsystems.java +++ b/src/main/java/frc/robot/InitSubsystems.java @@ -14,6 +14,7 @@ import frc.robot.constants.JsonConstants; import frc.robot.constants.ModeConstants; import frc.robot.subsystems.climb.ClimbIOSim; +import frc.robot.subsystems.climb.ClimbIOTalonFX; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConfiguration; @@ -89,7 +90,7 @@ public static ScoringSubsystem initScoringSubsystem() { public static ClimbSubsystem initClimbSubsystem() { switch (ModeConstants.currentMode) { case REAL: - throw new UnsupportedOperationException("Climb real functions are not yet implemented."); + return new ClimbSubsystem(new ClimbIOTalonFX()); case SIM: case MAPLESIM: return new ClimbSubsystem(new ClimbIOSim()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1fece201..6ed95d8d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.commands.drive.AkitDriveCommands; import frc.robot.constants.AutoStrategy; import frc.robot.constants.AutoStrategyContainer; +import frc.robot.constants.ClimbConstants; import frc.robot.constants.FeatureFlags; import frc.robot.constants.JsonConstants; import frc.robot.constants.OperatorConstants; @@ -101,6 +102,7 @@ public void loadConstants() { JsonConstants.loadConstants(); FeatureFlags.synced.loadData(); OperatorConstants.synced.loadData(); + ClimbConstants.synced.loadData(); } public void configureAutos() { diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index f5a15aa0..bac91156 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -15,7 +15,7 @@ public enum Mode { * mode. The robot will automatically detect a real robot, so this value doesn't need to be * changed to Mode.Real when on a real robot. */ - public static final Mode simMode = Mode.MAPLESIM; + public static final Mode simMode = Mode.SIM; public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } diff --git a/src/main/java/frc/robot/constants/subsystems/ElevatorConstants.java b/src/main/java/frc/robot/constants/subsystems/ElevatorConstants.java index f3c4dc96..dac51c12 100644 --- a/src/main/java/frc/robot/constants/subsystems/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/subsystems/ElevatorConstants.java @@ -71,7 +71,7 @@ public final class ElevatorConstants { public final Double elevatorCANCoderDiscontinuityPoint = 1.0; // TODO: Tune encoder directions! - public final int elevatorLargeCANCoderID = 11; + public final Integer elevatorLargeCANCoderID = 11; public final Boolean isLargeCANcoderClockwisePositive = true; @JSONExclude @@ -80,7 +80,7 @@ public final class ElevatorConstants { ? SensorDirectionValue.CounterClockwise_Positive : SensorDirectionValue.Clockwise_Positive; - public final int elevatorSmallCANCoderID = 12; + public final Integer elevatorSmallCANCoderID = 12; public final Boolean isSmallCANcoderClockwisePositive = true; @JSONExclude From 8bf40a5df54058e991bc7db03114be88fc048955 Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Mon, 17 Feb 2025 15:38:33 -0500 Subject: [PATCH 11/31] wip need to ask nathan about 3d cad --- src/main/java/frc/robot/BuildConstants.java | 14 ++-- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 10 +++ .../frc/robot/constants/ModeConstants.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../scoring/ScoringSubsystemMapleSim.java | 71 +++++++++++++++++++ .../subsystems/scoring/states/ScoreState.java | 1 + 7 files changed, 92 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e0c5dbdc..bb0dfa12 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code"; + public static final String MAVEN_NAME = "2025-Robot-Code-2"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 42; - public static final String GIT_SHA = "05518ae9889665c7e51c81aa19dc6d1bd51d2fc0"; - public static final String GIT_DATE = "2025-02-17 13:05:26 EST"; - public static final String GIT_BRANCH = "63-add-3d-robot-to-sim"; - public static final String BUILD_DATE = "2025-02-17 14:14:21 EST"; - public static final long BUILD_UNIX_TIME = 1739819661073L; + public static final int GIT_REVISION = 43; + public static final String GIT_SHA = "a3f36b0c4c12630f6e22893270ab542e2dfc2727"; + public static final String GIT_DATE = "2025-02-17 14:22:41 EST"; + public static final String GIT_BRANCH = "maplesim-intake"; + public static final String BUILD_DATE = "2025-02-17 15:31:52 EST"; + public static final long BUILD_UNIX_TIME = 1739824312868L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index df7fc3a5..6c5b1837 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.ModeConstants; -import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -151,8 +150,7 @@ public void testPeriodic() { /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { - SimulatedArena.getInstance().resetFieldForAuto(); - // SimulatedArena.overrideInstance(new Arena2025Reefscape()); + robotContainer.resetMapleSim(); } /** This function is called periodically whilst in simulation. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ed95d8d..be29c6c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,9 +26,11 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; import java.io.File; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; import org.littletonrobotics.junction.Logger; /** @@ -252,6 +254,14 @@ public void disabledInit() { CommandScheduler.getInstance().cancelAll(); } + public void resetMapleSim() { + SimulatedArena.getInstance().resetFieldForAuto(); + ReefscapeAlgaeOnFly.setHitNetCallBack(() -> System.out.println("ALGAE hits NET!")); + + ScoringSubsystemMapleSim.configDrive(drive); + ScoringSubsystemMapleSim.configScoring(scoringSubsystem); + } + public void updateMapleSim() { SimulatedArena.getInstance().simulationPeriodic(); if (driveSim != null) { diff --git a/src/main/java/frc/robot/constants/ModeConstants.java b/src/main/java/frc/robot/constants/ModeConstants.java index bac91156..f5a15aa0 100644 --- a/src/main/java/frc/robot/constants/ModeConstants.java +++ b/src/main/java/frc/robot/constants/ModeConstants.java @@ -15,7 +15,7 @@ public enum Mode { * mode. The robot will automatically detect a real robot, so this value doesn't need to be * changed to Mode.Real when on a real robot. */ - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.MAPLESIM; public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index dac079f9..2f652351 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -778,7 +778,7 @@ private SwerveModulePosition[] getModulePositions() { /** Returns the measured chassis speeds of the robot. */ @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - private ChassisSpeeds getChassisSpeeds() { + public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java new file mode 100644 index 00000000..2e942f28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.scoring; + +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.subsystems.drive.Drive; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly; +import org.littletonrobotics.junction.Logger; + +public class ScoringSubsystemMapleSim { + + public static Drive drive; + public static ScoringSubsystem scoring; + + public static void shootAlgae() { + if (drive == null || scoring == null) { + return; + } + SimulatedArena.getInstance() + .addGamePieceProjectile( + new ReefscapeAlgaeOnFly( + drive.getPose().getTranslation(), + new Translation2d(0.5, 0), + drive.getChassisSpeeds(), + drive.getPose().getRotation(), + scoring.getElevatorHeight(), // initial height of the ball, in meters + MetersPerSecond.of(5), // initial velocity, in m/s + scoring.getWristAngle()) // shooter angle + .withProjectileTrajectoryDisplayCallBack( + (poses) -> + Logger.recordOutput( + "successfulShotsTrajectory", poses.toArray(Pose3d[]::new)), + (poses) -> + Logger.recordOutput( + "missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + } + + public static void shootCoral() { + if (drive == null || scoring == null) { + return; + } + SimulatedArena.getInstance() + .addGamePieceProjectile( + new ReefscapeCoralOnFly( + drive.getPose().getTranslation(), + new Translation2d(0.5, 0), + drive.getChassisSpeeds(), + drive.getPose().getRotation(), + scoring.getElevatorHeight(), // initial height of the ball, in meters + MetersPerSecond.of(5), // initial velocity, in m/s + scoring.getWristAngle()) // shooter angle + .withProjectileTrajectoryDisplayCallBack( + (poses) -> + Logger.recordOutput( + "successfulShotsTrajectory", poses.toArray(Pose3d[]::new)), + (poses) -> + Logger.recordOutput( + "missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + } + + public static void configDrive(Drive driveSet) { + drive = driveSet; + } + + public static void configScoring(ScoringSubsystem scoringSet) { + scoring = scoringSet; + } +} diff --git a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java index 94ad4b32..c924bcb7 100644 --- a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java +++ b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java @@ -23,6 +23,7 @@ public void periodic() { switch (scoringSubsystem.getGamePiece()) { case Coral: scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + if (!scoringSubsystem.isCoralDetected()) { scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); } From 998fd5cde79e4fd67bd15c06b67b6746d6af0e3e Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Wed, 26 Feb 2025 19:37:56 -0500 Subject: [PATCH 12/31] can score in trough --- src/main/deploy/constants/comp/FeatureFlags.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/InitBindings.java | 8 +++++++- src/main/java/frc/robot/RobotContainer.java | 1 + .../subsystems/scoring/ScoringSubsystemMapleSim.java | 9 +++++++-- 5 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/constants/comp/FeatureFlags.json b/src/main/deploy/constants/comp/FeatureFlags.json index 99498676..ff6bb68b 100644 --- a/src/main/deploy/constants/comp/FeatureFlags.json +++ b/src/main/deploy/constants/comp/FeatureFlags.json @@ -1,7 +1,7 @@ { "runDrive": true, "runVision": true, - "runScoring": false, + "runScoring": true, "runClimb": true, "runRamp": true } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bb0dfa12..76f23182 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code-2"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 43; - public static final String GIT_SHA = "a3f36b0c4c12630f6e22893270ab542e2dfc2727"; - public static final String GIT_DATE = "2025-02-17 14:22:41 EST"; + public static final int GIT_REVISION = 44; + public static final String GIT_SHA = "8bf40a5df54058e991bc7db03114be88fc048955"; + public static final String GIT_DATE = "2025-02-17 15:38:33 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-17 15:31:52 EST"; - public static final long BUILD_UNIX_TIME = 1739824312868L; + public static final String BUILD_DATE = "2025-02-26 19:36:06 EST"; + public static final long BUILD_UNIX_TIME = 1740616566101L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 6bad5a02..bc70d446 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.drive.Drive.DriveTrigger; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public final class InitBindings { // Controller @@ -139,7 +140,7 @@ public static void initRampBindings(RampSubsystem rampSubsystem) { rampSubsystem.prepareForClimb(); })); driverController - .y() + .b() .onTrue( new InstantCommand( () -> { @@ -152,6 +153,11 @@ public static void initClimbBindings(ClimbSubsystem climb) { driverController.b().onTrue(new InstantCommand(() -> climb.fireTrigger(ClimbAction.CANCEL))); } + public static void initScoringBindingsMaple() { + driverController.x().onTrue(new InstantCommand(() -> ScoringSubsystemMapleSim.shootAlgae())); + driverController.y().onTrue(new InstantCommand(() -> ScoringSubsystemMapleSim.shootCoral())); + } + /** * Initialize bindings specifically for Test Mode. * diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be29c6c4..f5397187 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -260,6 +260,7 @@ public void resetMapleSim() { ScoringSubsystemMapleSim.configDrive(drive); ScoringSubsystemMapleSim.configScoring(scoringSubsystem); + InitBindings.initScoringBindingsMaple(); } public void updateMapleSim() { diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index 2e942f28..9d779294 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.scoring; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.math.geometry.Pose3d; @@ -26,7 +27,9 @@ public static void shootAlgae() { new Translation2d(0.5, 0), drive.getChassisSpeeds(), drive.getPose().getRotation(), - scoring.getElevatorHeight(), // initial height of the ball, in meters + Meters.of( + scoring.getElevatorHeight().in(Meters) + + 0.5), // initial height of the ball, in meters MetersPerSecond.of(5), // initial velocity, in m/s scoring.getWristAngle()) // shooter angle .withProjectileTrajectoryDisplayCallBack( @@ -49,7 +52,9 @@ public static void shootCoral() { new Translation2d(0.5, 0), drive.getChassisSpeeds(), drive.getPose().getRotation(), - scoring.getElevatorHeight(), // initial height of the ball, in meters + Meters.of( + scoring.getElevatorHeight().in(Meters) + + 0.5), // initial height of the ball, in meters MetersPerSecond.of(5), // initial velocity, in m/s scoring.getWristAngle()) // shooter angle .withProjectileTrajectoryDisplayCallBack( From 0633ea4793828dcd3182e643e84150734ef996ed Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Wed, 26 Feb 2025 19:43:36 -0500 Subject: [PATCH 13/31] merge --- src/main/java/frc/robot/InitBindings.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 9442cb2d..acfe8043 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -21,13 +21,10 @@ import frc.robot.subsystems.drive.Drive.DriveTrigger; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; -<<<<<<< HEAD import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; -======= import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import frc.robot.subsystems.scoring.ScoringSubsystem.GamePiece; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; ->>>>>>> main public final class InitBindings { // Controller From cbae08dcec5b655bd16b1c4e6d1ffce6bb023ccd Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Wed, 26 Feb 2025 19:52:01 -0500 Subject: [PATCH 14/31] fixing formatting --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/InitBindings.java | 7 +++---- src/main/java/frc/robot/RobotContainer.java | 3 --- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 1d2b206d..a5821979 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code-2"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 45; - public static final String GIT_SHA = "64ec97cccc80dc01bcb489f4b297ad07118bd339"; - public static final String GIT_DATE = "2025-02-22 16:07:15 EST"; - public static final String GIT_BRANCH = "snakescreen-integration"; - public static final String BUILD_DATE = "2025-02-22 16:51:02 EST"; - public static final long BUILD_UNIX_TIME = 1740261062158L; + public static final int GIT_REVISION = 57; + public static final String GIT_SHA = "0633ea4793828dcd3182e643e84150734ef996ed"; + public static final String GIT_DATE = "2025-02-26 19:43:36 EST"; + public static final String GIT_BRANCH = "maplesim-intake"; + public static final String BUILD_DATE = "2025-02-26 19:51:42 EST"; + public static final long BUILD_UNIX_TIME = 1740617502764L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index acfe8043..85469525 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -21,10 +21,10 @@ import frc.robot.subsystems.drive.Drive.DriveTrigger; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; -import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import frc.robot.subsystems.scoring.ScoringSubsystem.GamePiece; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public final class InitBindings { // Controller @@ -164,9 +164,6 @@ public static void initClimbBindings(ClimbSubsystem climb) { driverController.b().onTrue(new InstantCommand(() -> climb.fireTrigger(ClimbAction.CANCEL))); } - public static void initScoringBindingsMaple() { - driverController.x().onTrue(new InstantCommand(() -> ScoringSubsystemMapleSim.shootAlgae())); - driverController.y().onTrue(new InstantCommand(() -> ScoringSubsystemMapleSim.shootCoral())); public static void initScoringBindings(ScoringSubsystem scoring) { driverController .y() @@ -176,6 +173,7 @@ public static void initScoringBindings(ScoringSubsystem scoring) { scoring.setTarget(FieldTarget.L4); scoring.setGamePiece(GamePiece.Coral); scoring.fireTrigger(ScoringTrigger.StartWarmup); + ScoringSubsystemMapleSim.shootCoral(); })); driverController .rightBumper() @@ -183,6 +181,7 @@ public static void initScoringBindings(ScoringSubsystem scoring) { new InstantCommand( () -> { scoring.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + ScoringSubsystemMapleSim.shootAlgae(); })) .onFalse( new InstantCommand( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cbbc6011..bc3e44d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,8 +1,6 @@ package frc.robot; import static edu.wpi.first.units.Units.Radians; - -import coppercore.vision.VisionLocalizer; import static edu.wpi.first.units.Units.RotationsPerSecond; import coppercore.vision.VisionLocalizer; @@ -283,7 +281,6 @@ public void resetMapleSim() { ScoringSubsystemMapleSim.configDrive(drive); ScoringSubsystemMapleSim.configScoring(scoringSubsystem); - InitBindings.initScoringBindingsMaple(); } public void updateMapleSim() { From 99cf1565635def121764acb365bc447f0ab2ed66 Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Thu, 27 Feb 2025 18:32:36 -0500 Subject: [PATCH 15/31] pushing wip --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/RobotContainer.java | 1 - 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index a5821979..1b2c31fb 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code-2"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 57; - public static final String GIT_SHA = "0633ea4793828dcd3182e643e84150734ef996ed"; - public static final String GIT_DATE = "2025-02-26 19:43:36 EST"; + public static final int GIT_REVISION = 58; + public static final String GIT_SHA = "cbae08dcec5b655bd16b1c4e6d1ffce6bb023ccd"; + public static final String GIT_DATE = "2025-02-26 19:52:01 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-26 19:51:42 EST"; - public static final long BUILD_UNIX_TIME = 1740617502764L; + public static final String BUILD_DATE = "2025-02-27 18:19:38 EST"; + public static final long BUILD_UNIX_TIME = 1740698378973L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc3e44d1..5ac9eb48 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -278,7 +278,6 @@ public void disabledInit() { public void resetMapleSim() { SimulatedArena.getInstance().resetFieldForAuto(); ReefscapeAlgaeOnFly.setHitNetCallBack(() -> System.out.println("ALGAE hits NET!")); - ScoringSubsystemMapleSim.configDrive(drive); ScoringSubsystemMapleSim.configScoring(scoringSubsystem); } From 78f6e7d88aa8aeb7d5011c839c966e2c694f9bde Mon Sep 17 00:00:00 2001 From: Sojo123456 Date: Thu, 27 Feb 2025 20:00:37 -0500 Subject: [PATCH 16/31] fixing claw to rotate to the right position, scoring on l4 and l3 and trough able --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/InitBindings.java | 5 ++--- .../subsystems/scoring/ScoringSubsystemMapleSim.java | 12 +++++------- .../robot/subsystems/scoring/states/ScoreState.java | 3 +++ 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 1b2c31fb..7e41bae1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-2"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 58; - public static final String GIT_SHA = "cbae08dcec5b655bd16b1c4e6d1ffce6bb023ccd"; - public static final String GIT_DATE = "2025-02-26 19:52:01 EST"; + public static final int GIT_REVISION = 59; + public static final String GIT_SHA = "99cf1565635def121764acb365bc447f0ab2ed66"; + public static final String GIT_DATE = "2025-02-27 18:32:36 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-27 18:19:38 EST"; - public static final long BUILD_UNIX_TIME = 1740698378973L; + public static final String BUILD_DATE = "2025-02-27 19:58:30 EST"; + public static final long BUILD_UNIX_TIME = 1740704310806L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 85469525..136a3439 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -24,7 +24,6 @@ import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import frc.robot.subsystems.scoring.ScoringSubsystem.GamePiece; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; -import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public final class InitBindings { // Controller @@ -173,15 +172,15 @@ public static void initScoringBindings(ScoringSubsystem scoring) { scoring.setTarget(FieldTarget.L4); scoring.setGamePiece(GamePiece.Coral); scoring.fireTrigger(ScoringTrigger.StartWarmup); - ScoringSubsystemMapleSim.shootCoral(); })); driverController .rightBumper() .onTrue( new InstantCommand( () -> { + scoring.setTarget(FieldTarget.Net); + scoring.setGamePiece(GamePiece.Algae); scoring.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); - ScoringSubsystemMapleSim.shootAlgae(); })) .onFalse( new InstantCommand( diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index 9d779294..92942b7a 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.scoring; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; @@ -49,21 +50,18 @@ public static void shootCoral() { .addGamePieceProjectile( new ReefscapeCoralOnFly( drive.getPose().getTranslation(), - new Translation2d(0.5, 0), + new Translation2d(0.45, 0), drive.getChassisSpeeds(), drive.getPose().getRotation(), Meters.of( scoring.getElevatorHeight().in(Meters) + 0.5), // initial height of the ball, in meters MetersPerSecond.of(5), // initial velocity, in m/s - scoring.getWristAngle()) // shooter angle + Degrees.of((scoring.getWristAngle().in(Degrees)) * -1)) // shooter angle .withProjectileTrajectoryDisplayCallBack( + (poses) -> {}, (poses) -> - Logger.recordOutput( - "successfulShotsTrajectory", poses.toArray(Pose3d[]::new)), - (poses) -> - Logger.recordOutput( - "missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + Logger.recordOutput("coralShotsTrajectory", poses.toArray(Pose3d[]::new)))); } public static void configDrive(Drive driveSet) { diff --git a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java index c924bcb7..b3dbf0aa 100644 --- a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java +++ b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java @@ -6,6 +6,7 @@ import frc.robot.constants.ScoringSetpoints.ScoringSetpoint; import frc.robot.subsystems.scoring.ScoringSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public class ScoreState implements PeriodicStateInterface { private ScoringSubsystem scoringSubsystem; @@ -26,12 +27,14 @@ public void periodic() { if (!scoringSubsystem.isCoralDetected()) { scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); + ScoringSubsystemMapleSim.shootCoral(); } break; case Algae: scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.algaeScoreVoltage); if (!scoringSubsystem.isAlgaeDetected()) { scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); + ScoringSubsystemMapleSim.shootAlgae(); } break; } From 0a9eabf5af022df360b26a5b98f544366d303bdc Mon Sep 17 00:00:00 2001 From: Sojo123456 Date: Fri, 28 Feb 2025 19:29:34 -0500 Subject: [PATCH 17/31] tried to add integration with states and intake, it is not working unfortunately I've apparently broken a lot of stuff, will fix soon --- simgui-ds.json | 3 +- src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/InitBindings.java | 11 +++ src/main/java/frc/robot/RobotContainer.java | 4 +- .../scoring/ScoringSubsystemMapleSim.java | 76 +++++++++++++++++-- .../subsystems/scoring/states/ScoreState.java | 4 +- 6 files changed, 92 insertions(+), 16 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 92d025e1..4d2db79e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -102,7 +102,8 @@ "guid": "Keyboard1" }, { - "guid": "Keyboard2" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7e41bae1..b6f9f4cc 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 59; - public static final String GIT_SHA = "99cf1565635def121764acb365bc447f0ab2ed66"; - public static final String GIT_DATE = "2025-02-27 18:32:36 EST"; + public static final int GIT_REVISION = 60; + public static final String GIT_SHA = "78f6e7d88aa8aeb7d5011c839c966e2c694f9bde"; + public static final String GIT_DATE = "2025-02-27 20:00:37 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-27 19:58:30 EST"; - public static final long BUILD_UNIX_TIME = 1740704310806L; + public static final String BUILD_DATE = "2025-02-28 19:23:50 EST"; + public static final long BUILD_UNIX_TIME = 1740788630155L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 136a3439..36886cf7 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -24,6 +24,7 @@ import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import frc.robot.subsystems.scoring.ScoringSubsystem.GamePiece; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public final class InitBindings { // Controller @@ -172,6 +173,7 @@ public static void initScoringBindings(ScoringSubsystem scoring) { scoring.setTarget(FieldTarget.L4); scoring.setGamePiece(GamePiece.Coral); scoring.fireTrigger(ScoringTrigger.StartWarmup); + ScoringSubsystemMapleSim.shootAlgae(); })); driverController .rightBumper() @@ -181,12 +183,21 @@ public static void initScoringBindings(ScoringSubsystem scoring) { scoring.setTarget(FieldTarget.Net); scoring.setGamePiece(GamePiece.Algae); scoring.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + ScoringSubsystemMapleSim.shootCoral(); })) .onFalse( new InstantCommand( () -> { scoring.setClawRollerVoltage(Volts.zero()); })); + driverController + .leftBumper() + .onTrue( + new InstantCommand( + () -> { + scoring.setTarget(FieldTarget.Ground); + scoring.setGamePiece(GamePiece.Coral); + })); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ac9eb48..d130b7b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -151,6 +151,7 @@ public void configureSubsystems() { } else { scoringSubsystem.setIsDriveLinedUpSupplier(() -> true); } + scoringSubsystem.setIsDriveLinedUpSupplier(() -> true); } strategyManager = new StrategyManager(drive, scoringSubsystem); } @@ -278,8 +279,7 @@ public void disabledInit() { public void resetMapleSim() { SimulatedArena.getInstance().resetFieldForAuto(); ReefscapeAlgaeOnFly.setHitNetCallBack(() -> System.out.println("ALGAE hits NET!")); - ScoringSubsystemMapleSim.configDrive(drive); - ScoringSubsystemMapleSim.configScoring(scoringSubsystem); + ScoringSubsystemMapleSim.config(drive, driveSim, scoringSubsystem); } public void updateMapleSim() { diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index 92942b7a..c51ac62e 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -6,16 +6,24 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.drive.Drive; +import java.util.List; +import org.ironmaple.simulation.IntakeSimulation; import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly; import org.littletonrobotics.junction.Logger; public class ScoringSubsystemMapleSim { - public static Drive drive; - public static ScoringSubsystem scoring; + private static Drive drive; + private static ScoringSubsystem scoring; + private static SwerveDriveSimulation driveSim; + + private static IntakeSimulation coralIntakeSimulation; + private static IntakeSimulation algaeIntakeSimulation; public static void shootAlgae() { if (drive == null || scoring == null) { @@ -40,6 +48,8 @@ public static void shootAlgae() { (poses) -> Logger.recordOutput( "missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + // SmartDashboard.putBoolean("clawSim/algaeAvailable", false); + // SmartDashboard.getBoolean("clawSim/algaeAvailable", false); } public static void shootCoral() { @@ -62,13 +72,67 @@ public static void shootCoral() { (poses) -> {}, (poses) -> Logger.recordOutput("coralShotsTrajectory", poses.toArray(Pose3d[]::new)))); + // SmartDashboard.putBoolean("clawSim/coralAvailable", false); + // SmartDashboard.getBoolean("clawSim/coralAvailable", false); } - public static void configDrive(Drive driveSet) { - drive = driveSet; + public static void intakeCoral() { + if (scoring.isCoralDetected() == false) { + List corals = SimulatedArena.getInstance().getGamePiecesByType("Coral"); + int indexOfMinHeightIntake; + double distanceOfMinHeightIntake = Integer.MAX_VALUE; + + for (int coralIndex = 0; coralIndex < corals.size(); coralIndex++) { + double heightDistance = + Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - corals.get(coralIndex).getZ()); + if (distanceOfMinHeightIntake > heightDistance) { + indexOfMinHeightIntake = coralIndex; + distanceOfMinHeightIntake = heightDistance; + } + } + + if (distanceOfMinHeightIntake < 0.1) { + coralIntakeSimulation.startIntake(); + SmartDashboard.putBoolean("clawSim/coralAvailable", true); + coralIntakeSimulation.stopIntake(); + } + } } - public static void configScoring(ScoringSubsystem scoringSet) { - scoring = scoringSet; + public static void intakeAlgae() { + if (scoring.isCoralDetected() == false) { + List algae = SimulatedArena.getInstance().getGamePiecesByType("Algae"); + int indexOfMinHeightIntake; + double distanceOfMinHeightIntake = Integer.MAX_VALUE; + + for (int algaeIndex = 0; algaeIndex < algae.size(); algaeIndex++) { + double heightDistance = + Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - algae.get(algaeIndex).getZ()); + if (distanceOfMinHeightIntake > heightDistance) { + indexOfMinHeightIntake = algaeIndex; + distanceOfMinHeightIntake = heightDistance; + } + } + + if (distanceOfMinHeightIntake < 0.1) { + algaeIntakeSimulation.startIntake(); + SmartDashboard.putBoolean("clawSim/algaeAvailable", true); + algaeIntakeSimulation.stopIntake(); + } + } + } + + public static void config( + Drive driveSet, SwerveDriveSimulation driveSimSet, ScoringSubsystem scoringSet) { + drive = driveSet; + driveSim = driveSimSet; + + coralIntakeSimulation = + IntakeSimulation.InTheFrameIntake( + "Coral", driveSim, Meters.of(0.2), IntakeSimulation.IntakeSide.FRONT, 1); + + algaeIntakeSimulation = + IntakeSimulation.InTheFrameIntake( + "Algae", driveSim, Meters.of(0.2), IntakeSimulation.IntakeSide.FRONT, 1); } } diff --git a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java index b3dbf0aa..875cbafb 100644 --- a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java +++ b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java @@ -24,17 +24,17 @@ public void periodic() { switch (scoringSubsystem.getGamePiece()) { case Coral: scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + ScoringSubsystemMapleSim.shootCoral(); if (!scoringSubsystem.isCoralDetected()) { scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); - ScoringSubsystemMapleSim.shootCoral(); } break; case Algae: scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.algaeScoreVoltage); + ScoringSubsystemMapleSim.shootAlgae(); if (!scoringSubsystem.isAlgaeDetected()) { scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); - ScoringSubsystemMapleSim.shootAlgae(); } break; } From a6486ac4824f0bda5580738e3cc5f6697fd32acb Mon Sep 17 00:00:00 2001 From: Sojo123456 Date: Fri, 28 Feb 2025 19:52:18 -0500 Subject: [PATCH 18/31] state machine and climb rotating into place is a little sus but pieces are being scored again --- src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/InitBindings.java | 3 - src/main/java/frc/robot/RobotContainer.java | 3 +- .../scoring/ScoringSubsystemMapleSim.java | 66 ++----------------- .../subsystems/scoring/states/ScoreState.java | 4 +- 5 files changed, 16 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index b6f9f4cc..379bf44e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 60; - public static final String GIT_SHA = "78f6e7d88aa8aeb7d5011c839c966e2c694f9bde"; - public static final String GIT_DATE = "2025-02-27 20:00:37 EST"; + public static final int GIT_REVISION = 61; + public static final String GIT_SHA = "0a9eabf5af022df360b26a5b98f544366d303bdc"; + public static final String GIT_DATE = "2025-02-28 19:29:34 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-28 19:23:50 EST"; - public static final long BUILD_UNIX_TIME = 1740788630155L; + public static final String BUILD_DATE = "2025-02-28 19:49:09 EST"; + public static final long BUILD_UNIX_TIME = 1740790149754L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 36886cf7..f54853eb 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -24,7 +24,6 @@ import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import frc.robot.subsystems.scoring.ScoringSubsystem.GamePiece; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; -import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public final class InitBindings { // Controller @@ -173,7 +172,6 @@ public static void initScoringBindings(ScoringSubsystem scoring) { scoring.setTarget(FieldTarget.L4); scoring.setGamePiece(GamePiece.Coral); scoring.fireTrigger(ScoringTrigger.StartWarmup); - ScoringSubsystemMapleSim.shootAlgae(); })); driverController .rightBumper() @@ -183,7 +181,6 @@ public static void initScoringBindings(ScoringSubsystem scoring) { scoring.setTarget(FieldTarget.Net); scoring.setGamePiece(GamePiece.Algae); scoring.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); - ScoringSubsystemMapleSim.shootCoral(); })) .onFalse( new InstantCommand( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d130b7b4..677381e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -279,7 +279,8 @@ public void disabledInit() { public void resetMapleSim() { SimulatedArena.getInstance().resetFieldForAuto(); ReefscapeAlgaeOnFly.setHitNetCallBack(() -> System.out.println("ALGAE hits NET!")); - ScoringSubsystemMapleSim.config(drive, driveSim, scoringSubsystem); + ScoringSubsystemMapleSim.configDrive(drive, driveSim); + ScoringSubsystemMapleSim.configScoring(scoringSubsystem); } public void updateMapleSim() { diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index c51ac62e..8122aa74 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -6,9 +6,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.drive.Drive; -import java.util.List; import org.ironmaple.simulation.IntakeSimulation; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; @@ -18,9 +16,8 @@ public class ScoringSubsystemMapleSim { - private static Drive drive; - private static ScoringSubsystem scoring; - private static SwerveDriveSimulation driveSim; + public static Drive drive; + public static ScoringSubsystem scoring; private static IntakeSimulation coralIntakeSimulation; private static IntakeSimulation algaeIntakeSimulation; @@ -48,8 +45,6 @@ public static void shootAlgae() { (poses) -> Logger.recordOutput( "missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); - // SmartDashboard.putBoolean("clawSim/algaeAvailable", false); - // SmartDashboard.getBoolean("clawSim/algaeAvailable", false); } public static void shootCoral() { @@ -72,61 +67,10 @@ public static void shootCoral() { (poses) -> {}, (poses) -> Logger.recordOutput("coralShotsTrajectory", poses.toArray(Pose3d[]::new)))); - // SmartDashboard.putBoolean("clawSim/coralAvailable", false); - // SmartDashboard.getBoolean("clawSim/coralAvailable", false); } - public static void intakeCoral() { - if (scoring.isCoralDetected() == false) { - List corals = SimulatedArena.getInstance().getGamePiecesByType("Coral"); - int indexOfMinHeightIntake; - double distanceOfMinHeightIntake = Integer.MAX_VALUE; - - for (int coralIndex = 0; coralIndex < corals.size(); coralIndex++) { - double heightDistance = - Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - corals.get(coralIndex).getZ()); - if (distanceOfMinHeightIntake > heightDistance) { - indexOfMinHeightIntake = coralIndex; - distanceOfMinHeightIntake = heightDistance; - } - } - - if (distanceOfMinHeightIntake < 0.1) { - coralIntakeSimulation.startIntake(); - SmartDashboard.putBoolean("clawSim/coralAvailable", true); - coralIntakeSimulation.stopIntake(); - } - } - } - - public static void intakeAlgae() { - if (scoring.isCoralDetected() == false) { - List algae = SimulatedArena.getInstance().getGamePiecesByType("Algae"); - int indexOfMinHeightIntake; - double distanceOfMinHeightIntake = Integer.MAX_VALUE; - - for (int algaeIndex = 0; algaeIndex < algae.size(); algaeIndex++) { - double heightDistance = - Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - algae.get(algaeIndex).getZ()); - if (distanceOfMinHeightIntake > heightDistance) { - indexOfMinHeightIntake = algaeIndex; - distanceOfMinHeightIntake = heightDistance; - } - } - - if (distanceOfMinHeightIntake < 0.1) { - algaeIntakeSimulation.startIntake(); - SmartDashboard.putBoolean("clawSim/algaeAvailable", true); - algaeIntakeSimulation.stopIntake(); - } - } - } - - public static void config( - Drive driveSet, SwerveDriveSimulation driveSimSet, ScoringSubsystem scoringSet) { + public static void configDrive(Drive driveSet, SwerveDriveSimulation driveSim) { drive = driveSet; - driveSim = driveSimSet; - coralIntakeSimulation = IntakeSimulation.InTheFrameIntake( "Coral", driveSim, Meters.of(0.2), IntakeSimulation.IntakeSide.FRONT, 1); @@ -135,4 +79,8 @@ public static void config( IntakeSimulation.InTheFrameIntake( "Algae", driveSim, Meters.of(0.2), IntakeSimulation.IntakeSide.FRONT, 1); } + + public static void configScoring(ScoringSubsystem scoringSet) { + scoring = scoringSet; + } } diff --git a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java index 875cbafb..5859447e 100644 --- a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java +++ b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java @@ -24,16 +24,16 @@ public void periodic() { switch (scoringSubsystem.getGamePiece()) { case Coral: scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); - ScoringSubsystemMapleSim.shootCoral(); if (!scoringSubsystem.isCoralDetected()) { + ScoringSubsystemMapleSim.shootCoral(); scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); } break; case Algae: scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.algaeScoreVoltage); - ScoringSubsystemMapleSim.shootAlgae(); if (!scoringSubsystem.isAlgaeDetected()) { + ScoringSubsystemMapleSim.shootAlgae(); scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); } break; From 77da6e9c485e77439690e47e86740b5ea086ebc9 Mon Sep 17 00:00:00 2001 From: Sojo123456 Date: Fri, 28 Feb 2025 19:54:39 -0500 Subject: [PATCH 19/31] replaced intake methods, intake still not verified (how does claw subsystem manage hasCoral/hasAlgae?) --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- .../scoring/ScoringSubsystemMapleSim.java | 48 +++++++++++++++++++ 2 files changed, 53 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 379bf44e..b913f374 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 61; - public static final String GIT_SHA = "0a9eabf5af022df360b26a5b98f544366d303bdc"; - public static final String GIT_DATE = "2025-02-28 19:29:34 EST"; + public static final int GIT_REVISION = 62; + public static final String GIT_SHA = "a6486ac4824f0bda5580738e3cc5f6697fd32acb"; + public static final String GIT_DATE = "2025-02-28 19:52:18 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-28 19:49:09 EST"; - public static final long BUILD_UNIX_TIME = 1740790149754L; + public static final String BUILD_DATE = "2025-02-28 19:53:05 EST"; + public static final long BUILD_UNIX_TIME = 1740790385509L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index 8122aa74..618d8588 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -6,7 +6,9 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.drive.Drive; +import java.util.List; import org.ironmaple.simulation.IntakeSimulation; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; @@ -69,6 +71,52 @@ public static void shootCoral() { Logger.recordOutput("coralShotsTrajectory", poses.toArray(Pose3d[]::new)))); } + public static void intakeCoral() { + if (scoring.isCoralDetected() == false) { + List corals = SimulatedArena.getInstance().getGamePiecesByType("Coral"); + int indexOfMinHeightIntake; + double distanceOfMinHeightIntake = Integer.MAX_VALUE; + + for (int coralIndex = 0; coralIndex < corals.size(); coralIndex++) { + double heightDistance = + Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - corals.get(coralIndex).getZ()); + if (distanceOfMinHeightIntake > heightDistance) { + indexOfMinHeightIntake = coralIndex; + distanceOfMinHeightIntake = heightDistance; + } + } + + if (distanceOfMinHeightIntake < 0.1) { + coralIntakeSimulation.startIntake(); + SmartDashboard.putBoolean("clawSim/coralAvailable", true); + coralIntakeSimulation.stopIntake(); + } + } + } + + public static void intakeAlgae() { + if (scoring.isCoralDetected() == false) { + List algae = SimulatedArena.getInstance().getGamePiecesByType("Algae"); + int indexOfMinHeightIntake; + double distanceOfMinHeightIntake = Integer.MAX_VALUE; + + for (int algaeIndex = 0; algaeIndex < algae.size(); algaeIndex++) { + double heightDistance = + Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - algae.get(algaeIndex).getZ()); + if (distanceOfMinHeightIntake > heightDistance) { + indexOfMinHeightIntake = algaeIndex; + distanceOfMinHeightIntake = heightDistance; + } + } + + if (distanceOfMinHeightIntake < 0.1) { + algaeIntakeSimulation.startIntake(); + SmartDashboard.putBoolean("clawSim/algaeAvailable", true); + algaeIntakeSimulation.stopIntake(); + } + } + } + public static void configDrive(Drive driveSet, SwerveDriveSimulation driveSim) { drive = driveSet; coralIntakeSimulation = From bb675603c5a05f83ae416fe6ec23f9a95e07daa7 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Mon, 3 Mar 2025 16:03:21 -0800 Subject: [PATCH 20/31] saving --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../subsystems/scoring/ScoringSubsystemMapleSim.java | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index b913f374..7805f0a0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 62; - public static final String GIT_SHA = "a6486ac4824f0bda5580738e3cc5f6697fd32acb"; - public static final String GIT_DATE = "2025-02-28 19:52:18 EST"; + public static final int GIT_REVISION = 63; + public static final String GIT_SHA = "77da6e9c485e77439690e47e86740b5ea086ebc9"; + public static final String GIT_DATE = "2025-02-28 19:54:39 EST"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-02-28 19:53:05 EST"; - public static final long BUILD_UNIX_TIME = 1740790385509L; + public static final String BUILD_DATE = "2025-03-03 18:57:53 EST"; + public static final long BUILD_UNIX_TIME = 1741046273024L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index 618d8588..6e93f558 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -63,7 +63,7 @@ public static void shootCoral() { Meters.of( scoring.getElevatorHeight().in(Meters) + 0.5), // initial height of the ball, in meters - MetersPerSecond.of(5), // initial velocity, in m/s + MetersPerSecond.of(4), // initial velocity, in m/s Degrees.of((scoring.getWristAngle().in(Degrees)) * -1)) // shooter angle .withProjectileTrajectoryDisplayCallBack( (poses) -> {}, From eff94da2d82342adc0b7ab1866ccf1d48262190d Mon Sep 17 00:00:00 2001 From: sleepyghost <145573597+sleepyghost-zzz@users.noreply.github.com> Date: Sat, 8 Mar 2025 13:50:28 -0500 Subject: [PATCH 21/31] adds comp leds (#166) * better rainbow and correct orientation * comp leds --- src/main/java/frc/robot/BuildConstants.java | 14 +++--- .../frc/robot/constants/LEDConstants.java | 7 +-- .../java/frc/robot/subsystems/led/LED.java | 43 ++++++++++++++----- 3 files changed, 43 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f52ddd56..6621dd5b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-3"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 68; - public static final String GIT_SHA = "9899bcb2854cc592c52ca78edd1259dc08feb736"; - public static final String GIT_DATE = "2025-03-08 09:57:29 EST"; - public static final String GIT_BRANCH = "156-ramp-work"; - public static final String BUILD_DATE = "2025-03-08 10:03:36 EST"; - public static final long BUILD_UNIX_TIME = 1741446216963L; + public static final int GIT_REVISION = 61; + public static final String GIT_SHA = "491a6bfbac68bf32a0aaa0cf501a2df6cc6a8ade"; + public static final String GIT_DATE = "2025-03-08 12:07:24 EST"; + public static final String GIT_BRANCH = "165-adds-better-leds"; + public static final String BUILD_DATE = "2025-03-08 12:45:58 EST"; + public static final long BUILD_UNIX_TIME = 1741455958418L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 3c4a4ce9..fb79e700 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -13,9 +13,10 @@ public final class LEDConstants { public static final int ledPort = 9; - public static final int halfLength = 14; // alternate number to test = 504 - public static final int totalLength = 28; // alternate number to test = 1008 - public static final double rainbowSpeed = 50; + public static final int halfLength = 14; + public static final int totalLength = 28; + public static final int totalLengthRainbow = 50; + public static final double rainbowSpeed = 20; public static final Color off = Color.kBlack; // LED Patterns diff --git a/src/main/java/frc/robot/subsystems/led/LED.java b/src/main/java/frc/robot/subsystems/led/LED.java index 1f0e1c8e..9d404a93 100644 --- a/src/main/java/frc/robot/subsystems/led/LED.java +++ b/src/main/java/frc/robot/subsystems/led/LED.java @@ -24,11 +24,13 @@ public class LED extends SubsystemBase { private List rightPatterns = new ArrayList<>(); private final AddressableLED led = new AddressableLED(LEDConstants.ledPort); - public final AddressableLEDBuffer ledStrip = new AddressableLEDBuffer(LEDConstants.totalLength); + public AddressableLEDBuffer ledStrip = new AddressableLEDBuffer(LEDConstants.totalLengthRainbow); - public final AddressableLEDBufferView leftData = - ledStrip.createView(0, LEDConstants.halfLength - 1); - private final AddressableLEDBufferView rightData = + public AddressableLEDBufferView leftData = + ledStrip + .createView(0, LEDConstants.halfLength - 1) + .reversed(); // these should both be reversed + private AddressableLEDBufferView rightData = ledStrip.createView(LEDConstants.halfLength, LEDConstants.totalLength - 1).reversed(); private Supplier visionWorkingSupplier = () -> true; @@ -37,6 +39,8 @@ public class LED extends SubsystemBase { private ClimbSubsystem climbSubsystem; private Drive driveSubsystem; + private boolean enabled = false; + /** * Constructs the LED subsystem. * @@ -48,14 +52,30 @@ public LED(ScoringSubsystem scoringSubsystem, ClimbSubsystem climbSubsystem, Dri this.scoringSubsystem = scoringSubsystem; this.climbSubsystem = climbSubsystem; this.driveSubsystem = drive; - led.setLength(LEDConstants.totalLength); + led.setLength(LEDConstants.totalLengthRainbow); led.start(); } + private void setEnabled(boolean enabled) { + if (enabled != this.enabled) { + this.enabled = enabled; + if (enabled) { + ledStrip = new AddressableLEDBuffer(LEDConstants.totalLength); + leftData = ledStrip.createView(0, LEDConstants.halfLength - 1); + rightData = + ledStrip.createView(LEDConstants.halfLength, LEDConstants.totalLength - 1).reversed(); + led.setLength(LEDConstants.totalLength); + } else { + ledStrip = new AddressableLEDBuffer(LEDConstants.totalLengthRainbow); + led.setLength(LEDConstants.totalLengthRainbow); + } + } + } + /** Periodic method called every loop cycle. Updates LED patterns based on robot state. */ @Override public void periodic() { - + setEnabled(DriverStation.isEnabled()); if (!DriverStation.isDisabled()) { // Drive Subsystem Checks @@ -111,11 +131,12 @@ public void periodic() { // } else { // rainbow - all thirds - addPattern(LEDConstants.rainbowPattern); - if (!visionWorkingSupplier.get()) { - addPattern(LEDConstants.isBeeLinkWorkingPattern); - } - applyPatterns(); + // addPattern(LEDConstants.rainbowPattern); + // if (!visionWorkingSupplier.get()) { + // addPattern(LEDConstants.isBeeLinkWorkingPattern); + // } + // applyPatterns(); + LEDConstants.rainbowPattern.applyTo(ledStrip); } led.setData(ledStrip); } From bedbf67634050658d9a580c7ab8f0443aa7e1c3b Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Sat, 8 Mar 2025 14:04:41 -0500 Subject: [PATCH 22/31] Morning setpoint tuning --- src/main/deploy/constants/ScoringSetpoints.json | 4 ++-- src/main/deploy/constants/comp/ClawConstants.json | 2 +- src/main/java/frc/robot/InitBindings.java | 11 +++++++++++ src/main/java/frc/robot/RobotContainer.java | 7 +++++++ 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/constants/ScoringSetpoints.json b/src/main/deploy/constants/ScoringSetpoints.json index 9f856c38..b9e31509 100644 --- a/src/main/deploy/constants/ScoringSetpoints.json +++ b/src/main/deploy/constants/ScoringSetpoints.json @@ -35,7 +35,7 @@ "L2": { "name": "L2", "elevatorHeight": { - "value": 0.25, + "value": 0.29, "unit": "Meter" }, "wristAngle": { @@ -112,7 +112,7 @@ "processor": { "name": "processor", "elevatorHeight": { - "value": 0.2, + "value": 0.18, "unit": "Meter" }, "wristAngle": { diff --git a/src/main/deploy/constants/comp/ClawConstants.json b/src/main/deploy/constants/comp/ClawConstants.json index 2b6132bc..95870dc0 100644 --- a/src/main/deploy/constants/comp/ClawConstants.json +++ b/src/main/deploy/constants/comp/ClawConstants.json @@ -38,7 +38,7 @@ "unit": "Volt" }, "coralScoreVoltage": { - "value": 12.0, + "value": 10.0, "unit": "Volt" }, "algaeScoreVoltage": { diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 0adbad9c..e235d757 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -298,6 +298,17 @@ public static void initScoringBindings(ScoringSubsystem scoring) { () -> { scoring.setClawRollerVoltage(Volts.zero()); })); + + rightJoystick + .top() + .onTrue( + new InstantCommand( + () -> { + if (ScoringSubsystem.getInstance().getGamePiece() == GamePiece.Algae + && ScoringSubsystem.getInstance().getTarget() == FieldTarget.Processor) { + ScoringSubsystem.getInstance().fireTrigger(ScoringTrigger.WarmupReady); + } + })); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 934f1dad..81340426 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -378,10 +378,17 @@ public void disabledPeriodic() { strategyManager.logActions(); checkSwitchForDisabled(); checkLedSwitch(); + + Logger.recordOutput("Switches/brake", brakeSwitch.get()); + Logger.recordOutput("Switches/led", ledSwitch.get()); } public void disabledInit() { CommandScheduler.getInstance().cancelAll(); + + if (FeatureFlags.synced.getObject().runClimb) { + climbSubsystem.setBrakeMode(true); + } } public void updateMapleSim() { From 94d449f009665caad10078e274ee34eea04f1140 Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Sat, 8 Mar 2025 17:34:13 -0500 Subject: [PATCH 23/31] Add force score to mixed auto and other comp changes --- src/main/deploy/constants/comp/ClawConstants.json | 8 ++++++-- src/main/deploy/constants/comp/ClimbConstants.json | 3 ++- .../deploy/constants/comp/ElevatorConstants.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 14 +++++++------- src/main/java/frc/robot/RobotContainer.java | 13 ++++++++----- .../java/frc/robot/constants/ClawConstants.java | 1 + .../java/frc/robot/constants/ClimbConstants.java | 2 ++ .../java/frc/robot/subsystems/climb/ClimbIO.java | 2 ++ .../frc/robot/subsystems/climb/ClimbIOTalonFX.java | 9 ++++++++- .../frc/robot/subsystems/climb/ClimbSubsystem.java | 4 ++++ .../robot/subsystems/climb/states/IdleState.java | 1 + .../subsystems/climb/states/LiftingState.java | 2 ++ .../subsystems/climb/states/SearchingState.java | 1 + .../subsystems/climb/states/WaitingState.java | 1 + .../subsystems/scoring/states/ScoreState.java | 7 ++++++- 15 files changed, 52 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/constants/comp/ClawConstants.json b/src/main/deploy/constants/comp/ClawConstants.json index 95870dc0..55930abc 100644 --- a/src/main/deploy/constants/comp/ClawConstants.json +++ b/src/main/deploy/constants/comp/ClawConstants.json @@ -21,8 +21,8 @@ }, "coralMinSignalStrengthForValidMeasurement": 2500.0, "algaeProximityThreshold": { - "value": 5.0, - "unit": "Centimeter" + "value": 2.0, + "unit": "Inch" }, "algaeProximityHysteresis": { "value": 0.5, @@ -41,6 +41,10 @@ "value": 10.0, "unit": "Volt" }, + "coralL23ScoreVoltage": { + "value": 4.0, + "unit": "Volt" + }, "algaeScoreVoltage": { "value": 12.0, "unit": "Volt" diff --git a/src/main/deploy/constants/comp/ClimbConstants.json b/src/main/deploy/constants/comp/ClimbConstants.json index 44c6c32f..4e4ac128 100644 --- a/src/main/deploy/constants/comp/ClimbConstants.json +++ b/src/main/deploy/constants/comp/ClimbConstants.json @@ -17,5 +17,6 @@ "climbkI": 0.0, "climbkD": 0.0, "climbCurrentLimit": 60, - "invertFollowerClimbMotor": true + "invertFollowerClimbMotor": true, + "climbFFVolts": -10.0 } diff --git a/src/main/deploy/constants/comp/ElevatorConstants.json b/src/main/deploy/constants/comp/ElevatorConstants.json index fb18ec87..d236382d 100644 --- a/src/main/deploy/constants/comp/ElevatorConstants.json +++ b/src/main/deploy/constants/comp/ElevatorConstants.json @@ -23,7 +23,7 @@ "elevatorLargeCANCoderDirection": "Clockwise_Positive", "elevatorSmallCANCoderID": 15, "elevatorSmallCANCoderDirection": "CounterClockwise_Positive", - "elevatorkP": 40.0, + "elevatorkP": 50.0, "elevatorkI": 0.0, "elevatorkD": 4.0, "elevatorkS": 0.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6621dd5b..320483c3 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code"; + public static final String MAVEN_NAME = "2025-Robot-Code-3"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 61; - public static final String GIT_SHA = "491a6bfbac68bf32a0aaa0cf501a2df6cc6a8ade"; - public static final String GIT_DATE = "2025-03-08 12:07:24 EST"; - public static final String GIT_BRANCH = "165-adds-better-leds"; - public static final String BUILD_DATE = "2025-03-08 12:45:58 EST"; - public static final long BUILD_UNIX_TIME = 1741455958418L; + public static final int GIT_REVISION = 62; + public static final String GIT_SHA = "bedbf67634050658d9a580c7ab8f0443aa7e1c3b"; + public static final String GIT_DATE = "2025-03-08 14:04:41 EST"; + public static final String GIT_BRANCH = "comp/VAPOR"; + public static final String BUILD_DATE = "2025-03-08 17:23:44 EST"; + public static final long BUILD_UNIX_TIME = 1741472624107L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 81340426..5098c72b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.StrategyManager.AutonomyMode; import frc.robot.commands.drive.AkitDriveCommands; import frc.robot.constants.AutoStrategy; import frc.robot.constants.AutoStrategyContainer; @@ -207,10 +206,14 @@ public void configureSubsystems() { if (FeatureFlags.synced.getObject().runDrive) { scoringSubsystem.setIsDriveLinedUpSupplier( () -> { - if (strategyManager.getAutonomyMode() != AutonomyMode.Manual) { - return drive.isDriveAlignmentFinished(); - } else { - return InitBindings.isManualScorePressed(); + switch (strategyManager.getAutonomyMode()) { + case Manual: + return InitBindings.isManualScorePressed(); + case Mixed: + return drive.isDriveAlignmentFinished() || InitBindings.isManualScorePressed(); + case Full: + default: + return drive.isDriveAlignmentFinished(); } }); scoringSubsystem.setReefDistanceSupplier( diff --git a/src/main/java/frc/robot/constants/ClawConstants.java b/src/main/java/frc/robot/constants/ClawConstants.java index 6db4943e..f0e84262 100644 --- a/src/main/java/frc/robot/constants/ClawConstants.java +++ b/src/main/java/frc/robot/constants/ClawConstants.java @@ -48,6 +48,7 @@ public class ClawConstants { public final Voltage coralIntakeVoltage = Volts.of(3.0); public final Voltage algaeIntakeVoltage = Volts.of(-3.0); public final Voltage coralScoreVoltage = Volts.of(3.0); + public final Voltage coralL23ScoreVoltage = Volts.of(6.0); public final Voltage algaeScoreVoltage = Volts.of(-3.0); /** Voltage the claw should use to hold the algae in place while idling */ diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 861e6802..584626b5 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -40,6 +40,8 @@ public class ClimbConstants { public final InvertedValue climbInvertValue = InvertedValue.Clockwise_Positive; public final Boolean invertFollowerClimbMotor = true; + public final Double climbFFVolts = -10.0; + public static final class Sim { @JSONExclude public static final JSONSync synced = diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java index 7fd3b3bf..72b96532 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -36,5 +36,7 @@ public default void setBrakeMode(boolean brake) {} public default void setPID(double p, double i, double d) {} + public default void setFF(double newFF) {} + public default void setFF(double kS, double kV, double kA, double kG) {} } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java index 5c7a0176..92389a81 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java @@ -96,10 +96,16 @@ public void updateInputs(ClimbInputs inputs) { inputs.motorAngle.mut_replace(climbAngleCoder.getAbsolutePosition().getValue()); } + private double feedforward = 0.0; + + public void setFF(double newFF) { + this.feedforward = newFF; + } + @Override public void applyOutputs(ClimbOutputs outputs) { - calculator.withPosition(goalAngle.in(Rotations)); + calculator.withPosition(goalAngle.in(Rotations)).withFeedForward(feedforward); Logger.recordOutput("climb/calculatorAngle", leadMotor.getPosition().getValueAsDouble()); @@ -108,6 +114,7 @@ public void applyOutputs(ClimbOutputs outputs) { } else { leadMotor.setControl(calculator); } + // tried to replace the above if statement with leadMotor.setControl(new VoltageOut(5));, // doesn't work // the getMotorVoltage continuously returns 0 despite leadMotor's literal voltage being set to 5 diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 7632a216..db21ab27 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -211,4 +211,8 @@ public void testPeriodic() { break; } } + + public void setFeedforward(double newFF) { + io.setFF(newFF); + } } diff --git a/src/main/java/frc/robot/subsystems/climb/states/IdleState.java b/src/main/java/frc/robot/subsystems/climb/states/IdleState.java index 77e63b09..a0fc5b75 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/IdleState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/IdleState.java @@ -13,6 +13,7 @@ public IdleState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(0.0); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().restingAngle); } } diff --git a/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java b/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java index dcdc2c65..47daf878 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java @@ -2,6 +2,7 @@ import coppercore.controls.state_machine.state.PeriodicStateInterface; import frc.robot.constants.ClimbConstants; +import frc.robot.constants.JsonConstants; import frc.robot.subsystems.climb.ClimbSubsystem; public class LiftingState implements PeriodicStateInterface { @@ -14,6 +15,7 @@ public LiftingState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(JsonConstants.climbConstants.climbFFVolts); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().finalHangingAngle); } } diff --git a/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java b/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java index b0fdc239..4ce90dc3 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java @@ -14,6 +14,7 @@ public SearchingState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(0.0); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().searchingAngle); if (climbSubsystem.getLockedToCage()) { climbSubsystem.fireTrigger(ClimbAction.CLIMB); diff --git a/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java b/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java index d20c0da9..b3152c85 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java @@ -14,6 +14,7 @@ public WaitingState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(0.0); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().restingAngle); if (climbSubsystem.getRampClear()) { climbSubsystem.fireTrigger(ClimbAction.CLIMB); diff --git a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java index e4574b64..cee93377 100644 --- a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java +++ b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java @@ -36,7 +36,12 @@ public void periodic() { switch (scoringSubsystem.getGamePiece()) { case Coral: - scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + if (scoringSubsystem.getTarget() == FieldTarget.L2 + || scoringSubsystem.getTarget() == FieldTarget.L3) { + scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralL23ScoreVoltage); + } else { + scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + } if (!scoringSubsystem.isCoralDetected()) { scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); } From aecf26c5772689087f5023b285dce9832e008e67 Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Sat, 8 Mar 2025 18:55:08 -0500 Subject: [PATCH 24/31] Hopefully fix scoring bug --- src/main/deploy/constants/comp/ClimbConstants.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../frc/robot/subsystems/climb/ClimbIOTalonFX.java | 7 +++++-- .../frc/robot/subsystems/scoring/ScoringSubsystem.java | 2 +- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/deploy/constants/comp/ClimbConstants.json b/src/main/deploy/constants/comp/ClimbConstants.json index 4e4ac128..c9a858a6 100644 --- a/src/main/deploy/constants/comp/ClimbConstants.json +++ b/src/main/deploy/constants/comp/ClimbConstants.json @@ -18,5 +18,5 @@ "climbkD": 0.0, "climbCurrentLimit": 60, "invertFollowerClimbMotor": true, - "climbFFVolts": -10.0 + "climbFFVolts": -2.0 } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 320483c3..0b53f5d0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code-3"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 62; - public static final String GIT_SHA = "bedbf67634050658d9a580c7ab8f0443aa7e1c3b"; - public static final String GIT_DATE = "2025-03-08 14:04:41 EST"; + public static final int GIT_REVISION = 63; + public static final String GIT_SHA = "94d449f009665caad10078e274ee34eea04f1140"; + public static final String GIT_DATE = "2025-03-08 17:34:13 EST"; public static final String GIT_BRANCH = "comp/VAPOR"; - public static final String BUILD_DATE = "2025-03-08 17:23:44 EST"; - public static final long BUILD_UNIX_TIME = 1741472624107L; + public static final String BUILD_DATE = "2025-03-08 18:15:48 EST"; + public static final long BUILD_UNIX_TIME = 1741475748529L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java index 92389a81..a541a728 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java @@ -104,8 +104,11 @@ public void setFF(double newFF) { @Override public void applyOutputs(ClimbOutputs outputs) { - - calculator.withPosition(goalAngle.in(Rotations)).withFeedForward(feedforward); + if (goalAngle.lt(climbAngleCoder.getAbsolutePosition().getValue())) { + calculator.withPosition(goalAngle.in(Rotations)).withFeedForward(feedforward); + } else { + calculator.withPosition(goalAngle.in(Rotations)).withFeedForward(0.0); + } Logger.recordOutput("climb/calculatorAngle", leadMotor.getPosition().getValueAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index f14ce234..9c621986 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -633,7 +633,7 @@ public boolean shouldWaitOnScore() { hasGamePiece = clawMechanism.isAlgaeDetected(); break; } - return ((stateMachine.getCurrentState() == ScoringState.Warmup) + return ((stateMachine.getCurrentState() == ScoringState.Init) && (stateMachine.getCurrentState() == ScoringState.Warmup) || (stateMachine.getCurrentState() == ScoringState.Score)) && hasGamePiece; } From dc9d58226009f1aa101edaaa47ab887da4e55f30 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sat, 8 Mar 2025 23:29:53 -0500 Subject: [PATCH 25/31] Manually schedule and execute an OTF command in drive init to hopefully alleviate massive lagspike on auto init --- src/main/java/frc/robot/BuildConstants.java | 12 +-- .../frc/robot/subsystems/drive/Drive.java | 89 +++++++++++++------ 2 files changed, 68 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 0b53f5d0..b347d801 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-3"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 63; - public static final String GIT_SHA = "94d449f009665caad10078e274ee34eea04f1140"; - public static final String GIT_DATE = "2025-03-08 17:34:13 EST"; + public static final int GIT_REVISION = 64; + public static final String GIT_SHA = "aecf26c5772689087f5023b285dce9832e008e67"; + public static final String GIT_DATE = "2025-03-08 18:55:08 EST"; public static final String GIT_BRANCH = "comp/VAPOR"; - public static final String BUILD_DATE = "2025-03-08 18:15:48 EST"; - public static final long BUILD_UNIX_TIME = 1741475748529L; + public static final String BUILD_DATE = "2025-03-08 23:29:12 EST"; + public static final long BUILD_UNIX_TIME = 1741494552677L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 97884599..c8eacf8c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import coppercore.controls.state_machine.StateMachine; @@ -235,6 +236,8 @@ public enum DriveTrigger { private LocalADStarAK localADStar = new LocalADStarAK(); + private Command pathfindWarmupSpoofCommand; + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -276,7 +279,64 @@ public Drive( }); // warm up java processing for faster pathfind later + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { + localADStar.setDynamicObstacles( + List.of( + new Pair( + JsonConstants.redFieldLocations.coralAlgaeStackLeftTopCorner, + JsonConstants.redFieldLocations.coralAlgaeStackLeftBottomCorner), + new Pair( + JsonConstants.redFieldLocations.coralAlgaeStackMiddleTopCorner, + JsonConstants.redFieldLocations.coralAlgaeStackMiddleBottomCorner), + new Pair( + JsonConstants.redFieldLocations.coralAlgaeStackRightTopCorner, + JsonConstants.redFieldLocations.coralAlgaeStackRightBottomCorner)), + getPose().getTranslation()); + } else { + localADStar.setDynamicObstacles( + List.of( + new Pair( + JsonConstants.blueFieldLocations.coralAlgaeStackLeftTopCorner, + JsonConstants.blueFieldLocations.coralAlgaeStackLeftBottomCorner), + new Pair( + JsonConstants.blueFieldLocations.coralAlgaeStackMiddleTopCorner, + JsonConstants.blueFieldLocations.coralAlgaeStackMiddleBottomCorner), + new Pair( + JsonConstants.blueFieldLocations.coralAlgaeStackRightTopCorner, + JsonConstants.blueFieldLocations.coralAlgaeStackRightBottomCorner)), + getPose().getTranslation()); + } + PathfindingCommand.warmupCommand().schedule(); + + // DriveState.OTF.getState().getDriveToPoseCommand(). + // Manually create an OTF command on robot init to avoid massive lagspike when auto begins: + Pose2d goalPose = + isAllianceRed() + ? new Pose2d( + JsonConstants.redFieldLocations.redReef23Translation, + JsonConstants.redFieldLocations.redReef23Rotation) + : new Pose2d( + JsonConstants.blueFieldLocations.blueReef23Translation, + JsonConstants.blueFieldLocations.blueReef23Rotation); + + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + JsonConstants.drivetrainConstants.OTFMaxLinearVelocity, + JsonConstants.drivetrainConstants.OTFMaxLinearAccel, + JsonConstants.drivetrainConstants.OTFMaxAngularVelocity, + JsonConstants.drivetrainConstants.OTFMaxAngularAccel); + + pathfindWarmupSpoofCommand = + AutoBuilder.pathfindToPose( + goalPose, constraints, JsonConstants.drivetrainConstants.otfPoseEndingVelocity); + + pathfindWarmupSpoofCommand.schedule(); + + // Manually call execute because the 1st execute seems to take 0.9 seconds in auto + pathfindWarmupSpoofCommand.execute(); + angleController.enableContinuousInput(-Math.PI, Math.PI); // Configure SysId sysId = @@ -324,34 +384,9 @@ public void configureStates() { stateMachine = new StateMachine<>(stateMachineConfiguration, DriveState.Joystick); } - /** add algae coral stack obstacles for on the fly */ public void autonomousInit() { - if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { - localADStar.setDynamicObstacles( - List.of( - new Pair( - JsonConstants.redFieldLocations.coralAlgaeStackLeftTopCorner, - JsonConstants.redFieldLocations.coralAlgaeStackLeftBottomCorner), - new Pair( - JsonConstants.redFieldLocations.coralAlgaeStackMiddleTopCorner, - JsonConstants.redFieldLocations.coralAlgaeStackMiddleBottomCorner), - new Pair( - JsonConstants.redFieldLocations.coralAlgaeStackRightTopCorner, - JsonConstants.redFieldLocations.coralAlgaeStackRightBottomCorner)), - getPose().getTranslation()); - } else { - localADStar.setDynamicObstacles( - List.of( - new Pair( - JsonConstants.blueFieldLocations.coralAlgaeStackLeftTopCorner, - JsonConstants.blueFieldLocations.coralAlgaeStackLeftBottomCorner), - new Pair( - JsonConstants.blueFieldLocations.coralAlgaeStackMiddleTopCorner, - JsonConstants.blueFieldLocations.coralAlgaeStackMiddleBottomCorner), - new Pair( - JsonConstants.blueFieldLocations.coralAlgaeStackRightTopCorner, - JsonConstants.blueFieldLocations.coralAlgaeStackRightBottomCorner)), - getPose().getTranslation()); + if (pathfindWarmupSpoofCommand != null && pathfindWarmupSpoofCommand.isScheduled()) { + pathfindWarmupSpoofCommand.cancel(); } } From 7aece5edc14e8a4581f6617b78ef8b4502bcbc47 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sat, 8 Mar 2025 23:30:39 -0500 Subject: [PATCH 26/31] #167: Hopefully make scoring shouldWaitOnScore logic more logical to avoid edge case where auto lines up before scoring has initialized --- .../java/frc/robot/subsystems/scoring/ScoringSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 9c621986..112c6197 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -633,7 +633,8 @@ public boolean shouldWaitOnScore() { hasGamePiece = clawMechanism.isAlgaeDetected(); break; } - return ((stateMachine.getCurrentState() == ScoringState.Init) && (stateMachine.getCurrentState() == ScoringState.Warmup) + return ((stateMachine.getCurrentState() == ScoringState.Init) + || (stateMachine.getCurrentState() == ScoringState.Warmup) || (stateMachine.getCurrentState() == ScoringState.Score)) && hasGamePiece; } From 620d714148bb31c526199ae75defa6682a661301 Mon Sep 17 00:00:00 2001 From: linglejack06 Date: Tue, 11 Mar 2025 18:38:35 -0400 Subject: [PATCH 27/31] final comp changes --- build.gradle | 22 +++++-- src/main/deploy/auto/ExampleAutoPath.json | 4 +- src/main/java/frc/robot/BuildConstants.java | 12 ++-- src/main/java/frc/robot/RobotContainer.java | 8 +-- .../frc/robot/subsystems/drive/Drive.java | 64 +++++++++++-------- .../java/frc/robot/subsystems/led/LED.java | 5 +- 6 files changed, 66 insertions(+), 49 deletions(-) diff --git a/build.gradle b/build.gradle index e17d1e30..53bf715a 100644 --- a/build.gradle +++ b/build.gradle @@ -30,15 +30,23 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // https://www.chiefdelphi.com/t/2024-wpilib-feedback/464322/141 - final MAX_JAVA_HEAP_SIZE_MB = 100; - // jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + final MAX_JAVA_HEAP_SIZE_MB = 200; + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") // Set the minimum heap size to the maximum heap size to avoid resizing - // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") - //jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") - //jvmArgs.add("-XX:GCTimeRatio=5") - //jvmArgs.add("-XX:+UseSerialGC") - //jvmArgs.add("-XX:MaxGCPauseMillis=50") + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // VisualVM stuff (comment out in match) + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.4.1.2") } // Static files artifact diff --git a/src/main/deploy/auto/ExampleAutoPath.json b/src/main/deploy/auto/ExampleAutoPath.json index 54908a25..2db912bc 100644 --- a/src/main/deploy/auto/ExampleAutoPath.json +++ b/src/main/deploy/auto/ExampleAutoPath.json @@ -4,8 +4,8 @@ "Reef5" ], "scoringLevels": [ - "L2", - "L4" + "L1", + "L1" ], "intakeLocation": "CoralStationRight" } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index b347d801..6d1304d0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code"; + public static final String MAVEN_NAME = "2025-Robot-Code-3"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 64; - public static final String GIT_SHA = "aecf26c5772689087f5023b285dce9832e008e67"; - public static final String GIT_DATE = "2025-03-08 18:55:08 EST"; + public static final int GIT_REVISION = 66; + public static final String GIT_SHA = "7aece5edc14e8a4581f6617b78ef8b4502bcbc47"; + public static final String GIT_DATE = "2025-03-08 23:30:39 EST"; public static final String GIT_BRANCH = "comp/VAPOR"; - public static final String BUILD_DATE = "2025-03-08 23:29:12 EST"; - public static final long BUILD_UNIX_TIME = 1741494552677L; + public static final String BUILD_DATE = "2025-03-09 10:29:40 EDT"; + public static final long BUILD_UNIX_TIME = 1741530580198L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5098c72b..9c099616 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -279,10 +279,10 @@ public void periodic() { public void autonomousInit() { drive.autonomousInit(); - setSubsystemsToBrake(); - if (FeatureFlags.synced.getObject().runLEDs) { - led.setLedOn(true); - } + // setSubsystemsToBrake(); + // if (FeatureFlags.synced.getObject().runLEDs) { + // led.setLedOn(true); + // } // load chosen strategy strategyManager.autonomousInit(autoChooser.getSelected()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c8eacf8c..d8193728 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -9,7 +9,6 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import coppercore.controls.state_machine.StateMachine; @@ -237,6 +236,7 @@ public enum DriveTrigger { private LocalADStarAK localADStar = new LocalADStarAK(); private Command pathfindWarmupSpoofCommand; + private Command warmupCommand = PathfindingCommand.warmupCommand(); public Drive( GyroIO gyroIO, @@ -307,35 +307,35 @@ public Drive( getPose().getTranslation()); } - PathfindingCommand.warmupCommand().schedule(); + warmupCommand.schedule(); - // DriveState.OTF.getState().getDriveToPoseCommand(). - // Manually create an OTF command on robot init to avoid massive lagspike when auto begins: - Pose2d goalPose = - isAllianceRed() - ? new Pose2d( - JsonConstants.redFieldLocations.redReef23Translation, - JsonConstants.redFieldLocations.redReef23Rotation) - : new Pose2d( - JsonConstants.blueFieldLocations.blueReef23Translation, - JsonConstants.blueFieldLocations.blueReef23Rotation); + // // DriveState.OTF.getState().getDriveToPoseCommand(). + // // Manually create an OTF command on robot init to avoid massive lagspike when auto begins: + // Pose2d goalPose = + // isAllianceRed() + // ? new Pose2d( + // JsonConstants.redFieldLocations.redReef23Translation, + // JsonConstants.redFieldLocations.redReef23Rotation) + // : new Pose2d( + // JsonConstants.blueFieldLocations.blueReef23Translation, + // JsonConstants.blueFieldLocations.blueReef23Rotation); - // Create the constraints to use while pathfinding - PathConstraints constraints = - new PathConstraints( - JsonConstants.drivetrainConstants.OTFMaxLinearVelocity, - JsonConstants.drivetrainConstants.OTFMaxLinearAccel, - JsonConstants.drivetrainConstants.OTFMaxAngularVelocity, - JsonConstants.drivetrainConstants.OTFMaxAngularAccel); + // // Create the constraints to use while pathfinding + // PathConstraints constraints = + // new PathConstraints( + // JsonConstants.drivetrainConstants.OTFMaxLinearVelocity, + // JsonConstants.drivetrainConstants.OTFMaxLinearAccel, + // JsonConstants.drivetrainConstants.OTFMaxAngularVelocity, + // JsonConstants.drivetrainConstants.OTFMaxAngularAccel); - pathfindWarmupSpoofCommand = - AutoBuilder.pathfindToPose( - goalPose, constraints, JsonConstants.drivetrainConstants.otfPoseEndingVelocity); + // pathfindWarmupSpoofCommand = + // AutoBuilder.pathfindToPose( + // goalPose, constraints, JsonConstants.drivetrainConstants.otfPoseEndingVelocity); - pathfindWarmupSpoofCommand.schedule(); + // pathfindWarmupSpoofCommand.schedule(); - // Manually call execute because the 1st execute seems to take 0.9 seconds in auto - pathfindWarmupSpoofCommand.execute(); + // // Manually call execute because the 1st execute seems to take 0.9 seconds in auto + // pathfindWarmupSpoofCommand.execute(); angleController.enableContinuousInput(-Math.PI, Math.PI); // Configure SysId @@ -385,8 +385,12 @@ public void configureStates() { } public void autonomousInit() { - if (pathfindWarmupSpoofCommand != null && pathfindWarmupSpoofCommand.isScheduled()) { - pathfindWarmupSpoofCommand.cancel(); + // if (pathfindWarmupSpoofCommand != null && pathfindWarmupSpoofCommand.isScheduled()) { + // pathfindWarmupSpoofCommand.cancel(); + // } + + if (warmupCommand != null && warmupCommand.isScheduled()) { + warmupCommand.cancel(); } } @@ -483,6 +487,12 @@ else if (!DriverStation.isDisabled()) { Logger.recordOutput("Drive/driveLinedUp", driveLinedUp); } + public void disabledPeriodic() { + if (warmupCommand != null) { + Logger.recordOutput("Drive/warmupScheduled", warmupCommand.isScheduled()); + } + } + /** * sets desired speeds of robot * diff --git a/src/main/java/frc/robot/subsystems/led/LED.java b/src/main/java/frc/robot/subsystems/led/LED.java index 9d404a93..361f7ba8 100644 --- a/src/main/java/frc/robot/subsystems/led/LED.java +++ b/src/main/java/frc/robot/subsystems/led/LED.java @@ -24,7 +24,7 @@ public class LED extends SubsystemBase { private List rightPatterns = new ArrayList<>(); private final AddressableLED led = new AddressableLED(LEDConstants.ledPort); - public AddressableLEDBuffer ledStrip = new AddressableLEDBuffer(LEDConstants.totalLengthRainbow); + public AddressableLEDBuffer ledStrip = new AddressableLEDBuffer(LEDConstants.totalLength); public AddressableLEDBufferView leftData = ledStrip @@ -52,7 +52,7 @@ public LED(ScoringSubsystem scoringSubsystem, ClimbSubsystem climbSubsystem, Dri this.scoringSubsystem = scoringSubsystem; this.climbSubsystem = climbSubsystem; this.driveSubsystem = drive; - led.setLength(LEDConstants.totalLengthRainbow); + led.setLength(LEDConstants.totalLength); led.start(); } @@ -75,7 +75,6 @@ private void setEnabled(boolean enabled) { /** Periodic method called every loop cycle. Updates LED patterns based on robot state. */ @Override public void periodic() { - setEnabled(DriverStation.isEnabled()); if (!DriverStation.isDisabled()) { // Drive Subsystem Checks From e48933290017a51ad8d1411f02f50849ec1391b2 Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Tue, 11 Mar 2025 19:31:12 -0400 Subject: [PATCH 28/31] wip --- src/main/java/frc/robot/BuildConstants.java | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8eece176..b347d801 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,22 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; -<<<<<<< HEAD - public static final int GIT_REVISION = 63; - public static final String GIT_SHA = "77da6e9c485e77439690e47e86740b5ea086ebc9"; - public static final String GIT_DATE = "2025-02-28 19:54:39 EST"; - public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-03-03 18:57:53 EST"; - public static final long BUILD_UNIX_TIME = 1741046273024L; - -======= public static final int GIT_REVISION = 64; public static final String GIT_SHA = "aecf26c5772689087f5023b285dce9832e008e67"; public static final String GIT_DATE = "2025-03-08 18:55:08 EST"; public static final String GIT_BRANCH = "comp/VAPOR"; public static final String BUILD_DATE = "2025-03-08 23:29:12 EST"; public static final long BUILD_UNIX_TIME = 1741494552677L; ->>>>>>> origin/comp/VAPOR public static final int DIRTY = 1; private BuildConstants() {} From e23c81318dd0d14f2c79d7a739da70bb334ea43d Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Thu, 13 Mar 2025 13:56:54 -0400 Subject: [PATCH 29/31] wip --- simgui-ds.json | 15 +++++++++------ src/main/java/frc/robot/BuildConstants.java | 16 ++++++++-------- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 442546ae..cbda5545 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -79,12 +79,12 @@ "axisCount": 2, "buttonCount": 6, "buttonKeys": [ - 260, - 268, - 266, - 261, - 269, - 267 + 49, + 50, + 51, + 52, + 53, + 54 ], "povCount": 0 }, @@ -100,6 +100,9 @@ }, { "guid": "Keyboard1" + }, + { + "guid": "Keyboard2" } ] } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6d1304d0..e7c7af51 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,15 +3,15 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-3"; + public static final String MAVEN_NAME = "2025-Robot-Code-1"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 66; - public static final String GIT_SHA = "7aece5edc14e8a4581f6617b78ef8b4502bcbc47"; - public static final String GIT_DATE = "2025-03-08 23:30:39 EST"; - public static final String GIT_BRANCH = "comp/VAPOR"; - public static final String BUILD_DATE = "2025-03-09 10:29:40 EDT"; - public static final long BUILD_UNIX_TIME = 1741530580198L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 95; + public static final String GIT_SHA = "d1fde06a809143e8516dc6c5d2b6a6d48e17ca87"; + public static final String GIT_DATE = "2025-03-11 19:31:18 EDT"; + public static final String GIT_BRANCH = "maplesim-intake"; + public static final String BUILD_DATE = "2025-03-11 19:33:45 EDT"; + public static final long BUILD_UNIX_TIME = 1741736025974L; + public static final int DIRTY = 0; private BuildConstants() {} } From 11e804357aa0e68a9318f719feff477e4b6b8d97 Mon Sep 17 00:00:00 2001 From: Sojo123456 Date: Mon, 17 Mar 2025 19:17:23 -0400 Subject: [PATCH 30/31] fixing weird broken maplesim components --- simgui-ds.json | 7 ++++--- .../test_drivebase/ElevatorConstants.json | 2 +- src/main/java/frc/robot/BuildConstants.java | 14 +++++++------- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/scoring/ClawIOSim.java | 4 ++-- .../robot/subsystems/scoring/ScoringSubsystem.java | 2 +- 6 files changed, 16 insertions(+), 15 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index cbda5545..8bcd0c7b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,13 +96,14 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "030000006d04000014c2000000000000" }, { - "guid": "Keyboard1" + "guid": "030000006d04000014c2000000000000" }, { - "guid": "Keyboard2" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/deploy/constants/test_drivebase/ElevatorConstants.json b/src/main/deploy/constants/test_drivebase/ElevatorConstants.json index b21df5ee..8cd68dd8 100644 --- a/src/main/deploy/constants/test_drivebase/ElevatorConstants.json +++ b/src/main/deploy/constants/test_drivebase/ElevatorConstants.json @@ -20,7 +20,7 @@ }, "elevatorCANCoderDiscontinuityPoint": 1.0, "elevatorLargeCANCoderID": 32, - "elevatorLargeCANCoderDirection": "CounterClockwise_Positive", + "elevatorLargeCANCoderDirection": "Clockwise_Positive", "elevatorSmallCANCoderID": 33, "elevatorSmallCANCoderDirection": "CounterClockwise_Positive", "elevatorkP": 60.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e7c7af51..63018a16 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,15 +3,15 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-1"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 95; - public static final String GIT_SHA = "d1fde06a809143e8516dc6c5d2b6a6d48e17ca87"; - public static final String GIT_DATE = "2025-03-11 19:31:18 EDT"; + public static final int GIT_REVISION = 96; + public static final String GIT_SHA = "e23c81318dd0d14f2c79d7a739da70bb334ea43d"; + public static final String GIT_DATE = "2025-03-13 13:56:54 EDT"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-03-11 19:33:45 EDT"; - public static final long BUILD_UNIX_TIME = 1741736025974L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2025-03-17 19:13:17 EDT"; + public static final long BUILD_UNIX_TIME = 1742253197181L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ca85db0..c91d2f59 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -142,7 +142,7 @@ public void updateRobotModel() { new Translation3d(-0.16, 0.31, 0.115), new Rotation3d(climb_rotation, 0.0, 0.0)), new Pose3d( new Translation3d(0.34, 0.12, height + 0.35), - new Rotation3d(0.0, -claw_rotation + 0.465719787 * 180.0, 0.0)), + new Rotation3d(0.0, -claw_rotation + 0.465719787 * 180.0 + 3.14, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_two_height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_one_height), new Rotation3d(0.0, 0.0, 0.0)) diff --git a/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java b/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java index 66b9fb3b..1d542752 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java @@ -75,11 +75,11 @@ private void updateSimState() { .times(Seconds.of(0.02))); if (has == HasState.NONE) { - if (outputVoltage.in(Volts) > 0.0 && algaeAvailable) { + if (outputVoltage.in(Volts) > 0.0 || algaeAvailable) { has = HasState.ALGAE; piecePos = 1.0; } - if (outputVoltage.in(Volts) > 0.0 && coralAvailable) { + if (outputVoltage.in(Volts) > 0.0 || coralAvailable) { has = HasState.CORAL; piecePos = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 112c6197..b3001390 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -278,7 +278,7 @@ && canFarWarmup()) .permitIf( ScoringTrigger.WarmupReady, ScoringState.Score, - () -> isDriveLinedUpSupplier.getAsBoolean()) + () -> true) // () -> isDriveLinedUpSupplier.getAsBoolean()) .permit(ScoringTrigger.ReturnToIdle, ScoringState.Idle) .permit(ScoringTrigger.CancelWarmup, ScoringState.Idle); From 9fd5de189ed2d787c99f82ed1350d2f4ebba6a52 Mon Sep 17 00:00:00 2001 From: Sojo123456 Date: Mon, 17 Mar 2025 19:52:51 -0400 Subject: [PATCH 31/31] wip, can someone run this on a better computer than shop laptops and see what it does? thank you! --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/scoring/ScoringSubsystemMapleSim.java | 8 ++++++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 63018a16..72f0bf72 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 96; - public static final String GIT_SHA = "e23c81318dd0d14f2c79d7a739da70bb334ea43d"; - public static final String GIT_DATE = "2025-03-13 13:56:54 EDT"; + public static final int GIT_REVISION = 97; + public static final String GIT_SHA = "11e804357aa0e68a9318f719feff477e4b6b8d97"; + public static final String GIT_DATE = "2025-03-17 19:17:23 EDT"; public static final String GIT_BRANCH = "maplesim-intake"; - public static final String BUILD_DATE = "2025-03-17 19:13:17 EDT"; - public static final long BUILD_UNIX_TIME = 1742253197181L; + public static final String BUILD_DATE = "2025-03-17 19:45:03 EDT"; + public static final long BUILD_UNIX_TIME = 1742255103915L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c91d2f59..0c600eef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -141,7 +141,7 @@ public void updateRobotModel() { new Pose3d( new Translation3d(-0.16, 0.31, 0.115), new Rotation3d(climb_rotation, 0.0, 0.0)), new Pose3d( - new Translation3d(0.34, 0.12, height + 0.35), + new Translation3d(0.34, 0.12, height + 0.4), new Rotation3d(0.0, -claw_rotation + 0.465719787 * 180.0 + 3.14, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_two_height), new Rotation3d(0.0, 0.0, 0.0)), diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java index 6e93f558..0e19b0d5 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; @@ -62,9 +63,12 @@ public static void shootCoral() { drive.getPose().getRotation(), Meters.of( scoring.getElevatorHeight().in(Meters) - + 0.5), // initial height of the ball, in meters + + 0.4), // initial height of the ball, in meters MetersPerSecond.of(4), // initial velocity, in m/s - Degrees.of((scoring.getWristAngle().in(Degrees)) * -1)) // shooter angle + Degrees.of( + (scoring.getWristAngle().in(Radians) + + 0.465719787 * 180.0 + + 3.14))) // shooter angle .withProjectileTrajectoryDisplayCallBack( (poses) -> {}, (poses) ->