diff --git a/settings.gradle b/settings.gradle index f6d097cbfb..19cfba147d 100644 --- a/settings.gradle +++ b/settings.gradle @@ -45,6 +45,7 @@ include 'simulation:halsim_gui' include 'simulation:halsim_ws_core' include 'simulation:halsim_ws_client' include 'simulation:halsim_ws_server' +include 'simulation:halsim_xrp' include 'cameraserver' include 'cameraserver:multiCameraServer' include 'wpilibNewCommands' diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index 7a54c48c50..0a1a953543 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -5,3 +5,4 @@ add_subdirectory(halsim_ds_socket) add_subdirectory(halsim_ws_core) add_subdirectory(halsim_ws_client) add_subdirectory(halsim_ws_server) +add_subdirectory(halsim_xrp) diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_HAL.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_HAL.cpp new file mode 100644 index 0000000000..4010a85ace --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_HAL.cpp @@ -0,0 +1,59 @@ +// 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 "WSProvider_HAL.h" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace wpilibws { + +void HALSimWSProviderHAL::Initialize(WSRegisterFunc webRegisterFunc) { + CreateSingleProvider("HAL", webRegisterFunc); +} + +HALSimWSProviderHAL::~HALSimWSProviderHAL() { + DoCancelCallbacks(); +} + +void HALSimWSProviderHAL::RegisterCallbacks() { + m_simPeriodicBeforeCbKey = HALSIM_RegisterSimPeriodicBeforeCallback( + [](void* param) { + static_cast(param)->ProcessHalCallback( + {{">sim_periodic_before", true}}); + }, + this); + + m_simPeriodicAfterCbKey = HALSIM_RegisterSimPeriodicAfterCallback( + [](void* param) { + static_cast(param)->ProcessHalCallback( + {{">sim_periodic_after", true}}); + }, + this); +} + +void HALSimWSProviderHAL::CancelCallbacks() { + DoCancelCallbacks(); +} + +void HALSimWSProviderHAL::DoCancelCallbacks() { + HALSIM_CancelSimPeriodicBeforeCallback(m_simPeriodicBeforeCbKey); + HALSIM_CancelSimPeriodicAfterCallback(m_simPeriodicAfterCbKey); + + m_simPeriodicBeforeCbKey = 0; + m_simPeriodicAfterCbKey = 0; +} + +void HALSimWSProviderHAL::OnNetValueChanged(const wpi::json& json) { + // no-op. This is all one way +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_HAL.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_HAL.h new file mode 100644 index 0000000000..05ea722a22 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_HAL.h @@ -0,0 +1,32 @@ +// 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 + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderHAL : public HALSimWSHalProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalProvider::HALSimWSHalProvider; + ~HALSimWSProviderHAL() override; + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + void DoCancelCallbacks(); + + private: + int32_t m_simPeriodicBeforeCbKey = 0; + int32_t m_simPeriodicAfterCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_xrp/CMakeLists.txt b/simulation/halsim_xrp/CMakeLists.txt new file mode 100644 index 0000000000..cc83874440 --- /dev/null +++ b/simulation/halsim_xrp/CMakeLists.txt @@ -0,0 +1,16 @@ +project(halsim_xrp) + +include(CompileWarnings) + +file(GLOB halsim_xrp_src src/main/native/cpp/*.cpp) + +add_library(halsim_xrp SHARED ${halsim_xrp_src}) +wpilib_target_warnings(halsim_xrp) +set_target_properties(halsim_xrp PROPERTIES DEBUG_POSTFIX "d") +target_link_libraries(halsim_xrp PUBLIC hal halsim_ws_core) + +target_include_directories(halsim_xrp PRIVATE src/main/native/include) + +set_property(TARGET halsim_xrp PROPERTY FOLDER "libraries") + +install(TARGETS halsim_xrp EXPORT halsim_xrp DESTINATION "${main_lib_dest}") diff --git a/simulation/halsim_xrp/README.md b/simulation/halsim_xrp/README.md new file mode 100644 index 0000000000..79d6d525c4 --- /dev/null +++ b/simulation/halsim_xrp/README.md @@ -0,0 +1,125 @@ +# HAL XRP Client + +This is an extension that provides a client version of the XRP protocol for transmitting robot hardware interface state to an XRP robot over UDP. + +## Configuration + +The XRP client has a number of configuration options available through environment variables. + +``HALSIMXRP_HOST``: The host to connect to. Defaults to localhost. + +``HALSIMXRP_PORT``: The port number to connect to. Defaults to 3540. + +## XRP Protocol + +The WPILib -> XRP protocol is binary-based to save on bandwidth due to hardware limitations of the XRP robot. The messages to/from the XRP follow a the format below: + +| 2 bytes | 1 byte | n bytes | +|---------------------|-------------------|-------------------------------------| +| _uint16_t_ sequence | _uint8_t_ control | [<Tagged Data>](#tagged-data) | + +### Control Byte + +The control byte is used to indicate the current `enabled` state of the WPILib robot code. When this is set to `1`, the robot is enabled, and when it is set to `0` it is disabled. + +Messages originating from the XRP have an unspecified value for the control byte. + +### Tagged Data + +The `Tagged Data` section can contain an arbitrary number of data blocks. Each block has the format below: + +| 1 byte | 1 byte | n bytes | +|----------------|-----------------|-----------------| +| _uint8_t_ size | _uint8_t_ tagID | <payload> | + +The `size` byte encodes the size of the data block, _excluding_ itself. Thus the smallest block size is 2 bytes, with a size value of 1 (1 size byte, 1 tag byte, 0 payload bytes). Maximum size of the payload is 254 bytes. + + +Utilizing tagged data blocks allows us to send multiple pieces of data in a single UDP packet. The tags currently implemented for the XRP are as follows: + +| Tag | Description | +|------|-------------------------------| +| 0x12 | [XRPMotor](#xrpmotor) | +| 0x13 | [XRPServo](#xrpservo) | +| 0x14 | [DIO](#dio) | +| 0x15 | [AnalogIn](#analogin) | +| 0x16 | [XRPGyro](#xrpgyro) | +| 0x17 | [BuiltInAccel](#builtinaccel) | +| 0x18 | [Encoder](#encoder) | + + +#### XRPMotor + +| Order | Data Type | Description | +|-------|-----------|-------------------| +| 0 | _uint8_t_ | ID | +| 1 | _float_ | Value [-1.0, 1.0] | + +IDs: +| ID | Description | +|----|-------------| +| 0 | Left Motor | +| 1 | Right Motor | +| 2 | Motor 3 | +| 3 | Motor 4 | + +#### XRPServo + +| Order | Data Type | Description | +|-------|-----------|------------------| +| 0 | _uint8_t_ | ID | +| 1 | _float_ | Value [0.0, 1.0] | + +IDs: +| ID | Description | +|----|-------------| +| 4 | Servo 1 | +| 5 | Servo 2 | + +#### DIO + +| Order | Data Type | Description | +|-------|-----------|--------------------| +| 0 | _uint8_t_ | ID | +| 1 | _uint8_t_ | Value (True/False) | + +#### AnalogIn + +| Order | Data Type | Description | +|-------|-----------|-------------| +| 0 | _uint8_t_ | ID | +| 1 | _float_ | Value | + +#### XRPGyro + +| Order | Data Type | Description | +|-------|-----------|---------------| +| 0 | _float_ | rate_x (dps) | +| 1 | _float_ | rate_y (dps) | +| 2 | _float_ | rate_z (dps) | +| 3 | _float_ | angle_x (deg) | +| 4 | _float_ | angle_y (deg) | +| 5 | _float_ | angle_z (deg) | + +#### BuiltInAccel + +| Order | Data Type | Description | +|-------|-----------|-------------| +| 0 | _float_ | accel_x (g) | +| 1 | _float_ | accel_y (g) | +| 2 | _float_ | accel_z (g) | + +#### Encoder + +| Order | Data Type | Description | +|-------|-----------|-------------| +| 0 | _uint8_t_ | ID | +| 1 | _int32_t_ | Value | + +IDs: +| ID | Description | +|----|---------------------| +| 0 | Left Motor Encoder | +| 1 | Right Motor Encoder | +| 2 | Motor 3 Encoder | +| 3 | Motor 4 Encoder | diff --git a/simulation/halsim_xrp/build.gradle b/simulation/halsim_xrp/build.gradle new file mode 100644 index 0000000000..7235a56893 --- /dev/null +++ b/simulation/halsim_xrp/build.gradle @@ -0,0 +1,35 @@ +if (!project.hasProperty('onlylinuxathena')) { + + description = "XRP Extension" + + ext { + includeWpiutil = true + pluginName = 'halsim_xrp' + } + + apply plugin: 'google-test-test-suite' + + + ext { + staticGtestConfigs = [:] + } + + staticGtestConfigs["${pluginName}Test"] = [] + apply from: "${rootDir}/shared/googletest.gradle" + + apply from: "${rootDir}/shared/plugins/setupBuild.gradle" + + model { + binaries { + all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + it.buildable = false + return + } + + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static" + } + } + } +} diff --git a/simulation/halsim_xrp/src/dev/native/cpp/main.cpp b/simulation/halsim_xrp/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..a3e363efca --- /dev/null +++ b/simulation/halsim_xrp/src/dev/native/cpp/main.cpp @@ -0,0 +1,5 @@ +// 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. + +int main() {} diff --git a/simulation/halsim_xrp/src/main/native/cpp/HALSimXRP.cpp b/simulation/halsim_xrp/src/main/native/cpp/HALSimXRP.cpp new file mode 100644 index 0000000000..0f2b93b042 --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/cpp/HALSimXRP.cpp @@ -0,0 +1,164 @@ +// 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 "HALSimXRP.h" + +#include + +#include +#include +#include +#include +#include +#include + +namespace uv = wpi::uv; + +using namespace wpilibxrp; + +HALSimXRP::HALSimXRP(wpi::uv::Loop& loop, + wpilibws::ProviderContainer& providers, + wpilibws::HALSimWSProviderSimDevices& simDevicesProvider) + : m_loop(loop), + m_providers(providers), + m_simDevicesProvider(simDevicesProvider) { + m_loop.error.connect([](uv::Error err) { + fmt::print(stderr, "HALSim XRP Client libuv Error: {}\n", err.str()); + }); + + m_udp_client = uv::Udp::Create(m_loop); + m_exec = UvExecFunc::Create(m_loop); + if (m_exec) { + m_exec->wakeup.connect([](auto func) { func(); }); + } +} + +bool HALSimXRP::Initialize() { + if (!m_udp_client || !m_exec) { + return false; + } + + const char* host = std::getenv("HALSIMXRP_HOST"); + if (host != nullptr) { + m_host = host; + } else { + m_host = "localhost"; + } + + const char* port = std::getenv("HALSIMXRP_PORT"); + if (port != nullptr) { + try { + m_port = std::stoi(port); + } catch (const std::invalid_argument& err) { + fmt::print(stderr, "Error decoding HALSIMXRP_PORT ({})\n", err.what()); + return false; + } + } else { + m_port = 3540; + } + + wpilibxrp::WPILibUpdateFunc func = [&](const wpi::json& data) { + OnNetValueChanged(data); + }; + + m_xrp.SetWPILibUpdateFunc(func); + + return true; +} + +void HALSimXRP::Start() { + // struct sockaddr_in dest; + uv::NameToAddr(m_host, m_port, &m_dest); + + m_udp_client->Connect(m_dest); + + m_udp_client->received.connect( + [this, socket = m_udp_client.get()](auto data, size_t len, auto rinfo, + unsigned int flags) { + ParsePacket({reinterpret_cast(data.base), len}); + }); + + m_udp_client->closed.connect([]() { fmt::print("Socket Closed\n"); }); + + // Fake the OnNetworkConnected call + auto hws = shared_from_this(); + m_simDevicesProvider.OnNetworkConnected(hws); + m_providers.ForEach( + [hws](std::shared_ptr provider) { + provider->OnNetworkConnected(hws); + }); + + m_udp_client->StartRecv(); + + std::puts("HALSimXRP Initialized"); +} + +void HALSimXRP::ParsePacket(std::span packet) { + if (packet.size() < 3) { + return; + } + + // Hand this off to the XRP object to deal with the messages + m_xrp.HandleXRPUpdate(packet); +} + +void HALSimXRP::OnNetValueChanged(const wpi::json& msg) { + try { + auto& type = msg.at("type").get_ref(); + auto& device = msg.at("device").get_ref(); + + wpi::SmallString<64> key; + key.append(type); + if (!device.empty()) { + key.append("/"); + key.append(device); + } + + auto provider = m_providers.Get(key.str()); + if (provider) { + provider->OnNetValueChanged(msg.at("data")); + } + } catch (wpi::json::exception& e) { + fmt::print(stderr, "Error with incoming message: {}\n", e.what()); + } +} + +void HALSimXRP::OnSimValueChanged(const wpi::json& simData) { + // We'll use a signal from robot code to send all the data + if (simData["type"] == "HAL") { + auto halData = simData["data"]; + if (halData.find(">sim_periodic_after") != halData.end()) { + SendStateToXRP(); + } + } else { + m_xrp.HandleWPILibUpdate(simData); + } +} + +uv::SimpleBufferPool<4>& HALSimXRP::GetBufferPool() { + static uv::SimpleBufferPool<4> bufferPool(128); + return bufferPool; +} + +void HALSimXRP::SendStateToXRP() { + wpi::SmallVector sendBufs; + wpi::raw_uv_ostream stream{sendBufs, [&] { + std::lock_guard lock(m_buffer_mutex); + return GetBufferPool().Allocate(); + }}; + m_xrp.SetupXRPSendBuffer(stream); + + m_exec->Send([this, sendBufs]() mutable { + m_udp_client->Send(sendBufs, [&](auto bufs, uv::Error err) { + { + std::lock_guard lock(m_buffer_mutex); + GetBufferPool().Release(bufs); + } + + if (err) { + // no-op + } + }); + }); +} diff --git a/simulation/halsim_xrp/src/main/native/cpp/HALSimXRPClient.cpp b/simulation/halsim_xrp/src/main/native/cpp/HALSimXRPClient.cpp new file mode 100644 index 0000000000..2cd36c39be --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/cpp/HALSimXRPClient.cpp @@ -0,0 +1,48 @@ +// 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 "HALSimXRPClient.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace wpilibxrp; +using namespace wpilibws; + +bool HALSimXRPClient::Initialize() { + bool result = true; + runner.ExecSync([&](wpi::uv::Loop& loop) { + simxrp = std::make_shared(loop, providers, simDevices); + + if (!simxrp->Initialize()) { + result = false; + return; + } + + WSRegisterFunc registerFunc = [&](auto key, auto provider) { + providers.Add(key, provider); + }; + + // Minimized set of HAL providers + HALSimWSProviderAnalogIn::Initialize(registerFunc); + HALSimWSProviderBuiltInAccelerometer::Initialize(registerFunc); + HALSimWSProviderDIO::Initialize(registerFunc); + HALSimWSProviderDriverStation::Initialize(registerFunc); + HALSimWSProviderEncoder::Initialize(registerFunc); + HALSimWSProviderHAL::Initialize(registerFunc); + + simDevices.Initialize(loop); + + simxrp->Start(); + }); + + return result; +} diff --git a/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp new file mode 100644 index 0000000000..33a49c4b91 --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/cpp/XRP.cpp @@ -0,0 +1,373 @@ +// 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 "XRP.h" + +#include +#include +#include + +using namespace wpilibxrp; + +XRP::XRP() + : m_gyro_name{"XRPGyro"}, m_wpilib_update_func([](const wpi::json&) {}) { + // Set up the inputs and outputs + m_motor_outputs.emplace(0, 0.0f); + m_motor_outputs.emplace(1, 0.0f); + m_motor_outputs.emplace(2, 0.0f); + m_motor_outputs.emplace(3, 0.0f); + + m_servo_outputs.emplace(4, 0.5f); + m_servo_outputs.emplace(5, 0.5f); + + m_encoder_inputs.emplace(1, 0); + m_encoder_inputs.emplace(2, 0); + m_encoder_inputs.emplace(0, 0); + m_encoder_inputs.emplace(3, 0); +} + +void XRP::HandleWPILibUpdate(const wpi::json& data) { + if (data.count("type") == 0) { + return; + } + + if (data["type"] == "DriverStation") { + HandleDriverStationSimValueChanged(data); + } else if (data["type"] == "XRPMotor") { + HandleMotorSimValueChanged(data); + } else if (data["type"] == "XRPServo") { + HandleServoSimValueChanged(data); + } else if (data["type"] == "DIO") { + HandleDIOSimValueChanged(data); + } else if (data["type"] == "Gyro") { + HandleGyroSimValueChanged(data); + } else if (data["type"] == "Encoder") { + HandleEncoderSimValueChanged(data); + } +} + +void XRP::HandleXRPUpdate(std::span packet) { + uint16_t seq = (packet[0] << 8) + packet[1]; + + if (seq <= m_wpilib_bound_seq) { + // If the old sequence was within 3 or uint16_t max and the new + // sequence is < 3 - we've prob rolled over + if (!((0xFFFF - m_wpilib_bound_seq < 3) && seq < 3)) { + return; + } + } + + m_wpilib_bound_seq = seq; + + // Tagged data starts at byte 3 + packet = packet.subspan(3); + + // Loop to handle multiple tags + while (!packet.empty()) { + auto tagLength = packet[0]; + auto tagPacket = packet.subspan(0, tagLength + 1); + + // NOTE: tagPacket contains the size and tag bytes as well + // Verify that the packet is indeed the right size + if (tagPacket.size() != static_cast(tagLength + 1)) { + break; + } + + switch (packet[1]) { + case XRP_TAG_GYRO: + ReadGyroTag(tagPacket); + break; + case XRP_TAG_ACCEL: + ReadAccelTag(tagPacket); + break; + case XRP_TAG_DIO: + ReadDIOTag(tagPacket); + break; + case XRP_TAG_ENCODER: + ReadEncoderTag(tagPacket); + break; + case XRP_TAG_ANALOG: + ReadAnalogTag(tagPacket); + break; + } + + packet = packet.subspan(tagLength + 1); + } +} + +void XRP::SetupXRPSendBuffer(wpi::raw_uv_ostream& buf) { + SetupSendHeader(buf); + SetupMotorTag(buf); + SetupServoTag(buf); + SetupDigitalOutTag(buf); + m_xrp_bound_seq++; +} + +// WPILib Sim Handlers +void XRP::HandleDriverStationSimValueChanged(const wpi::json& data) { + auto dsData = data["data"]; + if (dsData.find(">enabled") != dsData.end()) { + m_robot_enabled = dsData[">enabled"]; + } +} + +void XRP::HandleMotorSimValueChanged(const wpi::json& data) { + int deviceId = -1; + auto motorData = data["data"]; + + if (data["device"] == "motorL") { + deviceId = 0; + } else if (data["device"] == "motorR") { + deviceId = 1; + } else if (data["device"] == "motor3") { + deviceId = 2; + } else if (data["device"] == "motor4") { + deviceId = 3; + } + + if (deviceId != -1 && motorData.find("()); + } catch (const std::invalid_argument&) { + deviceId = -1; + } + + // Bail out early if device ID is invalid or if it's "spoken for" + if (deviceId == -1) { + return; + } + + if (dioData.find("value") != dioData.end() && + m_digital_outputs.count(deviceId) > 0) { + m_digital_outputs[deviceId] = dioData["<>value"]; + } +} + +void XRP::HandleGyroSimValueChanged(const wpi::json& data) { + m_gyro_name = data["device"].get(); +} + +void XRP::HandleEncoderSimValueChanged(const wpi::json& data) { + // We need to handle the various encoder cases + // 4/5 -> Encoder 0 + // 6/7 -> Encoder 1 + // 8/9 -> Encoder 2 + // 10/11 -> Encoder 3 + int deviceId = -1; + auto encData = data["data"]; + + try { + deviceId = std::stoi(data["device"].get()); + } catch (const std::invalid_argument&) { + deviceId = -1; + } + + if (deviceId == -1) { + return; + } + + if (encData.find("(m_robot_enabled ? 1 : 0); +} + +void XRP::SetupMotorTag(wpi::raw_uv_ostream& buf) { + uint8_t value[4]; + + for (auto motor : m_motor_outputs) { + // Motor payload is 6 bytes + buf << static_cast(6) // Size + << static_cast(XRP_TAG_MOTOR) // Tag + << static_cast(motor.first); // Channel + + // Convert the value + wpi::support::endian::write32be(value, wpi::FloatToBits(motor.second)); + buf << value[0] << value[1] << value[2] << value[3]; + } +} + +void XRP::SetupServoTag(wpi::raw_uv_ostream& buf) { + uint8_t value[4]; + + for (auto servo : m_servo_outputs) { + // Servo payload is 6 bytes + buf << static_cast(6) // Size + << static_cast(XRP_TAG_SERVO) // Tag + << static_cast(servo.first); // Channel + + // Convert the value + wpi::support::endian::write32be(value, wpi::FloatToBits(servo.second)); + buf << value[0] << value[1] << value[2] << value[3]; + } +} + +void XRP::SetupDigitalOutTag(wpi::raw_uv_ostream& buf) { + for (auto digitalOut : m_digital_outputs) { + // DIO payload is 3 bytes + buf << static_cast(3) // Size + << static_cast(XRP_TAG_DIO) // Tag + << static_cast(digitalOut.first) // Channel + << static_cast(digitalOut.second ? 1 : 0); // Value + } +} + +void XRP::ReadGyroTag(std::span packet) { + if (packet.size() < 26) { + return; // size(1) + tag(1) + 6x 4byte + } + + packet = packet.subspan(2); // Skip past the size and tag + float rate_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0])); + float rate_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[4])); + float rate_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[8])); + float angle_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[12])); + float angle_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[16])); + float angle_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[20])); + + // Make the json object + wpi::json gyroJson; + gyroJson["type"] = "Gyro"; + gyroJson["device"] = m_gyro_name; + gyroJson["data"] = {{">rate_x", rate_x}, {">rate_y", rate_y}, + {">rate_z", rate_z}, {">angle_x", angle_x}, + {">angle_y", angle_y}, {">angle_z", angle_z}}; + + // Update WPILib + m_wpilib_update_func(gyroJson); +} + +void XRP::ReadAccelTag(std::span packet) { + if (packet.size() < 14) { + return; // size(1) + tag(1) + 3x 4 byte + } + + packet = packet.subspan(2); // Skip past the size and tag + float accel_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0])); + float accel_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[4])); + float accel_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[8])); + + wpi::json accelJson; + accelJson["type"] = "Accel"; + accelJson["device"] = "BuiltInAccel"; + accelJson["data"] = {{">x", accel_x}, {">y", accel_y}, {">z", accel_z}}; + + // Update WPILib + m_wpilib_update_func(accelJson); +} + +void XRP::ReadDIOTag(std::span packet) { + if (packet.size() < 4) { + return; // size(1) + tag(1) + id(1) + value(1) + } + + wpi::json dioJson; + dioJson["type"] = "DIO"; + dioJson["device"] = std::to_string(packet[2]); + dioJson["data"] = {{"<>value", packet[3] == 1}}; + + m_wpilib_update_func(dioJson); +} + +void XRP::ReadEncoderTag(std::span packet) { + if (packet.size() < 7) { + return; // size(1) + tag(1) + id(1) + value(4) + } + + uint8_t encoderId = packet[2]; + + packet = packet.subspan(3); // Skip past the size and tag and ID + int32_t value = + static_cast(wpi::support::endian::read32be(&packet[0])); + + // Look up the registered encoders + if (m_encoder_channel_map.count(encoderId) == 0) { + return; + } + + uint8_t wpilibEncoderChannel = m_encoder_channel_map[encoderId]; + + wpi::json encJson; + encJson["type"] = "Encoder"; + encJson["device"] = std::to_string(wpilibEncoderChannel); + encJson["data"] = {{">count", value}}; + + m_wpilib_update_func(encJson); +} + +void XRP::ReadAnalogTag(std::span packet) { + if (packet.size() < 7) { + return; // size(1) + tag(1) + id(1) + float + } + + uint8_t analogId = packet[2]; + + packet = packet.subspan(3); + float voltage = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0])); + + wpi::json analogJson; + analogJson["type"] = "AI"; + analogJson["device"] = std::to_string(analogId); + analogJson["data"] = {{">voltage", voltage}}; + + m_wpilib_update_func(analogJson); +} diff --git a/simulation/halsim_xrp/src/main/native/cpp/main.cpp b/simulation/halsim_xrp/src/main/native/cpp/main.cpp new file mode 100644 index 0000000000..50f4944a23 --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/cpp/main.cpp @@ -0,0 +1,42 @@ +// 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 +#include + +#include + +#include "HALSimXRPClient.h" + +#if defined(Win32) || defined(_WIN32) +#pragma comment(lib, "Ws2_32.lib") +#endif + +using namespace wpilibxrp; + +static std::unique_ptr gClient; + +/*-------------------------------------------------------------------------- +** Main Entry Point. Start up the listening threads +**------------------------------------------------------------------------*/ +extern "C" { +#if defined(WIN32) || defined(_WIN32) +__declspec(dllexport) +#endif + + int HALSIM_InitExtension(void) { + std::puts("HALSim XRP Extension Initializing"); + + HAL_OnShutdown(nullptr, [](void*) { gClient.reset(); }); + + gClient = std::make_unique(); + if (!gClient->Initialize()) { + return -1; + } + + std::puts("HALSim XRP Extention Initialized"); + return 0; +} + +} // extern "C" diff --git a/simulation/halsim_xrp/src/main/native/include/HALSimXRP.h b/simulation/halsim_xrp/src/main/native/include/HALSimXRP.h new file mode 100644 index 0000000000..df2114f411 --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/include/HALSimXRP.h @@ -0,0 +1,75 @@ +// 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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "XRP.h" + +namespace wpi { +class json; +} // namespace wpi + +namespace wpilibxrp { + +// This masquerades as a "WebSocket" so that we can reuse the +// stuff in halsim_ws_core +class HALSimXRP : public wpilibws::HALSimBaseWebSocketConnection, + public std::enable_shared_from_this { + public: + using LoopFunc = std::function; + using UvExecFunc = wpi::uv::Async; + + HALSimXRP(wpi::uv::Loop& loop, wpilibws::ProviderContainer& providers, + wpilibws::HALSimWSProviderSimDevices& simDevicesProvider); + HALSimXRP(const HALSimXRP&) = delete; + HALSimXRP& operator=(const HALSimXRP&) = delete; + + bool Initialize(); + void Start(); + + void ParsePacket(std::span packet); + void OnNetValueChanged(const wpi::json& msg); + void OnSimValueChanged(const wpi::json& simData) override; + + const std::string& GetTargetHost() const { return m_host; } + int GetTargetPort() const { return m_port; } + wpi::uv::Loop& GetLoop() { return m_loop; } + + UvExecFunc& GetExec() { return *m_exec; } + + private: + XRP m_xrp; + + wpi::uv::Loop& m_loop; + std::shared_ptr m_udp_client; + std::shared_ptr m_exec; + + wpilibws::ProviderContainer& m_providers; + wpilibws::HALSimWSProviderSimDevices& m_simDevicesProvider; + + std::string m_host; + int m_port; + + void SendStateToXRP(); + wpi::uv::SimpleBufferPool<4>& GetBufferPool(); + std::mutex m_buffer_mutex; + + struct sockaddr_in m_dest; +}; + +} // namespace wpilibxrp diff --git a/simulation/halsim_xrp/src/main/native/include/HALSimXRPClient.h b/simulation/halsim_xrp/src/main/native/include/HALSimXRPClient.h new file mode 100644 index 0000000000..d6b26ffd38 --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/include/HALSimXRPClient.h @@ -0,0 +1,31 @@ +// 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 + +#include +#include +#include + +#include "HALSimXRP.h" + +namespace wpilibxrp { + +class HALSimXRPClient { + public: + HALSimXRPClient() = default; + HALSimXRPClient(const HALSimXRPClient&) = delete; + HALSimXRPClient& operator=(const HALSimXRPClient&) = delete; + + bool Initialize(); + + wpilibws::ProviderContainer providers; + wpilibws::HALSimWSProviderSimDevices simDevices{providers}; + wpi::EventLoopRunner runner; + std::shared_ptr simxrp; +}; + +} // namespace wpilibxrp diff --git a/simulation/halsim_xrp/src/main/native/include/XRP.h b/simulation/halsim_xrp/src/main/native/include/XRP.h new file mode 100644 index 0000000000..c9a0377859 --- /dev/null +++ b/simulation/halsim_xrp/src/main/native/include/XRP.h @@ -0,0 +1,88 @@ +// 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 +#include +#include +#include + +#include +#include + +#define XRP_TAG_MOTOR 0x12 +#define XRP_TAG_SERVO 0x13 +#define XRP_TAG_DIO 0x14 +#define XRP_TAG_ANALOG 0x15 +#define XRP_TAG_GYRO 0x16 +#define XRP_TAG_ACCEL 0x17 +#define XRP_TAG_ENCODER 0x18 + +namespace wpilibxrp { + +using WPILibUpdateFunc = std::function; + +class XRP { + public: + XRP(); + + void SetWPILibUpdateFunc(WPILibUpdateFunc func) { + m_wpilib_update_func = func; + } + + void HandleWPILibUpdate(const wpi::json& data); + void HandleXRPUpdate(std::span packet); + + void SetupXRPSendBuffer(wpi::raw_uv_ostream& buf); + + private: + // To XRP Methods + void SetupSendHeader(wpi::raw_uv_ostream& buf); + void SetupMotorTag(wpi::raw_uv_ostream& buf); + void SetupServoTag(wpi::raw_uv_ostream& buf); + void SetupDigitalOutTag(wpi::raw_uv_ostream& buf); + + // WPILib Sim Update Handlers + void HandleDriverStationSimValueChanged(const wpi::json& data); + void HandleMotorSimValueChanged(const wpi::json& data); + void HandleServoSimValueChanged(const wpi::json& data); + void HandleDIOSimValueChanged(const wpi::json& data); + void HandleGyroSimValueChanged(const wpi::json& data); + void HandleEncoderSimValueChanged(const wpi::json& data); + + // XRP Packet Update Handlers + void ReadGyroTag(std::span packet); + void ReadAccelTag(std::span packet); + void ReadDIOTag(std::span packet); + void ReadEncoderTag(std::span packet); + void ReadAnalogTag(std::span packet); + + // Robot State + std::map m_digital_outputs; + std::map m_motor_outputs; + std::map m_servo_outputs; + + // Might not need these + std::map m_digital_inputs; + std::map m_analog_inputs; + std::map m_encoder_inputs; + + // We need a map from XRP encoder channels (0=left, 1=right etc) + // to WPILib device ID + // Key: XRP encoder number, Value: WPILib channel + // If no encoders are init-ed, this map is empty + std::map m_encoder_channel_map; + + uint16_t m_wpilib_bound_seq = 0; + uint16_t m_xrp_bound_seq = 0; + + bool m_robot_enabled = false; + + std::string m_gyro_name; + + WPILibUpdateFunc m_wpilib_update_func; +}; + +} // namespace wpilibxrp