From 1392db45299d93a20bd9d98942c2d19d29a3fabf Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 23 May 2026 17:38:36 -0700 Subject: [PATCH] [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 | 10 ++ .../main/java/org/wpilib/romi/OnBoardIO.java | 14 +++ .../main/java/org/wpilib/romi/RomiMotor.java | 64 +++++++++-- .../main/java/org/wpilib/romi/RomiServo.java | 105 ++++++++++++++++++ .../src/main/native/cpp/romi/OnBoardIO.cpp | 16 +++ .../src/main/native/cpp/romi/RomiMotor.cpp | 51 ++++++++- .../src/main/native/cpp/romi/RomiServo.cpp | 44 ++++++++ .../native/include/wpi/romi/OnBoardIO.hpp | 5 + .../native/include/wpi/romi/RomiMotor.hpp | 32 ++++-- .../native/include/wpi/romi/RomiServo.hpp | 52 +++++++++ romiVendordep/src/main/python/pyproject.toml | 1 + .../src/main/python/romi/__init__.py | 4 +- .../src/main/python/semiwrap/OnBoardIO.yml | 1 + .../src/main/python/semiwrap/RomiMotor.yml | 7 ++ .../src/main/python/semiwrap/RomiServo.yml | 6 + 15 files changed, 389 insertions(+), 23 deletions(-) create mode 100644 romiVendordep/src/main/java/org/wpilib/romi/RomiServo.java create mode 100644 romiVendordep/src/main/native/cpp/romi/RomiServo.cpp create mode 100644 romiVendordep/src/main/native/include/wpi/romi/RomiServo.hpp create mode 100644 romiVendordep/src/main/python/semiwrap/RomiServo.yml diff --git a/romiVendordep/robotpy_pybind_build_info.bzl b/romiVendordep/robotpy_pybind_build_info.bzl index 9e115333a8..fe5e1b6f75 100644 --- a/romiVendordep/robotpy_pybind_build_info.bzl +++ b/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( diff --git a/romiVendordep/src/main/java/org/wpilib/romi/OnBoardIO.java b/romiVendordep/src/main/java/org/wpilib/romi/OnBoardIO.java index 8dc3fccc82..00e7c0a51c 100644 --- a/romiVendordep/src/main/java/org/wpilib/romi/OnBoardIO.java +++ b/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; + } } diff --git a/romiVendordep/src/main/java/org/wpilib/romi/RomiMotor.java b/romiVendordep/src/main/java/org/wpilib/romi/RomiMotor.java index c6ae05c9eb..c8288e4bfc 100644 --- a/romiVendordep/src/main/java/org/wpilib/romi/RomiMotor.java +++ b/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. * - *

A general use PWM motor controller representing the motors on a Romi robot + *

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: + 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); } } diff --git a/romiVendordep/src/main/java/org/wpilib/romi/RomiServo.java b/romiVendordep/src/main/java/org/wpilib/romi/RomiServo.java new file mode 100644 index 0000000000..49b88254ea --- /dev/null +++ b/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. + * + *

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: + 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; + } +} diff --git a/romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp b/romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp index 1a5c6e1706..162f0635f6 100644 --- a/romiVendordep/src/main/native/cpp/romi/OnBoardIO.cpp +++ b/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; +} diff --git a/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp b/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp index 1686b2cc9b..0f2672b11b 100644 --- a/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp +++ b/romiVendordep/src/main/native/cpp/romi/RomiMotor.cpp @@ -4,9 +4,56 @@ #include "wpi/romi/RomiMotor.hpp" +#include + +#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; +} diff --git a/romiVendordep/src/main/native/cpp/romi/RomiServo.cpp b/romiVendordep/src/main/native/cpp/romi/RomiServo.cpp new file mode 100644 index 0000000000..0eb818a22b --- /dev/null +++ b/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 +#include + +#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(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; +} diff --git a/romiVendordep/src/main/native/include/wpi/romi/OnBoardIO.hpp b/romiVendordep/src/main/native/include/wpi/romi/OnBoardIO.hpp index 4451575710..cb7c2a0fdd 100644 --- a/romiVendordep/src/main/native/include/wpi/romi/OnBoardIO.hpp +++ b/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}; diff --git a/romiVendordep/src/main/native/include/wpi/romi/RomiMotor.hpp b/romiVendordep/src/main/native/include/wpi/romi/RomiMotor.hpp index 92ed28a543..fc6f7c6adf 100644 --- a/romiVendordep/src/main/native/include/wpi/romi/RomiMotor.hpp +++ b/romiVendordep/src/main/native/include/wpi/romi/RomiMotor.hpp @@ -4,7 +4,11 @@ #pragma once -#include "wpi/hardware/motor/PWMMotorController.hpp" +#include + +#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 + *

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; }; /** @} */ diff --git a/romiVendordep/src/main/native/include/wpi/romi/RomiServo.hpp b/romiVendordep/src/main/native/include/wpi/romi/RomiServo.hpp new file mode 100644 index 0000000000..994d0fd553 --- /dev/null +++ b/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. + * + *

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 diff --git a/romiVendordep/src/main/python/pyproject.toml b/romiVendordep/src/main/python/pyproject.toml index 98e1bbbc6b..5a016b74eb 100644 --- a/romiVendordep/src/main/python/pyproject.toml +++ b/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" diff --git a/romiVendordep/src/main/python/romi/__init__.py b/romiVendordep/src/main/python/romi/__init__.py index be1981eb5f..4c8163a2bd 100644 --- a/romiVendordep/src/main/python/romi/__init__.py +++ b/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 diff --git a/romiVendordep/src/main/python/semiwrap/OnBoardIO.yml b/romiVendordep/src/main/python/semiwrap/OnBoardIO.yml index 12c28b5713..94c500a219 100644 --- a/romiVendordep/src/main/python/semiwrap/OnBoardIO.yml +++ b/romiVendordep/src/main/python/semiwrap/OnBoardIO.yml @@ -13,3 +13,4 @@ classes: SetGreenLed: SetRedLed: SetYellowLed: + AllocatePWM: diff --git a/romiVendordep/src/main/python/semiwrap/RomiMotor.yml b/romiVendordep/src/main/python/semiwrap/RomiMotor.yml index 22af2f09a5..5869584e1f 100644 --- a/romiVendordep/src/main/python/semiwrap/RomiMotor.yml +++ b/romiVendordep/src/main/python/semiwrap/RomiMotor.yml @@ -2,3 +2,10 @@ classes: wpi::romi::RomiMotor: methods: RomiMotor: + SetThrottle: + GetThrottle: + SetInverted: + GetInverted: + Disable: + StopMotor: + GetDescription: diff --git a/romiVendordep/src/main/python/semiwrap/RomiServo.yml b/romiVendordep/src/main/python/semiwrap/RomiServo.yml new file mode 100644 index 0000000000..5fa334a0bb --- /dev/null +++ b/romiVendordep/src/main/python/semiwrap/RomiServo.yml @@ -0,0 +1,6 @@ +classes: + wpi::romi::RomiServo: + methods: + RomiServo: + SetAngle: + GetAngle: