[wpilib] Fix multiple motor safety issues (#4784)

Java was missing the motor safety thread entirely
C++ accidentally used a manual reset event, causing the motor safety thread to spin.
C++ PWMMotorController would not feed the watch kitty.
Both languages would call feed() from the StopMotor call, causing some ping ponging.

styleguide/spotbugs-exclude.xml

@@ -126,6 +126,9 @@
   <Match>
     <Bug pattern="VA_FORMAT_STRING_USES_NEWLINE" />
   </Match>
+  <Match>
+    <Bug pattern="SC_START_IN_CTOR" />
+  </Match>
   <Match>
     <Bug pattern="RV_RETURN_VALUE_OF_PUTIFABSENT_IGNORED" />
     <Class name="edu.wpi.first.networktables.NetworkTableInstance" />

wpilibc/src/main/native/cpp/MotorSafety.cpp

@@ -24,7 +24,7 @@ class Thread : public wpi::SafeThread {
 };
 
 void Thread::Main() {
-  wpi::Event event{true, false};
+  wpi::Event event{false, false};
   HAL_ProvideNewDataEventHandle(event.GetHandle());
 
   int safetyCounter = 0;

wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp

@@ -12,6 +12,7 @@ using namespace frc;
 
 void PWMMotorController::Set(double speed) {
   m_pwm.SetSpeed(m_isInverted ? -speed : speed);
+  Feed();
 }
 
 double PWMMotorController::Get() const {
@@ -31,7 +32,8 @@ void PWMMotorController::Disable() {
 }
 
 void PWMMotorController::StopMotor() {
-  Set(0);
+  // Don't use Set(0) as that will feed the watch kitty
+  m_pwm.SetSpeed(0);
 }
 
 std::string PWMMotorController::GetDescription() const {

wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java

@@ -4,6 +4,9 @@
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.util.WPIUtilJNI;
 import java.util.LinkedHashSet;
 import java.util.Set;
 
@@ -24,12 +27,49 @@ public abstract class MotorSafety {
   private final Object m_thisMutex = new Object();
   private static final Set<MotorSafety> m_instanceList = new LinkedHashSet<>();
   private static final Object m_listMutex = new Object();
+  private static Thread m_safetyThread;
+
+  @SuppressWarnings("PMD.AssignmentInOperand")
+  private static void threadMain() {
+    int event = WPIUtilJNI.createEvent(false, false);
+    DriverStationJNI.provideNewDataEventHandle(event);
+    ControlWord controlWord = new ControlWord();
+
+    int safetyCounter = 0;
+    while (true) {
+      boolean timedOut;
+      try {
+        timedOut = WPIUtilJNI.waitForObjectTimeout(event, 0.1);
+      } catch (InterruptedException e) {
+        DriverStationJNI.removeNewDataEventHandle(event);
+        WPIUtilJNI.destroyEvent(event);
+        Thread.currentThread().interrupt();
+        return;
+      }
+      if (!timedOut) {
+        DriverStationJNI.getControlWord(controlWord);
+        if (!(controlWord.getEnabled() && controlWord.getDSAttached())) {
+          safetyCounter = 0;
+        }
+        if (++safetyCounter >= 4) {
+          checkMotors();
+          safetyCounter = 0;
+        }
+      } else {
+        safetyCounter = 0;
+      }
+    }
+  }
 
   /** MotorSafety constructor. */
   public MotorSafety() {
     synchronized (m_listMutex) {
       m_instanceList.add(this);
-      // TODO Threads
+      if (m_safetyThread == null) {
+        m_safetyThread = new Thread(() -> threadMain(), "MotorSafety Thread");
+        m_safetyThread.setDaemon(true);
+        m_safetyThread.start();
+      }
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java

@@ -78,7 +78,8 @@ public abstract class PWMMotorController extends MotorSafety
 
   @Override
   public void stopMotor() {
-    set(0);
+    // Don't use set(0) as that will feed the watch kitty
+    m_pwm.setSpeed(0);
   }
 
   @Override