[wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288)

- Add a constructor parameter to configure the initial angle of the arm
- Also reorganizes cascading constructors for Java

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -33,6 +33,7 @@ class ElevatorSimTest {
             0.0,
             3.0,
             true,
+            0.0,
             VecBuilder.fill(0.01));
 
     try (var motor = new PWMVictorSPX(0);
@@ -71,6 +72,7 @@ class ElevatorSimTest {
             0.0,
             1.0,
             true,
+            0.0,
             VecBuilder.fill(0.01));
 
     for (int i = 0; i < 100; i++) {
@@ -91,7 +93,8 @@ class ElevatorSimTest {
   @Test
   void testStability() {
     var sim =
-        new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false);
+        new ElevatorSim(
+            DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0);
 
     sim.setState(VecBuilder.fill(0, 0));
     sim.setInput(12);

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java

@@ -14,10 +14,18 @@ import org.junit.jupiter.api.Test;
 class SingleJointedArmSimTest {
   SingleJointedArmSim m_sim =
       new SingleJointedArmSim(
-          DCMotor.getVex775Pro(2), 300, 3.0, Units.inchesToMeters(30.0), -Math.PI, 0.0, true);
+          DCMotor.getVex775Pro(2),
+          300,
+          3.0,
+          Units.inchesToMeters(30.0),
+          -Math.PI,
+          0.0,
+          true,
+          Math.PI / 2.0);
 
   @Test
   void testArmDisabled() {
+    // Reset Arm angle to 0
     m_sim.setState(VecBuilder.fill(0.0, 0.0));
 
     for (int i = 0; i < 12 / 0.02; i++) {