[wpilib] Add physics simulation support with state-space (#2615)

This includes physics simulation support for arms/elevator models, as well as differential drivetrains.

Swerve might be added at a later date.

Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/simulation/SingleJointedArmSim.h"
+#include "gtest/gtest.h"
+
+TEST(SingleJointedArmTest, Disabled) {
+  frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 10_kg,
+                                    9.5_in, -180_deg, 0_deg, false, {0.0});
+  sim.ResetState(frc::MakeMatrix<2, 1>(0.0, 0.0));
+
+  for (size_t i = 0; i < 12 / 0.02; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.Update(20_ms);
+  }
+
+  // The arm should swing down.
+  EXPECT_NEAR(sim.GetAngle().to<double>(), -wpi::math::pi / 2, 0.01);
+}