[wpimath] Fix ElevatorSim::GetCurrentDraw() in C++ (#8370)

The Kv calculation in C++ was missing a negative sign compared to the Java implementation.

Author: bhall-ctre

wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp

@@ -88,7 +88,7 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
   double kA = 1.0 / m_plant.B(1, 0);
   using Kv_t = units::unit_t<units::compound_unit<
       units::volt, units::inverse<units::meters_per_second>>>;
-  Kv_t Kv = Kv_t{kA * m_plant.A(1, 1)};
+  Kv_t Kv = Kv_t{-kA * m_plant.A(1, 1)};
   units::meters_per_second_t velocity{m_x(1)};
   units::radians_per_second_t motorVelocity = velocity * Kv * m_gearbox.Kv;
 

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

@@ -92,3 +92,15 @@ TEST(ElevatorSimTest, Stability) {
                                        frc::Vectord<1>{12.0}, 20_ms * 50)(0)},
       sim.GetPosition(), 1_cm);
 }
+
+TEST(ElevatorSimTest, CurrentDraw) {
+  auto const motor = frc::DCMotor::KrakenX60(2);
+  frc::sim::ElevatorSim sim(motor, 20, 8_kg, 0.1_m, 0_m, 1_m, true, 0_m,
+                            {0.01, 0.0});
+
+  EXPECT_DOUBLE_EQ(0.0, sim.GetCurrentDraw().value());
+  sim.SetInputVoltage(motor.Voltage(motor.Torque(60_A), 0_rad_per_s));
+  sim.Update(100_ms);
+  // current draw should start at 60 A and decrease as the back emf catches up
+  EXPECT_TRUE(0_A < sim.GetCurrentDraw() && sim.GetCurrentDraw() < 60_A);
+}

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

@@ -124,4 +124,16 @@ void testStability() {
         sim.getPositionMeters(),
         0.01);
   }
+
+  @Test
+  void testCurrentDraw() {
+    var motor = DCMotor.getKrakenX60(2);
+    var sim = new ElevatorSim(motor, 20, 8.0, 0.1, 0.0, 1.0, true, 0.0, 0.01, 0.0);
+
+    assertEquals(0.0, sim.getCurrentDrawAmps());
+    sim.setInputVoltage(motor.getVoltage(motor.getTorque(60.0), 0.0));
+    sim.update(0.100);
+    // current draw should start at 60 A and decrease as the back emf catches up
+    assertTrue(0.0 < sim.getCurrentDrawAmps() && sim.getCurrentDrawAmps() < 60.0);
+  }
 }