Maplesim Shoot

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

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
     },
@@ -96,10 +96,14 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "030000006d04000014c2000000000000"
     },
     {
-      "guid": "Keyboard1"
+      "guid": "030000006d04000014c2000000000000"
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

src/main/deploy/auto/ExampleAutoPath.json

@@ -1,11 +1,11 @@
 {
   "scoringLocations": [
-    "Reef2",
+    "Reef5",
     "Reef5"
   ],
   "scoringLevels": [
-    "L2",
-    "L4"
+    "L1",
+    "L1"
   ],
   "intakeLocation": "CoralStationRight"
 }

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": {

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,
@@ -38,7 +38,11 @@
     "unit": "Volt"
   },
   "coralScoreVoltage": {
-    "value": 12.0,
+    "value": 10.0,
+    "unit": "Volt"
+  },
+  "coralL23ScoreVoltage": {
+    "value": 4.0,
     "unit": "Volt"
   },
   "algaeScoreVoltage": {

src/main/deploy/constants/comp/ClimbConstants.json

@@ -17,5 +17,6 @@
   "climbkI": 0.0,
   "climbkD": 0.0,
   "climbCurrentLimit": 60,
-  "invertFollowerClimbMotor": true
+  "invertFollowerClimbMotor": true,
+  "climbFFVolts": -2.0
 }

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,

src/main/deploy/constants/config.json

@@ -1,5 +1,5 @@
 {
-  "environment": "comp",
+  "environment": "test_drivebase",
   "defaults": "comp",
   "environments": [
     {

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,

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 = 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:45:03 EDT";
+  public static final long BUILD_UNIX_TIME = 1742255103915L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/frc/robot/InitBindings.java

@@ -291,13 +291,35 @@ public static void initScoringBindings(ScoringSubsystem scoring) {
         .onTrue(
             new InstantCommand(
                 () -> {
+                  scoring.setTarget(FieldTarget.Net);
+                  scoring.setGamePiece(GamePiece.Algae);
                   scoring.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage);
                 }))
         .onFalse(
             new InstantCommand(
                 () -> {
                   scoring.setClawRollerVoltage(Volts.zero());
                 }));
+
+    driverController
+        .leftBumper()
+        .onTrue(
+            new InstantCommand(
+                () -> {
+                  scoring.setTarget(FieldTarget.Ground);
+                  scoring.setGamePiece(GamePiece.Coral);
+                }));
+
+    rightJoystick
+        .top()
+        .onTrue(
+            new InstantCommand(
+                () -> {
+                  if (ScoringSubsystem.getInstance().getGamePiece() == GamePiece.Algae
+                      && ScoringSubsystem.getInstance().getTarget() == FieldTarget.Processor) {
+                    ScoringSubsystem.getInstance().fireTrigger(ScoringTrigger.WarmupReady);
+                  }
+                }));
   }
 
   /**

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. */

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;
@@ -34,9 +33,11 @@
 import frc.robot.subsystems.led.LED;
 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;
 
 /**
@@ -140,8 +141,8 @@ 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 Rotation3d(0.0, -claw_rotation + 0.465719787 * 180.0, 0.0)),
+              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)),
           new Pose3d(new Translation3d(0.0, 0.0, stage_one_height), new Rotation3d(0.0, 0.0, 0.0))
@@ -207,10 +208,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(
@@ -224,6 +229,7 @@ public void configureSubsystems() {
       } else {
         scoringSubsystem.setIsDriveLinedUpSupplier(() -> true);
       }
+      scoringSubsystem.setIsDriveLinedUpSupplier(() -> true);
     }
 
     if (FeatureFlags.synced.getObject().runLEDs) {
@@ -276,10 +282,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());
@@ -378,10 +384,24 @@ 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 resetMapleSim() {
+    SimulatedArena.getInstance().resetFieldForAuto();
+    ReefscapeAlgaeOnFly.setHitNetCallBack(() -> System.out.println("ALGAE hits NET!"));
+    ScoringSubsystemMapleSim.configDrive(drive, driveSim);
+    ScoringSubsystemMapleSim.configScoring(scoringSubsystem);
   }
 
   public void updateMapleSim() {

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 */

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<ClimbConstants.Sim> synced =

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

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) {}
 }

src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java

@@ -96,10 +96,19 @@ 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));
+    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());
 
@@ -108,6 +117,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