[romi] Fix Romi motor support (#8915)

Since Servo has been removed from WPILib, add a RomiServo to replace it
(similar to XRPServo).

For Romi, move allocation of PWM channels to OnBoardIO since Motor and
Servo share channels.

romiVendordep/robotpy_pybind_build_info.bzl

@@ -37,6 +37,16 @@ def romi_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includes
                 ("wpi::romi::RomiMotor", "wpi__romi__RomiMotor.hpp"),
             ],
         ),
+        struct(
+            class_name = "RomiServo",
+            yml_file = "semiwrap/RomiServo.yml",
+            header_root = "$(execpath :robotpy-native-romi.copy_headers)",
+            header_file = "$(execpath :robotpy-native-romi.copy_headers)/wpi/romi/RomiServo.hpp",
+            tmpl_class_names = [],
+            trampolines = [
+                ("wpi::romi::RomiServo", "wpi__romi__RomiServo.hpp"),
+            ],
+        ),
     ]
 
     resolve_casters(

romiVendordep/src/main/java/org/wpilib/romi/OnBoardIO.java

@@ -150,4 +150,18 @@ public class OnBoardIO {
   public void setYellowLed(boolean value) {
     m_yellowLed.set(value);
   }
+
+  private static final boolean[] s_allocatedPWMs = new boolean[4];
+
+  static void allocatePWM(int deviceNum) {
+    if (deviceNum < 0 || deviceNum > 3) {
+      throw new IllegalArgumentException("Invalid Romi PWM output. Should be 0-3");
+    }
+
+    if (s_allocatedPWMs[deviceNum]) {
+      throw new IllegalArgumentException("Romi PWM output " + deviceNum + " already allocated");
+    }
+
+    s_allocatedPWMs[deviceNum] = true;
+  }
 }

romiVendordep/src/main/java/org/wpilib/romi/RomiMotor.java

@@ -4,19 +4,19 @@
 
 package org.wpilib.romi;
 
-import org.wpilib.hardware.motor.PWMMotorController;
+import org.wpilib.hardware.hal.SimDevice;
+import org.wpilib.hardware.hal.SimDevice.Direction;
+import org.wpilib.hardware.hal.SimDouble;
+import org.wpilib.hardware.motor.MotorController;
 
 /**
  * RomiMotor.
  *
- * <p>A general use PWM motor controller representing the motors on a Romi robot
+ * <p>A SimDevice based motor controller representing the motors on a Romi robot
  */
-public class RomiMotor extends PWMMotorController {
-  /** Common initialization code called by all constructors. */
-  protected final void initRomiMotor() {
-    m_pwm.setOutputPeriod(5);
-    setThrottle(0.0);
-  }
+public class RomiMotor implements MotorController {
+  private final SimDouble m_simSpeed;
+  private boolean m_inverted;
 
   /**
    * Constructor.
@@ -24,9 +24,49 @@ public class RomiMotor extends PWMMotorController {
    * @param channel The PWM channel that the RomiMotor is attached to. 0 is the left motor, 1 is the
    *     right.
    */
-  @SuppressWarnings("this-escape")
-  public RomiMotor(final int channel) {
-    super("Romi Motor", channel);
-    initRomiMotor();
+  public RomiMotor(int channel) {
+    OnBoardIO.allocatePWM(channel);
+
+    // We want this to appear on the WS messages as type: "PWM", device: <channel>
+    String simDeviceName = "PWM:" + channel;
+    SimDevice romiMotorSimDevice = SimDevice.create(simDeviceName);
+
+    if (romiMotorSimDevice != null) {
+      romiMotorSimDevice.createBoolean("init", Direction.OUTPUT, true);
+      m_simSpeed = romiMotorSimDevice.createDouble("speed", Direction.OUTPUT, 0.0);
+    } else {
+      m_simSpeed = null;
+    }
+  }
+
+  @Override
+  public void setThrottle(double throttle) {
+    if (m_simSpeed != null) {
+      m_simSpeed.set(m_inverted ? -throttle : throttle);
+    }
+  }
+
+  @Override
+  public double getThrottle() {
+    if (m_simSpeed != null) {
+      return m_simSpeed.get();
+    }
+
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_inverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_inverted;
+  }
+
+  @Override
+  public void disable() {
+    setThrottle(0.0);
   }
 }

romiVendordep/src/main/java/org/wpilib/romi/RomiServo.java

@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.romi;
+
+import org.wpilib.hardware.hal.SimDevice;
+import org.wpilib.hardware.hal.SimDevice.Direction;
+import org.wpilib.hardware.hal.SimDouble;
+
+/**
+ * RomiServo.
+ *
+ * <p>A SimDevice based servo
+ */
+public class RomiServo {
+  private final SimDouble m_simPosition;
+
+  /**
+   * Constructs a RomiServo.
+   *
+   * @param channel the PWM channel the servo is attached to.
+   */
+  public RomiServo(int channel) {
+    OnBoardIO.allocatePWM(channel);
+
+    // We want this to appear on WS as type: "PWM", device: <channel>
+    String simDeviceName = "PWM:" + channel;
+    SimDevice romiServoSimDevice = SimDevice.create(simDeviceName);
+
+    if (romiServoSimDevice != null) {
+      romiServoSimDevice.createBoolean("init", Direction.OUTPUT, true);
+      // This should mimic PWM position [0.0, 1.0]
+      m_simPosition = romiServoSimDevice.createDouble("position", Direction.OUTPUT, 0.5);
+    } else {
+      m_simPosition = null;
+    }
+  }
+
+  /**
+   * Set the servo angle.
+   *
+   * @param angleDegrees Desired angle in degrees
+   */
+  public void setAngle(double angleDegrees) {
+    if (angleDegrees < 0.0) {
+      angleDegrees = 0.0;
+    }
+
+    if (angleDegrees > 180.0) {
+      angleDegrees = 180.0;
+    }
+
+    double pos = angleDegrees / 180.0;
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo angle.
+   *
+   * @return Current servo angle
+   */
+  public double getAngle() {
+    if (m_simPosition != null) {
+      return m_simPosition.get() * 180.0;
+    }
+
+    return 90.0;
+  }
+
+  /**
+   * Set the servo position.
+   *
+   * @param position Desired position (Between 0.0 and 1.0)
+   */
+  public void setPosition(double position) {
+    if (position < 0.0) {
+      position = 0.0;
+    }
+
+    if (position > 1.0) {
+      position = 1.0;
+    }
+
+    if (m_simPosition != null) {
+      m_simPosition.set(position);
+    }
+  }
+
+  /**
+   * Get the servo position.
+   *
+   * @return Current servo position
+   */
+  public double getPosition() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    return 0.5;
+  }
+}

romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp

@@ -83,3 +83,19 @@ void OnBoardIO::SetRedLed(bool value) {
 void OnBoardIO::SetYellowLed(bool value) {
   m_yellowLed.Set(value);
 }
+
+void OnBoardIO::AllocatePWM(int channel) {
+  if (channel < 0 || channel > 3) {
+    throw WPILIB_MakeError(wpi::err::ChannelIndexOutOfRange, "Channel {}",
+                           channel);
+  }
+
+  static bool allocatedChannels[4] = {false, false, false, false};
+
+  if (allocatedChannels[channel]) {
+    throw WPILIB_MakeError(wpi::err::ResourceAlreadyAllocated, "Channel {}",
+                           channel);
+  }
+
+  allocatedChannels[channel] = true;
+}

romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp

@@ -4,9 +4,56 @@
 
 #include "wpi/romi/RomiMotor.hpp"
 
+#include <string>
+
+#include "fmt/format.h"
+#include "wpi/romi/OnBoardIO.hpp"
+
 using namespace wpi::romi;
 
-RomiMotor::RomiMotor(int channel) : PWMMotorController("Romi Motor", channel) {
-  m_pwm.SetOutputPeriod(5_ms);
+RomiMotor::RomiMotor(int channel) {
+  OnBoardIO::AllocatePWM(channel);
+
+  m_deviceName = fmt::format("PWM:{}", channel);
+  m_simDevice = hal::SimDevice(m_deviceName.c_str());
+
+  if (m_simDevice) {
+    m_simDevice.CreateBoolean("init", hal::SimDevice::Direction::OUTPUT, true);
+    m_simSpeed = m_simDevice.CreateDouble(
+        "speed", hal::SimDevice::Direction::OUTPUT, 0.0);
+  }
+}
+
+void RomiMotor::SetThrottle(double throttle) {
+  if (m_simSpeed) {
+    m_simSpeed.Set(m_inverted ? -throttle : throttle);
+  }
+}
+
+double RomiMotor::GetThrottle() const {
+  if (m_simSpeed) {
+    return m_simSpeed.Get();
+  }
+
+  return 0.0;
+}
+
+void RomiMotor::SetInverted(bool isInverted) {
+  m_inverted = isInverted;
+}
+
+bool RomiMotor::GetInverted() const {
+  return m_inverted;
+}
+
+void RomiMotor::Disable() {
   SetThrottle(0.0);
 }
+
+void RomiMotor::StopMotor() {
+  SetThrottle(0.0);
+}
+
+std::string RomiMotor::GetDescription() const {
+  return m_deviceName;
+}

romiVendordep/src/main/native/cpp/romi/RomiServo.cpp

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/romi/RomiServo.hpp"
+
+#include <numbers>
+#include <string>
+
+#include "fmt/format.h"
+#include "wpi/romi/OnBoardIO.hpp"
+#include "wpi/units/angle.hpp"
+
+using namespace wpi::romi;
+
+RomiServo::RomiServo(int channel) {
+  OnBoardIO::AllocatePWM(channel);
+
+  std::string deviceName = fmt::format("PWM:{}", channel);
+  m_simDevice = hal::SimDevice(deviceName.c_str());
+
+  if (m_simDevice) {
+    m_simDevice.CreateBoolean("init", hal::SimDevice::Direction::OUTPUT, true);
+    m_simPosition = m_simDevice.CreateDouble(
+        "position", hal::SimDevice::Direction::OUTPUT, 0.5);
+  }
+}
+
+void RomiServo::SetAngle(wpi::units::radian_t angle) {
+  angle = std::clamp<wpi::units::radian_t>(angle, 0_deg, 180_deg);
+  double pos = angle.value() / std::numbers::pi;
+
+  if (m_simPosition) {
+    m_simPosition.Set(pos);
+  }
+}
+
+wpi::units::radian_t RomiServo::GetAngle() const {
+  if (m_simPosition) {
+    return wpi::units::radian_t{m_simPosition.Get() * std::numbers::pi};
+  }
+
+  return 90_deg;
+}

romiVendordep/src/main/native/include/wpi/romi/OnBoardIO.hpp

@@ -66,6 +66,11 @@ class OnBoardIO {
    */
   void SetYellowLed(bool value);
 
+  /**
+   * Allocates a PWM output.
+   */
+  static void AllocatePWM(int channel);
+
  private:
   wpi::DigitalInput m_buttonA{0};
   wpi::DigitalOutput m_yellowLed{3};

romiVendordep/src/main/native/include/wpi/romi/RomiMotor.hpp

@@ -4,7 +4,11 @@
 
 #pragma once
 
-#include "wpi/hardware/motor/PWMMotorController.hpp"
+#include <string>
+
+#include "wpi/hal/SimDevice.hpp"
+#include "wpi/hardware/motor/MotorController.hpp"
+#include "wpi/hardware/motor/MotorSafety.hpp"
 
 namespace wpi::romi {
 
@@ -14,22 +18,36 @@ namespace wpi::romi {
  */
 
 /**
- * RomiMotor
+ * RomiMotor.
  *
- * A general use PWM motor controller representing the motors on a Romi robot
+ * <p>A SimDevice based motor controller representing the motors on a Romi robot
  */
-class RomiMotor : public wpi::PWMMotorController {
+class RomiMotor : public wpi::MotorController, public wpi::MotorSafety {
  public:
   /**
-   * Constructor for a RomiMotor.
+   * Constructs a RomiMotor.
    *
    * @param channel The PWM channel that the RomiMotor is attached to.
    *                0 is left, 1 is right
    */
   explicit RomiMotor(int channel);
 
-  RomiMotor(RomiMotor&&) = default;
-  RomiMotor& operator=(RomiMotor&&) = default;
+  void SetThrottle(double throttle) override;
+  double GetThrottle() const override;
+
+  void SetInverted(bool isInverted) override;
+  bool GetInverted() const override;
+
+  void Disable() override;
+
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+ private:
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simSpeed;
+  std::string m_deviceName;
+  bool m_inverted = false;
 };
 
 /** @} */

romiVendordep/src/main/native/include/wpi/romi/RomiServo.hpp

@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/hal/SimDevice.hpp"
+#include "wpi/units/angle.hpp"
+
+namespace wpi::romi {
+
+/**
+ * @ingroup romi_api
+ * @{
+ */
+
+/**
+ * RomiServo.
+ *
+ * <p>A SimDevice based servo
+ */
+class RomiServo {
+ public:
+  /**
+   * Constructs a RomiServo.
+   *
+   * @param channel the servo channel
+   */
+  explicit RomiServo(int channel);
+
+  /**
+   * Set the servo angle.
+   *
+   * @param angle Desired angle in radians
+   */
+  void SetAngle(wpi::units::radian_t angle);
+
+  /**
+   * Get the servo angle.
+   *
+   * @return Current servo angle in radians
+   */
+  wpi::units::radian_t GetAngle() const;
+
+ private:
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simPosition;
+};
+
+/** @} */
+
+}  // namespace wpi::romi

romiVendordep/src/main/python/pyproject.toml

@@ -58,3 +58,4 @@ depends = [
 OnBoardIO = "wpi/romi/OnBoardIO.hpp"
 RomiGyro = "wpi/romi/RomiGyro.hpp"
 RomiMotor = "wpi/romi/RomiMotor.hpp"
+RomiServo = "wpi/romi/RomiServo.hpp"

romiVendordep/src/main/python/romi/__init__.py

@@ -1,8 +1,8 @@
 from . import _init__romi
 
 # autogenerated by 'semiwrap create-imports romi romi._romi'
-from ._romi import OnBoardIO, RomiGyro, RomiMotor
+from ._romi import OnBoardIO, RomiGyro, RomiMotor, RomiServo
 
-__all__ = ["OnBoardIO", "RomiGyro", "RomiMotor"]
+__all__ = ["OnBoardIO", "RomiGyro", "RomiMotor", "RomiServo"]
 
 del _init__romi

romiVendordep/src/main/python/semiwrap/OnBoardIO.yml

@@ -13,3 +13,4 @@ classes:
       SetGreenLed:
       SetRedLed:
       SetYellowLed:
+      AllocatePWM:

romiVendordep/src/main/python/semiwrap/RomiMotor.yml

@@ -2,3 +2,10 @@ classes:
   wpi::romi::RomiMotor:
     methods:
       RomiMotor:
+      SetThrottle:
+      GetThrottle:
+      SetInverted:
+      GetInverted:
+      Disable:
+      StopMotor:
+      GetDescription:

romiVendordep/src/main/python/semiwrap/RomiServo.yml

@@ -0,0 +1,6 @@
+classes:
+  wpi::romi::RomiServo:
+    methods:
+      RomiServo:
+      SetAngle:
+      GetAngle: