From 666d1638ce5fca9042834b964f8f9d90a8bc2dbb Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 12 Jan 2025 16:58:46 -0800 Subject: [PATCH] [hal] Digital IO SystemCore implementation (#7621) --- developerRobot/build.gradle | 8 +- hal/src/main/native/systemcore/DIO.cpp | 109 ++++++++++++++++-- hal/src/main/native/systemcore/PWM.cpp | 40 ++++++- .../main/native/systemcore/PortsInternal.h | 2 +- hal/src/main/native/systemcore/SmartIo.cpp | 81 +++++++++++-- hal/src/main/native/systemcore/SmartIo.h | 26 ++++- 6 files changed, 235 insertions(+), 31 deletions(-) diff --git a/developerRobot/build.gradle b/developerRobot/build.gradle index 09b1441de1..ca404e82ff 100644 --- a/developerRobot/build.gradle +++ b/developerRobot/build.gradle @@ -71,12 +71,18 @@ deploy { directory = '/home/systemcore' maxChannels = 4 locations { - ssh(SshDeployLocation) { + mdns(SshDeployLocation) { address = "limelight.local" user = 'systemcore' password = 'systemcore' ipv6 = false } + usb(SshDeployLocation) { + address = "172.28.0.1" + user = 'systemcore' + password = 'systemcore' + ipv6 = false + } } timeout = 7 diff --git a/hal/src/main/native/systemcore/DIO.cpp b/hal/src/main/native/systemcore/DIO.cpp index 4360f9788a..46eb679cbb 100644 --- a/hal/src/main/native/systemcore/DIO.cpp +++ b/hal/src/main/native/systemcore/DIO.cpp @@ -11,6 +11,7 @@ #include "HALInitializer.h" #include "HALInternal.h" #include "PortsInternal.h" +#include "SmartIo.h" #include "hal/Errors.h" #include "hal/cpp/fpga_clock.h" #include "hal/handles/HandlesInternal.h" @@ -29,15 +30,69 @@ HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle, const char* allocationLocation, int32_t* status) { hal::init::CheckInit(); - *status = HAL_HANDLE_ERROR; - return HAL_kInvalidHandle; + + int16_t channel = getPortHandleChannel(portHandle); + if (channel == InvalidHandleIndex || channel >= kNumSmartIo) { + *status = RESOURCE_OUT_OF_RANGE; + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0, + kNumSmartIo, channel); + return HAL_kInvalidHandle; + } + + HAL_DigitalHandle handle; + + auto port = + smartIoHandles->Allocate(channel, HAL_HandleEnum::DIO, &handle, status); + + if (*status != 0) { + if (port) { + hal::SetLastErrorPreviouslyAllocated(status, "SmartIo", channel, + port->previousAllocation); + } else { + hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0, + kNumSmartIo, channel); + } + return HAL_kInvalidHandle; // failed to allocate. Pass error back. + } + + port->channel = channel; + + *status = port->InitializeMode(input ? SmartIoMode::DigitalInput + : SmartIoMode::DigitalOutput); + if (*status != 0) { + smartIoHandles->Free(handle, HAL_HandleEnum::DIO); + return HAL_kInvalidHandle; + } + + port->previousAllocation = allocationLocation ? allocationLocation : ""; + + return handle; } HAL_Bool HAL_CheckDIOChannel(int32_t channel) { - return channel < kNumDigitalChannels && channel >= 0; + return channel < kNumSmartIo && channel >= 0; } -void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {} +void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) { + auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO); + if (port == nullptr) { + return; + } + + smartIoHandles->Free(dioPortHandle, HAL_HandleEnum::DIO); + + // Wait for no other object to hold this handle. + auto start = hal::fpga_clock::now(); + while (port.use_count() != 1) { + auto current = hal::fpga_clock::now(); + if (start + std::chrono::seconds(1) < current) { + std::puts("DIO handle free timeout"); + std::fflush(stdout); + break; + } + std::this_thread::yield(); + } +} void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) { } @@ -74,24 +129,54 @@ void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator, void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value, int32_t* status) { - *status = HAL_HANDLE_ERROR; - return; + auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + *status = port->SetDigitalOutput(value); } void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input, int32_t* status) { - *status = HAL_HANDLE_ERROR; - return; + auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + *status = port->SwitchDioDirection(input); } HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status) { - *status = HAL_HANDLE_ERROR; - return false; + auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + bool ret = false; + *status = port->GetDigitalInput(&ret); + return ret; } HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) { - *status = HAL_HANDLE_ERROR; - return false; + auto port = smartIoHandles->Get(dioPortHandle, HAL_HandleEnum::DIO); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return false; + } + + switch (port->currentMode) { + case SmartIoMode::DigitalInput: + return true; + case SmartIoMode::DigitalOutput: + return false; + default: + *status = INCOMPATIBLE_STATE; + return false; + } } void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds, diff --git a/hal/src/main/native/systemcore/PWM.cpp b/hal/src/main/native/systemcore/PWM.cpp index 9f9cb56986..f5bb48430b 100644 --- a/hal/src/main/native/systemcore/PWM.cpp +++ b/hal/src/main/native/systemcore/PWM.cpp @@ -99,7 +99,7 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle, port->channel = channel; - *status = port->InitializeMode(SmartIoMode::PWMOutput); + *status = port->InitializeMode(SmartIoMode::PwmOutput); if (*status != 0) { smartIoHandles->Free(handle, HAL_HandleEnum::PWM); return HAL_kInvalidHandle; @@ -372,18 +372,48 @@ double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status) { } void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status) { - // TODO(thad) figure out what this actually means - return; + HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, 0, status); } void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask, int32_t* status) { - // TODO(thad) not currently supported - return; + auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + switch (squelchMask) { + case 0: + *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k5ms); + break; + case 1: + case 2: + *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k10ms); + break; + case 3: + *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k20ms); + break; + default: + *status = PARAMETER_OUT_OF_RANGE; + return; + } } void HAL_SetPWMAlwaysHighMode(HAL_DigitalHandle pwmPortHandle, int32_t* status) { + // Always high is going to have to use a 2ms period + auto port = smartIoHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM); + if (port == nullptr) { + *status = HAL_HANDLE_ERROR; + return; + } + + *status = port->SetPwmOutputPeriod(hal::PwmOutputPeriod::k2ms); + if (*status != 0) { + return; + } + HAL_SetPWMPulseTimeMicroseconds(pwmPortHandle, kPwmAlwaysHigh, status); } diff --git a/hal/src/main/native/systemcore/PortsInternal.h b/hal/src/main/native/systemcore/PortsInternal.h index 9bb0f8fc6b..0fc1c5639c 100644 --- a/hal/src/main/native/systemcore/PortsInternal.h +++ b/hal/src/main/native/systemcore/PortsInternal.h @@ -8,7 +8,7 @@ namespace hal { -constexpr int32_t kNumSmartIo = 4; +constexpr int32_t kNumSmartIo = 5; constexpr int32_t kNumAccumulators = 0; constexpr int32_t kNumAnalogTriggers = 0; constexpr int32_t kNumAnalogInputs = 8; diff --git a/hal/src/main/native/systemcore/SmartIo.cpp b/hal/src/main/native/systemcore/SmartIo.cpp index 99f4831dc7..b8fd11c37d 100644 --- a/hal/src/main/native/systemcore/SmartIo.cpp +++ b/hal/src/main/native/systemcore/SmartIo.cpp @@ -34,29 +34,90 @@ int32_t SmartIo::InitializeMode(SmartIoMode mode) { modePublisher = inst.GetIntegerTopic(subTableString + "type").Publish(); getSubscriber = - inst.GetIntegerTopic(subTableString + "valget").Subscribe(0.0, options); + inst.GetIntegerTopic(subTableString + "valget").Subscribe(0, options); + frequencySubscriber = + inst.GetIntegerTopic(subTableString + "freqget").Subscribe(0, options); + setPublisher = + inst.GetIntegerTopic(subTableString + "valset").Publish(options); + periodPublisher = + inst.GetIntegerTopic(subTableString + "periodset").Publish(options); currentMode = mode; switch (mode) { - case SmartIoMode::PWMOutput: - modePublisher.Set(4); - setPublisher = - inst.GetIntegerTopic(subTableString + "valset").Publish(options); + // These need to set a 0 output + case SmartIoMode::DigitalOutput: + case SmartIoMode::PwmOutput: setPublisher.Set(0); - return 0; + break; + + // These don't need to set any value + case SmartIoMode::DigitalInput: + case SmartIoMode::AnalogInput: + case SmartIoMode::PwmInput: + case SmartIoMode::SingleCounterRising: + case SmartIoMode::SingleCounterFalling: + break; default: return INCOMPATIBLE_STATE; } + + modePublisher.Set(static_cast(mode)); + return 0; } -int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) { - if (currentMode != SmartIoMode::PWMOutput) { +int32_t SmartIo::SwitchDioDirection(bool input) { + if (currentMode != SmartIoMode::DigitalInput && + currentMode != SmartIoMode::DigitalOutput) { return INCOMPATIBLE_STATE; } - // TODO(thad) add support for always on signal + modePublisher.Set(input ? 0 : 1); + currentMode = input ? SmartIoMode::DigitalInput : SmartIoMode::DigitalOutput; + return 0; +} + +int32_t SmartIo::SetDigitalOutput(bool value) { + if (currentMode != SmartIoMode::DigitalInput && + currentMode != SmartIoMode::DigitalOutput) { + return INCOMPATIBLE_STATE; + } + setPublisher.Set(value ? 255.0 : 0.0); + return 0; +} + +int32_t SmartIo::GetDigitalInput(bool* value) { + if (currentMode != SmartIoMode::DigitalInput && + currentMode != SmartIoMode::DigitalOutput) { + return INCOMPATIBLE_STATE; + } + *value = getSubscriber.Get() != 0; + return 0; +} + +int32_t SmartIo::SetPwmOutputPeriod(PwmOutputPeriod period) { + if (currentMode != SmartIoMode::PwmOutput) { + return INCOMPATIBLE_STATE; + } + + switch (period) { + case PwmOutputPeriod::k20ms: + case PwmOutputPeriod::k10ms: + case PwmOutputPeriod::k5ms: + case PwmOutputPeriod::k2ms: + periodPublisher.Set(static_cast(period)); + return 0; + + default: + return PARAMETER_OUT_OF_RANGE; + } +} + +int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) { + if (currentMode != SmartIoMode::PwmOutput) { + return INCOMPATIBLE_STATE; + } if (microseconds > 4095) { microseconds = 4095; @@ -68,7 +129,7 @@ int32_t SmartIo::SetPwmMicroseconds(uint16_t microseconds) { } int32_t SmartIo::GetPwmMicroseconds(uint16_t* microseconds) { - if (currentMode != SmartIoMode::PWMOutput) { + if (currentMode != SmartIoMode::PwmOutput) { return INCOMPATIBLE_STATE; } diff --git a/hal/src/main/native/systemcore/SmartIo.h b/hal/src/main/native/systemcore/SmartIo.h index 1e3275566d..80cdac6a99 100644 --- a/hal/src/main/native/systemcore/SmartIo.h +++ b/hal/src/main/native/systemcore/SmartIo.h @@ -17,8 +17,20 @@ constexpr int32_t kPwmDisabled = 0; constexpr int32_t kPwmAlwaysHigh = 0xFFFF; enum class SmartIoMode { - DigitalInput, - PWMOutput, + DigitalInput = 0, + DigitalOutput, + AnalogInput, + PwmInput, + PwmOutput, + SingleCounterRising, + SingleCounterFalling, +}; + +enum class PwmOutputPeriod { + k20ms = 0, + k10ms, + k5ms, + k2ms, }; struct SmartIo { @@ -37,7 +49,17 @@ struct SmartIo { nt::IntegerPublisher setPublisher; nt::IntegerSubscriber getSubscriber; + nt::IntegerPublisher periodPublisher; + nt::IntegerSubscriber frequencySubscriber; + int32_t InitializeMode(SmartIoMode mode); + int32_t SwitchDioDirection(bool input); + + int32_t SetDigitalOutput(bool value); + int32_t GetDigitalInput(bool* value); + + int32_t SetPwmOutputPeriod(PwmOutputPeriod period); + int32_t SetPwmMicroseconds(uint16_t microseconds); int32_t GetPwmMicroseconds(uint16_t* microseconds); };