[hal, wpilib] Add REV PneumaticsHub (#3600)

hal/src/main/native/sim/CTREPCM.cpp

@@ -48,7 +48,7 @@ HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
                                            pcm->previousAllocation);
     } else {
       hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0,
-                                       kNumAccumulators, module);
+                                       kNumCTREPCMModules, module);
     }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
   }

hal/src/main/native/sim/HAL.cpp

@@ -74,6 +74,7 @@ void InitializeHAL() {
   InitializeEncoderData();
   InitializeI2CData();
   InitializeCTREPCMData();
+  InitializeREVPHData();
   InitializePowerDistributionData();
   InitializePWMData();
   InitializeRelayData();
@@ -107,6 +108,7 @@ void InitializeHAL() {
   InitializePorts();
   InitializePower();
   InitializeCTREPCM();
+  InitializeREVPH();
   InitializePWM();
   InitializeRelay();
   InitializeSerialPort();

hal/src/main/native/sim/HALInitializer.h

@@ -32,6 +32,7 @@ extern void InitializeDriverStationData();
 extern void InitializeEncoderData();
 extern void InitializeI2CData();
 extern void InitializeCTREPCMData();
+extern void InitializeREVPHData();
 extern void InitializePowerDistributionData();
 extern void InitializePWMData();
 extern void InitializeRelayData();
@@ -65,6 +66,7 @@ extern void InitializePowerDistribution();
 extern void InitializePorts();
 extern void InitializePower();
 extern void InitializeCTREPCM();
+extern void InitializeREVPH();
 extern void InitializePWM();
 extern void InitializeRelay();
 extern void InitializeSerialPort();

hal/src/main/native/sim/Ports.cpp

@@ -73,6 +73,12 @@ int32_t HAL_GetNumREVPDHModules(void) {
 int32_t HAL_GetNumREVPDHChannels(void) {
   return kNumREVPDHChannels;
 }
+int32_t HAL_GetNumREVPHModules(void) {
+  return kNumREVPHModules;
+}
+int32_t HAL_GetNumREVPHChannels(void) {
+  return kNumREVPHChannels;
+}
 int32_t HAL_GetNumDutyCycles(void) {
   return kNumDutyCycles;
 }

hal/src/main/native/sim/PortsInternal.h

@@ -29,4 +29,6 @@ constexpr int32_t kNumREVPDHModules = 63;
 constexpr int32_t kNumREVPDHChannels = 24;
 constexpr int32_t kNumDutyCycles = 8;
 constexpr int32_t kNumAddressableLEDs = 1;
+constexpr int32_t kNumREVPHModules = 63;
+constexpr int32_t kNumREVPHChannels = 16;
 }  // namespace hal

hal/src/main/native/sim/REVPH.cpp

@@ -0,0 +1,181 @@
+// 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 "hal/REVPH.h"
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/REVPHDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct PCM {
+  int32_t module;
+  wpi::mutex lock;
+  std::string previousAllocation;
+};
+}  // namespace
+
+static IndexedHandleResource<HAL_REVPHHandle, PCM, kNumREVPHModules,
+                             HAL_HandleEnum::REVPH>* pcmHandles;
+
+namespace hal::init {
+void InitializeREVPH() {
+  static IndexedHandleResource<HAL_REVPHHandle, PCM, kNumREVPHModules,
+                               HAL_HandleEnum::REVPH>
+      pH;
+  pcmHandles = &pH;
+}
+}  // namespace hal::init
+
+HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
+                                    const char* allocationLocation,
+                                    int32_t* status) {
+  hal::init::CheckInit();
+
+  if (module == 0) {
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
+                                     kNumREVPHModules, module);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_REVPHHandle handle;
+  auto pcm = pcmHandles->Allocate(module, &handle, status);
+
+  if (*status != 0) {
+    if (pcm) {
+      hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module,
+                                           pcm->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
+                                       kNumREVPHModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  pcm->previousAllocation = allocationLocation ? allocationLocation : "";
+  pcm->module = module;
+
+  SimREVPHData[module].initialized = true;
+  // Enable closed loop
+  SimREVPHData[module].closedLoopEnabled = true;
+
+  return handle;
+}
+
+void HAL_FreeREVPH(HAL_REVPHHandle handle) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    pcmHandles->Free(handle);
+    return;
+  }
+  SimREVPHData[pcm->module].initialized = false;
+  pcmHandles->Free(handle);
+}
+
+HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) {
+  return module >= 1 && module < kNumREVPDHModules;
+}
+
+HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) {
+  return channel < kNumREVPHChannels && channel >= 0;
+}
+
+HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimREVPHData[pcm->module].compressorOn;
+}
+
+void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled,
+                                   int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  SimREVPHData[pcm->module].closedLoopEnabled = enabled;
+}
+
+HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle,
+                                       int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimREVPHData[pcm->module].closedLoopEnabled;
+}
+
+HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimREVPHData[pcm->module].pressureSwitch;
+}
+
+double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel,
+                                  int32_t* status) {
+  return 0;
+}
+
+double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  return SimREVPHData[pcm->module].compressorCurrent;
+}
+
+int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  std::scoped_lock lock{pcm->lock};
+  auto& data = SimREVPHData[pcm->module].solenoidOutput;
+  uint8_t ret = 0;
+  for (int i = 0; i < kNumREVPHChannels; i++) {
+    ret |= (data[i] << i);
+  }
+  return ret;
+}
+void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
+                           int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  auto& data = SimREVPHData[pcm->module].solenoidOutput;
+  std::scoped_lock lock{pcm->lock};
+  for (int i = 0; i < kNumREVPHChannels; i++) {
+    auto indexMask = (1 << i);
+    if ((mask & indexMask) != 0) {
+      data[i] = (values & indexMask) != 0;
+    }
+  }
+}
+
+void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
+                          int32_t* status) {}

hal/src/main/native/sim/mockdata/REVPHData.cpp

@@ -0,0 +1,83 @@
+// 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 "../PortsInternal.h"
+#include "REVPHDataInternal.h"
+
+using namespace hal;
+
+namespace hal::init {
+void InitializeREVPHData() {
+  static REVPHData spd[kNumREVPHModules];
+  ::hal::SimREVPHData = spd;
+}
+}  // namespace hal::init
+
+REVPHData* hal::SimREVPHData;
+void REVPHData::ResetData() {
+  for (int i = 0; i < kNumREVPHChannels; i++) {
+    solenoidOutput[i].Reset(false);
+  }
+  initialized.Reset(false);
+  compressorOn.Reset(false);
+  closedLoopEnabled.Reset(true);
+  pressureSwitch.Reset(false);
+  compressorCurrent.Reset(0.0);
+}
+
+extern "C" {
+void HALSIM_ResetREVPHData(int32_t index) {
+  SimREVPHData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                              \
+  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, REVPH##CAPINAME, SimREVPHData, \
+                               LOWERNAME)
+
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput,
+                                     SimREVPHData, solenoidOutput)
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(HAL_Bool, CompressorOn, compressorOn)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch)
+DEFINE_CAPI(double, CompressorCurrent, compressorCurrent)
+
+void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) {
+  auto& data = SimREVPHData[index].solenoidOutput;
+  uint8_t ret = 0;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    ret |= (data[i] << i);
+  }
+  *values = ret;
+}
+
+void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) {
+  auto& data = SimREVPHData[index].solenoidOutput;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    data[i] = (values & 0x1) != 0;
+    values >>= 1;
+  }
+}
+
+#define REGISTER(NAME) \
+  SimREVPHData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify) {
+  REGISTER(initialized);
+  REGISTER(compressorOn);
+  REGISTER(closedLoopEnabled);
+  REGISTER(pressureSwitch);
+  REGISTER(compressorCurrent);
+}
+
+void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {
+  REGISTER(solenoidOutput[channel]);
+}
+}  // extern "C"

hal/src/main/native/sim/mockdata/REVPHDataInternal.h

@@ -0,0 +1,43 @@
+// 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 "../PortsInternal.h"
+#include "hal/simulation/REVPHData.h"
+#include "hal/simulation/SimDataValue.h"
+
+namespace hal {
+class REVPHData {
+  HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+  HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidOutput)
+  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorOn)
+  HAL_SIMDATAVALUE_DEFINE_NAME(ClosedLoopEnabled)
+  HAL_SIMDATAVALUE_DEFINE_NAME(PressureSwitch)
+  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorCurrent)
+
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
+  GetSolenoidOutputDefault() {
+    return false;
+  }
+
+ public:
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+      false};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidOutputName,
+               GetSolenoidOutputDefault>
+      solenoidOutput[kNumREVPHChannels];
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorOnName> compressorOn{
+      false};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetClosedLoopEnabledName>
+      closedLoopEnabled{true};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetPressureSwitchName> pressureSwitch{
+      false};
+  SimDataValue<double, HAL_MakeDouble, GetCompressorCurrentName>
+      compressorCurrent{0.0};
+
+  virtual void ResetData();
+};
+extern REVPHData* SimREVPHData;
+}  // namespace hal