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

This commit is contained in:
Thad House
2021-10-10 20:04:50 -07:00
committed by GitHub
parent 4c61a13057
commit 10f19e6fc3
49 changed files with 8383 additions and 19 deletions

View File

@@ -6,6 +6,7 @@
#include "frc/Errors.h"
#include "frc/PneumaticsControlModule.h"
#include "frc/PneumaticsHub.h"
#include "frc/SensorUtil.h"
using namespace frc;
@@ -14,6 +15,8 @@ std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
int module, PneumaticsModuleType moduleType) {
if (moduleType == PneumaticsModuleType::CTREPCM) {
return PneumaticsControlModule::GetForModule(module);
} else if (moduleType == PneumaticsModuleType::REVPH) {
return PneumaticsHub::GetForModule(module);
}
throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
}
@@ -21,6 +24,8 @@ std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) {
if (moduleType == PneumaticsModuleType::CTREPCM) {
return SensorUtil::GetDefaultCTREPCMModule();
} else if (moduleType == PneumaticsModuleType::REVPH) {
return SensorUtil::GetDefaultREVPHModule();
}
throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
}

View File

@@ -0,0 +1,210 @@
// 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 "frc/PneumaticsHub.h"
#include <hal/REVPH.h>
#include <wpi/NullDeleter.h>
#include <wpi/StackTrace.h>
#include "frc/Compressor.h"
#include "frc/DoubleSolenoid.h"
#include "frc/Errors.h"
#include "frc/SensorUtil.h"
#include "frc/Solenoid.h"
using namespace frc;
wpi::mutex PneumaticsHub::m_handleLock;
std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticsHub::DataStore>>>
PneumaticsHub::m_handleMap = nullptr;
// Always called under lock, so we can avoid the double lock from the magic
// static
std::weak_ptr<PneumaticsHub::DataStore>& PneumaticsHub::GetDataStore(
int module) {
if (!m_handleMap) {
m_handleMap = std::make_unique<
wpi::DenseMap<int, std::weak_ptr<PneumaticsHub::DataStore>>>();
}
return (*m_handleMap)[module];
}
class PneumaticsHub::DataStore {
public:
explicit DataStore(int module, const char* stackTrace) {
int32_t status = 0;
HAL_REVPHHandle handle = HAL_InitializeREVPH(module, stackTrace, &status);
FRC_CheckErrorStatus(status, "Module {}", module);
m_moduleObject = PneumaticsHub{handle, module};
m_moduleObject.m_dataStore =
std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
}
~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); }
DataStore(DataStore&&) = delete;
DataStore& operator=(DataStore&&) = delete;
private:
friend class PneumaticsHub;
uint32_t m_reservedMask{0};
bool m_compressorReserved{false};
wpi::mutex m_reservedLock;
PneumaticsHub m_moduleObject{HAL_kInvalidHandle, 0};
};
PneumaticsHub::PneumaticsHub()
: PneumaticsHub{SensorUtil::GetDefaultREVPHModule()} {}
PneumaticsHub::PneumaticsHub(int module) {
std::string stackTrace = wpi::GetStackTrace(1);
std::scoped_lock lock(m_handleLock);
auto& res = GetDataStore(module);
m_dataStore = res.lock();
if (!m_dataStore) {
m_dataStore = std::make_shared<DataStore>(module, stackTrace.c_str());
res = m_dataStore;
}
m_handle = m_dataStore->m_moduleObject.m_handle;
m_module = module;
}
PneumaticsHub::PneumaticsHub(HAL_REVPHHandle handle, int module)
: m_handle{handle}, m_module{module} {}
bool PneumaticsHub::GetCompressor() const {
int32_t status = 0;
auto result = HAL_GetREVPHCompressor(m_handle, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
return result;
}
void PneumaticsHub::SetClosedLoopControl(bool enabled) {
int32_t status = 0;
HAL_SetREVPHClosedLoopControl(m_handle, enabled, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
}
bool PneumaticsHub::GetClosedLoopControl() const {
int32_t status = 0;
auto result = HAL_GetREVPHClosedLoopControl(m_handle, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
return result;
}
bool PneumaticsHub::GetPressureSwitch() const {
int32_t status = 0;
auto result = HAL_GetREVPHPressureSwitch(m_handle, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
return result;
}
double PneumaticsHub::GetCompressorCurrent() const {
int32_t status = 0;
auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
return result;
}
void PneumaticsHub::SetSolenoids(int mask, int values) {
int32_t status = 0;
HAL_SetREVPHSolenoids(m_handle, mask, values, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
}
int PneumaticsHub::GetSolenoids() const {
int32_t status = 0;
auto result = HAL_GetREVPHSolenoids(m_handle, &status);
FRC_CheckErrorStatus(status, "Module {}", m_module);
return result;
}
int PneumaticsHub::GetModuleNumber() const {
return m_module;
}
int PneumaticsHub::GetSolenoidDisabledList() const {
return 0;
// TODO Fix me
// int32_t status = 0;
// auto result = HAL_GetREVPHSolenoidDisabledList(m_handle, &status);
// FRC_CheckErrorStatus(status, "Module {}", m_module);
// return result;
}
void PneumaticsHub::FireOneShot(int index) {
// TODO Fix me
// int32_t status = 0;
// HAL_FireREVPHOneShot(m_handle, index, &status);
// FRC_CheckErrorStatus(status, "Module {}", m_module);
}
void PneumaticsHub::SetOneShotDuration(int index, units::second_t duration) {
// TODO Fix me
// int32_t status = 0;
// units::millisecond_t millis = duration;
// HAL_SetREVPHOneShotDuration(m_handle, index, millis.to<int32_t>(),
// &status); FRC_CheckErrorStatus(status, "Module {}", m_module);
}
bool PneumaticsHub::CheckSolenoidChannel(int channel) const {
return HAL_CheckREVPHSolenoidChannel(channel);
}
int PneumaticsHub::CheckAndReserveSolenoids(int mask) {
std::scoped_lock lock{m_dataStore->m_reservedLock};
uint32_t uMask = static_cast<uint32_t>(mask);
if ((m_dataStore->m_reservedMask & uMask) != 0) {
return m_dataStore->m_reservedMask & uMask;
}
m_dataStore->m_reservedMask |= uMask;
return 0;
}
void PneumaticsHub::UnreserveSolenoids(int mask) {
std::scoped_lock lock{m_dataStore->m_reservedLock};
m_dataStore->m_reservedMask &= ~(static_cast<uint32_t>(mask));
}
bool PneumaticsHub::ReserveCompressor() {
std::scoped_lock lock{m_dataStore->m_reservedLock};
if (m_dataStore->m_compressorReserved) {
return false;
}
m_dataStore->m_compressorReserved = true;
return true;
}
void PneumaticsHub::UnreserveCompressor() {
std::scoped_lock lock{m_dataStore->m_reservedLock};
m_dataStore->m_compressorReserved = false;
}
Solenoid PneumaticsHub::MakeSolenoid(int channel) {
return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
}
DoubleSolenoid PneumaticsHub::MakeDoubleSolenoid(int forwardChannel,
int reverseChannel) {
return DoubleSolenoid{m_module, PneumaticsModuleType::REVPH, forwardChannel,
reverseChannel};
}
Compressor PneumaticsHub::MakeCompressor() {
return Compressor{m_module, PneumaticsModuleType::REVPH};
}
std::shared_ptr<PneumaticsBase> PneumaticsHub::GetForModule(int module) {
std::string stackTrace = wpi::GetStackTrace(1);
std::scoped_lock lock(m_handleLock);
auto& res = GetDataStore(module);
std::shared_ptr<DataStore> dataStore = res.lock();
if (!dataStore) {
dataStore = std::make_shared<DataStore>(module, stackTrace.c_str());
res = dataStore;
}
return std::shared_ptr<PneumaticsBase>{dataStore, &dataStore->m_moduleObject};
}

View File

@@ -23,6 +23,10 @@ int SensorUtil::GetDefaultCTREPCMModule() {
return 0;
}
int SensorUtil::GetDefaultREVPHModule() {
return 1;
}
bool SensorUtil::CheckDigitalChannel(int channel) {
return HAL_CheckDIOChannel(channel);
}

View File

@@ -0,0 +1,139 @@
// 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 "frc/simulation/REVPHSim.h"
#include <memory>
#include <utility>
#include <hal/simulation/REVPHData.h>
#include "frc/SensorUtil.h"
using namespace frc;
using namespace frc::sim;
REVPHSim::REVPHSim() : m_index{SensorUtil::GetDefaultREVPHModule()} {}
REVPHSim::REVPHSim(int module) : m_index{module} {}
REVPHSim::REVPHSim(const PneumaticsBase& pneumatics)
: m_index{pneumatics.GetModuleNumber()} {}
std::unique_ptr<CallbackStore> REVPHSim::RegisterInitializedCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelREVPHInitializedCallback);
store->SetUid(HALSIM_RegisterREVPHInitializedCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool REVPHSim::GetInitialized() const {
return HALSIM_GetREVPHInitialized(m_index);
}
void REVPHSim::SetInitialized(bool solenoidInitialized) {
HALSIM_SetREVPHInitialized(m_index, solenoidInitialized);
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterSolenoidOutputCallback(
int channel, NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, channel, -1, callback,
&HALSIM_CancelREVPHSolenoidOutputCallback);
store->SetUid(HALSIM_RegisterREVPHSolenoidOutputCallback(
m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool REVPHSim::GetSolenoidOutput(int channel) const {
return HALSIM_GetREVPHSolenoidOutput(m_index, channel);
}
void REVPHSim::SetSolenoidOutput(int channel, bool solenoidOutput) {
HALSIM_SetREVPHSolenoidOutput(m_index, channel, solenoidOutput);
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorOnCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelREVPHCompressorOnCallback);
store->SetUid(HALSIM_RegisterREVPHCompressorOnCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool REVPHSim::GetCompressorOn() const {
return HALSIM_GetREVPHCompressorOn(m_index);
}
void REVPHSim::SetCompressorOn(bool compressorOn) {
HALSIM_SetREVPHCompressorOn(m_index, compressorOn);
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterClosedLoopEnabledCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelREVPHClosedLoopEnabledCallback);
store->SetUid(HALSIM_RegisterREVPHClosedLoopEnabledCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool REVPHSim::GetClosedLoopEnabled() const {
return HALSIM_GetREVPHClosedLoopEnabled(m_index);
}
void REVPHSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
HALSIM_SetREVPHClosedLoopEnabled(m_index, closedLoopEnabled);
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterPressureSwitchCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelREVPHPressureSwitchCallback);
store->SetUid(HALSIM_RegisterREVPHPressureSwitchCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool REVPHSim::GetPressureSwitch() const {
return HALSIM_GetREVPHPressureSwitch(m_index);
}
void REVPHSim::SetPressureSwitch(bool pressureSwitch) {
HALSIM_SetREVPHPressureSwitch(m_index, pressureSwitch);
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorCurrentCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelREVPHCompressorCurrentCallback);
store->SetUid(HALSIM_RegisterREVPHCompressorCurrentCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
double REVPHSim::GetCompressorCurrent() const {
return HALSIM_GetREVPHCompressorCurrent(m_index);
}
void REVPHSim::SetCompressorCurrent(double compressorCurrent) {
HALSIM_SetREVPHCompressorCurrent(m_index, compressorCurrent);
}
uint8_t REVPHSim::GetAllSolenoidOutputs() const {
uint8_t ret = 0;
HALSIM_GetREVPHAllSolenoids(m_index, &ret);
return ret;
}
void REVPHSim::SetAllSolenoidOutputs(uint8_t outputs) {
HALSIM_SetREVPHAllSolenoids(m_index, outputs);
}
void REVPHSim::ResetData() {
HALSIM_ResetREVPHData(m_index);
}

View File

@@ -0,0 +1,77 @@
// 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 <memory>
#include <hal/Types.h>
#include <wpi/DenseMap.h>
#include <wpi/mutex.h>
#include "PneumaticsBase.h"
namespace frc {
class PneumaticsHub : public PneumaticsBase {
public:
PneumaticsHub();
explicit PneumaticsHub(int module);
~PneumaticsHub() override = default;
bool GetCompressor() const override;
void SetClosedLoopControl(bool enabled) override;
bool GetClosedLoopControl() const override;
bool GetPressureSwitch() const override;
double GetCompressorCurrent() const override;
void SetSolenoids(int mask, int values) override;
int GetSolenoids() const override;
int GetModuleNumber() const override;
int GetSolenoidDisabledList() const override;
void FireOneShot(int index) override;
void SetOneShotDuration(int index, units::second_t duration) override;
bool CheckSolenoidChannel(int channel) const override;
int CheckAndReserveSolenoids(int mask) override;
void UnreserveSolenoids(int mask) override;
bool ReserveCompressor() override;
void UnreserveCompressor() override;
Solenoid MakeSolenoid(int channel) override;
DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
int reverseChannel) override;
Compressor MakeCompressor() override;
private:
class DataStore;
friend class DataStore;
friend class PneumaticsBase;
PneumaticsHub(HAL_REVPHHandle handle, int module);
static std::shared_ptr<PneumaticsBase> GetForModule(int module);
std::shared_ptr<DataStore> m_dataStore;
HAL_REVPHHandle m_handle;
int m_module;
static wpi::mutex m_handleLock;
static std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<DataStore>>>
m_handleMap;
static std::weak_ptr<DataStore>& GetDataStore(int module);
};
} // namespace frc

View File

@@ -21,6 +21,13 @@ class SensorUtil final {
*/
static int GetDefaultCTREPCMModule();
/**
* Get the number of the default solenoid module.
*
* @return The number of the default solenoid module.
*/
static int GetDefaultREVPHModule();
/**
* Check that the digital channel number is valid.
*

View File

@@ -0,0 +1,214 @@
// 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 <memory>
#include "frc/PneumaticsBase.h"
#include "frc/simulation/CallbackStore.h"
namespace frc {
class Compressor;
namespace sim {
/**
* Class to control a simulated Pneumatic Control Module (PCM).
*/
class REVPHSim {
public:
/**
* Constructs with the default PCM module number (CAN ID).
*/
REVPHSim();
/**
* Constructs from a PCM module number (CAN ID).
*
* @param module module number
*/
explicit REVPHSim(int module);
explicit REVPHSim(const PneumaticsBase& pneumatics);
/**
* Register a callback to be run when a solenoid is initialized on a channel.
*
* @param channel the channel to monitor
* @param callback the callback
* @param initialNotify should the callback be run with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
NotifyCallback callback, bool initialNotify);
/**
* Check if a solenoid has been initialized on a specific channel.
*
* @return true if initialized
*/
bool GetInitialized() const;
/**
* Define whether a solenoid has been initialized on a specific channel.
*
* @param channel the channel
* @param solenoidInitialized is there a solenoid initialized on that channel
*/
void SetInitialized(bool solenoidInitialized);
/**
* Register a callback to be run when the solenoid output on a channel
* changes.
*
* @param channel the channel to monitor
* @param callback the callback
* @param initialNotify should the callback be run with the initial value
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
int channel, NotifyCallback callback, bool initialNotify);
/**
* Check the solenoid output on a specific channel.
*
* @param channel the channel to check
* @return the solenoid output
*/
bool GetSolenoidOutput(int channel) const;
/**
* Change the solenoid output on a specific channel.
*
* @param channel the channel to check
* @param solenoidOutput the new solenoid output
*/
void SetSolenoidOutput(int channel, bool solenoidOutput);
/**
* Register a callback to be run when the compressor activates.
*
* @param callback the callback
* @param initialNotify whether to run the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
NotifyCallback callback, bool initialNotify);
/**
* Check if the compressor is on.
*
* @return true if the compressor is active
*/
bool GetCompressorOn() const;
/**
* Set whether the compressor is active.
*
* @param compressorOn the new value
*/
void SetCompressorOn(bool compressorOn);
/**
* Register a callback to be run whenever the closed loop state changes.
*
* @param callback the callback
* @param initialNotify whether the callback should be called with the
* initial value
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore>
RegisterClosedLoopEnabledCallback(NotifyCallback callback,
bool initialNotify);
/**
* Check whether the closed loop compressor control is active.
*
* @return true if active
*/
bool GetClosedLoopEnabled() const;
/**
* Turn on/off the closed loop control of the compressor.
*
* @param closedLoopEnabled whether the control loop is active
*/
void SetClosedLoopEnabled(bool closedLoopEnabled);
/**
* Register a callback to be run whenever the pressure switch value changes.
*
* @param callback the callback
* @param initialNotify whether the callback should be called with the
* initial value
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
NotifyCallback callback, bool initialNotify);
/**
* Check the value of the pressure switch.
*
* @return the pressure switch value
*/
bool GetPressureSwitch() const;
/**
* Set the value of the pressure switch.
*
* @param pressureSwitch the new value
*/
void SetPressureSwitch(bool pressureSwitch);
/**
* Register a callback to be run whenever the compressor current changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore>
RegisterCompressorCurrentCallback(NotifyCallback callback,
bool initialNotify);
/**
* Read the compressor current.
*
* @return the current of the compressor connected to this module
*/
double GetCompressorCurrent() const;
/**
* Set the compressor current.
*
* @param compressorCurrent the new compressor current
*/
void SetCompressorCurrent(double compressorCurrent);
/**
* Get the current value of all solenoid outputs.
*
* @return the solenoid outputs (1 bit per output)
*/
uint8_t GetAllSolenoidOutputs() const;
/**
* Change all of the solenoid outputs.
*
* @param outputs the new solenoid outputs (1 bit per output)
*/
void SetAllSolenoidOutputs(uint8_t outputs);
/**
* Reset all simulation data for this object.
*/
void ResetData();
private:
int m_index;
};
} // namespace sim
} // namespace frc