Remove MotorSafetyHelper, create MotorSafety base class instead (#562)

Most of the MotorSafety implementation was moved into the MotorSafety base
class. SafePWM's inheritance of MotorSafety was moved into PWM to
eliminate Java needing a helper class.

In Java, a helper class for Sendable (SendableImpl) was added due to
lack of multiple inheritance.

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -19,7 +19,7 @@
 #include <wpi/StringRef.h>
 
 #include "frc/AnalogInput.h"
-#include "frc/MotorSafetyHelper.h"
+#include "frc/MotorSafety.h"
 #include "frc/Timer.h"
 #include "frc/Utility.h"
 #include "frc/WPIErrors.h"
@@ -557,7 +557,7 @@ void DriverStation::Run() {
     if (IsDisabled()) safetyCounter = 0;
 
     if (++safetyCounter >= 4) {
-      MotorSafetyHelper::CheckMotors();
+      MotorSafety::CheckMotors();
       safetyCounter = 0;
     }
     if (m_userInDisabled) HAL_ObserveUserProgramDisabled();

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

@@ -5,58 +5,80 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "frc/MotorSafetyHelper.h"
+#include "frc/MotorSafety.h"
+
+#include <algorithm>
+#include <utility>
 
 #include <wpi/SmallPtrSet.h>
 #include <wpi/SmallString.h>
 #include <wpi/raw_ostream.h>
 
 #include "frc/DriverStation.h"
-#include "frc/MotorSafety.h"
-#include "frc/Timer.h"
 #include "frc/WPIErrors.h"
 
 using namespace frc;
 
-static wpi::SmallPtrSet<MotorSafetyHelper*, 32> helperList;
+static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
 static wpi::mutex listMutex;
 
-MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject)
-    : m_safeObject(safeObject) {
-  m_enabled = false;
-  m_expiration = DEFAULT_SAFETY_EXPIRATION;
-  m_stopTime = Timer::GetFPGATimestamp();
-
+MotorSafety::MotorSafety() {
   std::lock_guard<wpi::mutex> lock(listMutex);
-  helperList.insert(this);
+  instanceList.insert(this);
 }
 
-MotorSafetyHelper::~MotorSafetyHelper() {
+MotorSafety::~MotorSafety() {
   std::lock_guard<wpi::mutex> lock(listMutex);
-  helperList.erase(this);
+  instanceList.erase(this);
 }
 
-void MotorSafetyHelper::Feed() {
+MotorSafety::MotorSafety(MotorSafety&& rhs)
+    : ErrorBase(std::move(rhs)),
+      m_expiration(std::move(rhs.m_expiration)),
+      m_enabled(std::move(rhs.m_enabled)),
+      m_stopTime(std::move(rhs.m_stopTime)) {}
+
+MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
+  ErrorBase::operator=(std::move(rhs));
+
+  m_expiration = std::move(rhs.m_expiration);
+  m_enabled = std::move(rhs.m_enabled);
+  m_stopTime = std::move(rhs.m_stopTime);
+
+  return *this;
+}
+
+void MotorSafety::Feed() {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
 }
 
-void MotorSafetyHelper::SetExpiration(double expirationTime) {
+void MotorSafety::SetExpiration(double expirationTime) {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
   m_expiration = expirationTime;
 }
 
-double MotorSafetyHelper::GetExpiration() const {
+double MotorSafety::GetExpiration() const {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return m_expiration;
 }
 
-bool MotorSafetyHelper::IsAlive() const {
+bool MotorSafety::IsAlive() const {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
   return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
 }
 
-void MotorSafetyHelper::Check() {
+void MotorSafety::SetSafetyEnabled(bool enabled) {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  m_enabled = enabled;
+}
+
+bool MotorSafety::IsSafetyEnabled() const {
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  return m_enabled;
+}
+
+void MotorSafety::Check() {
   bool enabled;
   double stopTime;
 
@@ -67,31 +89,24 @@ void MotorSafetyHelper::Check() {
   }
 
   DriverStation& ds = DriverStation::GetInstance();
-  if (!enabled || ds.IsDisabled() || ds.IsTest()) return;
+  if (!enabled || ds.IsDisabled() || ds.IsTest()) {
+    return;
+  }
 
+  std::lock_guard<wpi::mutex> lock(m_thisMutex);
   if (stopTime < Timer::GetFPGATimestamp()) {
     wpi::SmallString<128> buf;
     wpi::raw_svector_ostream desc(buf);
-    m_safeObject->GetDescription(desc);
+    GetDescription(desc);
     desc << "... Output not updated often enough.";
     wpi_setWPIErrorWithContext(Timeout, desc.str());
-    m_safeObject->StopMotor();
+    StopMotor();
   }
 }
 
-void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
-  std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  m_enabled = enabled;
-}
-
-bool MotorSafetyHelper::IsSafetyEnabled() const {
-  std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  return m_enabled;
-}
-
-void MotorSafetyHelper::CheckMotors() {
+void MotorSafety::CheckMotors() {
   std::lock_guard<wpi::mutex> lock(listMutex);
-  for (auto elem : helperList) {
+  for (auto elem : instanceList) {
     elem->Check();
   }
 }

wpilibc/src/main/native/cpp/NidecBrushless.cpp

@@ -14,11 +14,11 @@
 using namespace frc;
 
 NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
-    : m_safetyHelper(this), m_dio(dioChannel), m_pwm(pwmChannel) {
+    : m_dio(dioChannel), m_pwm(pwmChannel) {
   AddChild(&m_dio);
   AddChild(&m_pwm);
-  m_safetyHelper.SetExpiration(0.0);
-  m_safetyHelper.SetSafetyEnabled(false);
+  SetExpiration(0.0);
+  SetSafetyEnabled(false);
 
   // the dio controls the output (in PWM mode)
   m_dio.SetPWMRate(15625);
@@ -34,7 +34,7 @@ void NidecBrushless::Set(double speed) {
     m_dio.UpdateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
     m_pwm.SetRaw(0xffff);
   }
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 double NidecBrushless::Get() const { return m_speed; }
@@ -49,31 +49,13 @@ void NidecBrushless::Disable() {
   m_pwm.SetDisabled();
 }
 
-void NidecBrushless::StopMotor() {
-  m_dio.UpdateDutyCycle(0.5);
-  m_pwm.SetDisabled();
-}
-
 void NidecBrushless::Enable() { m_disabled = false; }
 
 void NidecBrushless::PIDWrite(double output) { Set(output); }
 
-void NidecBrushless::SetExpiration(double timeout) {
-  m_safetyHelper.SetExpiration(timeout);
-}
-
-double NidecBrushless::GetExpiration() const {
-  return m_safetyHelper.GetExpiration();
-}
-
-bool NidecBrushless::IsAlive() const { return m_safetyHelper.IsAlive(); }
-
-void NidecBrushless::SetSafetyEnabled(bool enabled) {
-  m_safetyHelper.SetSafetyEnabled(enabled);
-}
-
-bool NidecBrushless::IsSafetyEnabled() const {
-  return m_safetyHelper.IsSafetyEnabled();
+void NidecBrushless::StopMotor() {
+  m_dio.UpdateDutyCycle(0.5);
+  m_pwm.SetDisabled();
 }
 
 void NidecBrushless::GetDescription(wpi::raw_ostream& desc) const {

wpilibc/src/main/native/cpp/PWM.cpp

@@ -47,6 +47,8 @@ PWM::PWM(int channel) {
 
   HAL_Report(HALUsageReporting::kResourceType_PWM, channel);
   SetName("PWM", channel);
+
+  SetSafetyEnabled(false);
 }
 
 PWM::~PWM() {
@@ -60,7 +62,7 @@ PWM::~PWM() {
 }
 
 PWM::PWM(PWM&& rhs)
-    : ErrorBase(std::move(rhs)),
+    : MotorSafety(std::move(rhs)),
       SendableBase(std::move(rhs)),
       m_channel(std::move(rhs.m_channel)) {
   std::swap(m_handle, rhs.m_handle);
@@ -76,6 +78,12 @@ PWM& PWM::operator=(PWM&& rhs) {
   return *this;
 }
 
+void PWM::StopMotor() { SetDisabled(); }
+
+void PWM::GetDescription(wpi::raw_ostream& desc) const {
+  desc << "PWM " << GetChannel();
+}
+
 void PWM::SetRaw(uint16_t value) {
   if (StatusIsFatal()) return;
 
@@ -114,6 +122,8 @@ void PWM::SetSpeed(double speed) {
   int32_t status = 0;
   HAL_SetPWMSpeed(m_handle, speed, &status);
   wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+  Feed();
 }
 
 double PWM::GetSpeed() const {

wpilibc/src/main/native/cpp/PWMSpeedController.cpp

@@ -25,11 +25,11 @@ bool PWMSpeedController::GetInverted() const { return m_isInverted; }
 
 void PWMSpeedController::Disable() { SetDisabled(); }
 
-void PWMSpeedController::StopMotor() { SafePWM::StopMotor(); }
+void PWMSpeedController::StopMotor() { PWM::StopMotor(); }
 
 void PWMSpeedController::PIDWrite(double output) { Set(output); }
 
-PWMSpeedController::PWMSpeedController(int channel) : SafePWM(channel) {}
+PWMSpeedController::PWMSpeedController(int channel) : PWM(channel) {}
 
 void PWMSpeedController::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Speed Controller");

wpilibc/src/main/native/cpp/Relay.cpp

@@ -14,7 +14,6 @@
 #include <hal/Relay.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/MotorSafetyHelper.h"
 #include "frc/SensorUtil.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -77,9 +76,6 @@ Relay::Relay(int channel, Relay::Direction direction)
     }
   }
 
-  m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
-  m_safetyHelper->SetSafetyEnabled(false);
-
   SetName("Relay", m_channel);
 }
 
@@ -94,25 +90,21 @@ Relay::~Relay() {
 
 Relay::Relay(Relay&& rhs)
     : MotorSafety(std::move(rhs)),
-      ErrorBase(std::move(rhs)),
       SendableBase(std::move(rhs)),
       m_channel(std::move(rhs.m_channel)),
-      m_direction(std::move(rhs.m_direction)),
-      m_safetyHelper(std::move(rhs.m_safetyHelper)) {
+      m_direction(std::move(rhs.m_direction)) {
   std::swap(m_forwardHandle, rhs.m_forwardHandle);
   std::swap(m_reverseHandle, rhs.m_reverseHandle);
 }
 
 Relay& Relay::operator=(Relay&& rhs) {
   MotorSafety::operator=(std::move(rhs));
-  ErrorBase::operator=(std::move(rhs));
   SendableBase::operator=(std::move(rhs));
 
   m_channel = std::move(rhs.m_channel);
   m_direction = std::move(rhs.m_direction);
   std::swap(m_forwardHandle, rhs.m_forwardHandle);
   std::swap(m_reverseHandle, rhs.m_reverseHandle);
-  m_safetyHelper = std::move(rhs.m_safetyHelper);
 
   return *this;
 }
@@ -204,24 +196,8 @@ Relay::Value Relay::Get() const {
 
 int Relay::GetChannel() const { return m_channel; }
 
-void Relay::SetExpiration(double timeout) {
-  m_safetyHelper->SetExpiration(timeout);
-}
-
-double Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
-
-bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); }
-
 void Relay::StopMotor() { Set(kOff); }
 
-void Relay::SetSafetyEnabled(bool enabled) {
-  m_safetyHelper->SetSafetyEnabled(enabled);
-}
-
-bool Relay::IsSafetyEnabled() const {
-  return m_safetyHelper->IsSafetyEnabled();
-}
-
 void Relay::GetDescription(wpi::raw_ostream& desc) const {
   desc << "Relay " << GetChannel();
 }

wpilibc/src/main/native/cpp/RobotDrive.cpp

@@ -298,7 +298,7 @@ void RobotDrive::MecanumDrive_Cartesian(double x, double y, double rotation,
   m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
   m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
 
-  m_safetyHelper->Feed();
+  Feed();
 }
 
 void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
@@ -330,7 +330,7 @@ void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
   m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
   m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
 
-  m_safetyHelper->Feed();
+  Feed();
 }
 
 void RobotDrive::HolonomicDrive(double magnitude, double direction,
@@ -350,7 +350,7 @@ void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
     m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
   m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
 
-  m_safetyHelper->Feed();
+  Feed();
 }
 
 void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
@@ -380,40 +380,19 @@ void RobotDrive::SetSensitivity(double sensitivity) {
 
 void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
 
-void RobotDrive::SetExpiration(double timeout) {
-  m_safetyHelper->SetExpiration(timeout);
+void RobotDrive::GetDescription(wpi::raw_ostream& desc) const {
+  desc << "RobotDrive";
 }
 
-double RobotDrive::GetExpiration() const {
-  return m_safetyHelper->GetExpiration();
-}
-
-bool RobotDrive::IsAlive() const { return m_safetyHelper->IsAlive(); }
-
 void RobotDrive::StopMotor() {
   if (m_frontLeftMotor != nullptr) m_frontLeftMotor->StopMotor();
   if (m_frontRightMotor != nullptr) m_frontRightMotor->StopMotor();
   if (m_rearLeftMotor != nullptr) m_rearLeftMotor->StopMotor();
   if (m_rearRightMotor != nullptr) m_rearRightMotor->StopMotor();
-  m_safetyHelper->Feed();
+  Feed();
 }
 
-bool RobotDrive::IsSafetyEnabled() const {
-  return m_safetyHelper->IsSafetyEnabled();
-}
-
-void RobotDrive::SetSafetyEnabled(bool enabled) {
-  m_safetyHelper->SetSafetyEnabled(enabled);
-}
-
-void RobotDrive::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "RobotDrive";
-}
-
-void RobotDrive::InitRobotDrive() {
-  m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
-  m_safetyHelper->SetSafetyEnabled(true);
-}
+void RobotDrive::InitRobotDrive() { SetSafetyEnabled(true); }
 
 double RobotDrive::Limit(double number) {
   if (number > 1.0) {

wpilibc/src/main/native/cpp/SafePWM.cpp

@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 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 "frc/SafePWM.h"
-
-using namespace frc;
-
-SafePWM::SafePWM(int channel) : PWM(channel) {
-  m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
-  m_safetyHelper->SetSafetyEnabled(false);
-}
-
-void SafePWM::SetExpiration(double timeout) {
-  m_safetyHelper->SetExpiration(timeout);
-}
-
-double SafePWM::GetExpiration() const {
-  return m_safetyHelper->GetExpiration();
-}
-
-bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); }
-
-void SafePWM::StopMotor() { SetDisabled(); }
-
-void SafePWM::SetSafetyEnabled(bool enabled) {
-  m_safetyHelper->SetSafetyEnabled(enabled);
-}
-
-bool SafePWM::IsSafetyEnabled() const {
-  return m_safetyHelper->IsSafetyEnabled();
-}
-
-void SafePWM::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "PWM " << GetChannel();
-}
-
-void SafePWM::SetSpeed(double speed) {
-  PWM::SetSpeed(speed);
-  m_safetyHelper->Feed();
-}

wpilibc/src/main/native/cpp/Servo.cpp

@@ -19,7 +19,7 @@ constexpr double Servo::kMinServoAngle;
 constexpr double Servo::kDefaultMaxServoPWM;
 constexpr double Servo::kDefaultMinServoPWM;
 
-Servo::Servo(int channel) : SafePWM(channel) {
+Servo::Servo(int channel) : PWM(channel) {
   // Set minimum and maximum PWM values supported by the servo
   SetBounds(kDefaultMaxServoPWM, 0.0, 0.0, 0.0, kDefaultMinServoPWM);
 

wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp

@@ -79,7 +79,7 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
   m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
                    m_rightSideInvertMultiplier);
 
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
@@ -152,7 +152,7 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
   m_rightMotor.Set(rightMotorOutput * m_maxOutput *
                    m_rightSideInvertMultiplier);
 
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
@@ -180,7 +180,7 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
   m_leftMotor.Set(leftSpeed * m_maxOutput);
   m_rightMotor.Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
 
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void DifferentialDrive::SetQuickStopThreshold(double threshold) {
@@ -202,7 +202,7 @@ void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
 void DifferentialDrive::StopMotor() {
   m_leftMotor.StopMotor();
   m_rightMotor.StopMotor();
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void DifferentialDrive::GetDescription(wpi::raw_ostream& desc) const {

wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp

@@ -73,7 +73,7 @@ void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
   m_rightMotor.Set(wheelSpeeds[kRight] * m_maxOutput);
   m_backMotor.Set(wheelSpeeds[kBack] * m_maxOutput);
 
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void KilloughDrive::DrivePolar(double magnitude, double angle,
@@ -92,7 +92,7 @@ void KilloughDrive::StopMotor() {
   m_leftMotor.StopMotor();
   m_rightMotor.StopMotor();
   m_backMotor.StopMotor();
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void KilloughDrive::GetDescription(wpi::raw_ostream& desc) const {

wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp

@@ -70,7 +70,7 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
   m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput *
                        m_rightSideInvertMultiplier);
 
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void MecanumDrive::DrivePolar(double magnitude, double angle,
@@ -98,7 +98,7 @@ void MecanumDrive::StopMotor() {
   m_frontRightMotor.StopMotor();
   m_rearLeftMotor.StopMotor();
   m_rearRightMotor.StopMotor();
-  m_safetyHelper.Feed();
+  Feed();
 }
 
 void MecanumDrive::GetDescription(wpi::raw_ostream& desc) const {

wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp

@@ -18,31 +18,13 @@
 
 using namespace frc;
 
-RobotDriveBase::RobotDriveBase() { m_safetyHelper.SetSafetyEnabled(true); }
+RobotDriveBase::RobotDriveBase() { SetSafetyEnabled(true); }
 
 void RobotDriveBase::SetDeadband(double deadband) { m_deadband = deadband; }
 
 void RobotDriveBase::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
 
-void RobotDriveBase::FeedWatchdog() { m_safetyHelper.Feed(); }
-
-void RobotDriveBase::SetExpiration(double timeout) {
-  m_safetyHelper.SetExpiration(timeout);
-}
-
-double RobotDriveBase::GetExpiration() const {
-  return m_safetyHelper.GetExpiration();
-}
-
-bool RobotDriveBase::IsAlive() const { return m_safetyHelper.IsAlive(); }
-
-bool RobotDriveBase::IsSafetyEnabled() const {
-  return m_safetyHelper.IsSafetyEnabled();
-}
-
-void RobotDriveBase::SetSafetyEnabled(bool enabled) {
-  m_safetyHelper.SetSafetyEnabled(enabled);
-}
+void RobotDriveBase::FeedWatchdog() { Feed(); }
 
 double RobotDriveBase::Limit(double value) {
   if (value > 1.0) {