[wpimath] Fix DCMotor.getSpeed() (#5061)

This bug didn't occur in C++ because the units system caught it at
compile time.

cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp

@@ -474,6 +474,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture() {
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
+  ::GetInstance();
   cs::UsbCamera camera{fmt::format("USB Camera {}", dev), dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -483,6 +484,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   int dev) {
+  ::GetInstance();
   cs::UsbCamera camera{name, dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -492,6 +494,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   std::string_view path) {
+  ::GetInstance();
   cs::UsbCamera camera{name, path};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -517,6 +520,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::span<const std::string> hosts) {
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::string_view host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -526,6 +530,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const char* host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -535,6 +540,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const std::string& host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -544,6 +550,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::span<const std::string> hosts) {
+  ::GetInstance();
   cs::AxisCamera camera{name, hosts};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -552,10 +559,11 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 }
 
 cs::MjpegServer CameraServer::AddSwitchedCamera(std::string_view name) {
+  auto& inst = ::GetInstance();
   // create a dummy CvSource
   cs::CvSource source{name, cs::VideoMode::PixelFormat::kMJPEG, 160, 120, 30};
   cs::MjpegServer server = StartAutomaticCapture(source);
-  ::GetInstance().m_fixedSources[server.GetHandle()] = source.GetHandle();
+  inst.m_fixedSources[server.GetHandle()] = source.GetHandle();
 
   return server;
 }
@@ -632,6 +640,7 @@ cs::CvSink CameraServer::GetVideo(std::string_view name) {
 
 cs::CvSource CameraServer::PutVideo(std::string_view name, int width,
                                     int height) {
+  ::GetInstance();
   cs::CvSource source{name, cs::VideoMode::kMJPEG, width, height, 30};
   StartAutomaticCapture(source);
   return source;
@@ -648,6 +657,7 @@ cs::MjpegServer CameraServer::AddServer(std::string_view name) {
 }
 
 cs::MjpegServer CameraServer::AddServer(std::string_view name, int port) {
+  ::GetInstance();
   cs::MjpegServer server{name, port};
   AddServer(server);
   return server;

ntcore/src/main/native/cpp/LocalStorage.cpp

@@ -1230,6 +1230,10 @@ PublisherData* LSImpl::PublishEntry(EntryData* entry, NT_Type type) {
   // create publisher
   entry->publisher = AddLocalPublisher(entry->topic, wpi::json::object(),
                                        entry->subscriber->config);
+  // exclude publisher if requested
+  if (entry->subscriber->config.excludeSelf) {
+    entry->subscriber->config.excludePublisher = entry->publisher->handle;
+  }
   return entry->publisher;
 }
 
@@ -1282,9 +1286,6 @@ bool LSImpl::SetEntryValue(NT_Handle pubentryHandle, const Value& value) {
   if (!publisher) {
     if (auto entry = m_entries.Get(pubentryHandle)) {
       publisher = PublishEntry(entry, value.type());
-      if (entry->subscriber->config.excludeSelf) {
-        entry->subscriber->config.excludePublisher = publisher->handle;
-      }
     }
     if (!publisher) {
       return false;

ntcore/src/main/native/cpp/net/ServerImpl.cpp

@@ -599,13 +599,17 @@ void ClientData4Base::ClientSetProperties(std::string_view name,
   auto topicIt = m_server.m_nameTopics.find(name);
   if (topicIt == m_server.m_nameTopics.end() ||
       !topicIt->second->IsPublished()) {
-    DEBUG3("ignored SetProperties from {} on non-existent topic '{}'", m_id,
-           name);
+    WARNING(
+        "server ignoring SetProperties({}) from client {} on unpublished topic "
+        "'{}'; publish or set a value first",
+        update.dump(), m_id, name);
     return;  // nothing to do
   }
   auto topic = topicIt->second;
   if (topic->special) {
-    DEBUG3("ignored SetProperties from {} on meta topic '{}'", m_id, name);
+    WARNING(
+        "server ignoring SetProperties({}) from client {} on meta topic '{}'",
+        update.dump(), m_id, name);
     return;  // nothing to do
   }
   m_server.SetProperties(nullptr, topic, update);

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

@@ -41,11 +41,11 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
       m_simulateGravity(simulateGravity) {}
 
 bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
-  return elevatorHeight < m_minHeight;
+  return elevatorHeight <= m_minHeight;
 }
 
 bool ElevatorSim::WouldHitUpperLimit(units::meter_t elevatorHeight) const {
-  return elevatorHeight > m_maxHeight;
+  return elevatorHeight >= m_maxHeight;
 }
 
 bool ElevatorSim::HasHitLowerLimit() const {

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

@@ -11,37 +11,20 @@
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
 
-#include "frc/Errors.h"
-
 using namespace frc;
 using namespace frc::sim;
 
 SimDeviceSim::SimDeviceSim(const char* name)
-    : m_handle{HALSIM_GetSimDeviceHandle(name)} {
-  if (m_handle == 0) {
-    throw FRC_MakeError(err::InvalidParameter,
-                        "No sim device exists with name '{}'.", name);
-  }
-}
+    : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
 
 SimDeviceSim::SimDeviceSim(const char* name, int index) {
   m_handle =
       HALSIM_GetSimDeviceHandle(fmt::format("{}[{}]", name, index).c_str());
-  if (m_handle == 0) {
-    throw FRC_MakeError(err::InvalidParameter,
-                        "No sim device exists with name '{}[{}]'.", name,
-                        index);
-  }
 }
 
 SimDeviceSim::SimDeviceSim(const char* name, int index, int channel) {
   m_handle = HALSIM_GetSimDeviceHandle(
       fmt::format("{}[{},{}]", name, index, channel).c_str());
-  if (m_handle == 0) {
-    throw FRC_MakeError(err::InvalidParameter,
-                        "No sim device exists with name '{}[{},{}]'.", name,
-                        index, channel);
-  }
 }
 
 SimDeviceSim::SimDeviceSim(HAL_SimDeviceHandle handle) : m_handle(handle) {}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

@@ -162,7 +162,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @return Whether the elevator would hit the lower limit.
    */
   public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
-    return elevatorHeightMeters < this.m_minHeight;
+    return elevatorHeightMeters <= this.m_minHeight;
   }
 
   /**
@@ -172,7 +172,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    * @return Whether the elevator would hit the upper limit.
    */
   public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
-    return elevatorHeightMeters > this.m_maxHeight;
+    return elevatorHeightMeters >= this.m_maxHeight;
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java

@@ -25,9 +25,6 @@ public class SimDeviceSim {
    */
   public SimDeviceSim(String name) {
     this(SimDeviceDataJNI.getSimDeviceHandle(name));
-    if (m_handle == 0) {
-      throw new IllegalArgumentException("No sim device exists with name '" + name + "'.");
-    }
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java

@@ -86,7 +86,8 @@ public class DCMotor {
    * @return The speed of the motor.
    */
   public double getSpeed(double torqueNm, double voltageInputVolts) {
-    return voltageInputVolts - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
+    return voltageInputVolts * KvRadPerSecPerVolt
+        - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
   }
 
   /**