blob: 5b18de6d040aa126d69ff5b7c16a0a576c34a264 [file] [log] [blame]
// 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 <thread>
#include <fmt/format.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "PortsInternal.h"
#include "hal/CANAPI.h"
#include "hal/Errors.h"
#include "hal/handles/IndexedHandleResource.h"
#include "rev/PHFrames.h"
using namespace hal;
static constexpr HAL_CANManufacturer manufacturer =
HAL_CANManufacturer::HAL_CAN_Man_kREV;
static constexpr HAL_CANDeviceType deviceType =
HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics;
static constexpr int32_t kDefaultControlPeriod = 20;
static constexpr uint8_t kDefaultCompressorDuty = 255;
static constexpr uint8_t kDefaultPressureTarget = 120;
static constexpr uint8_t kDefaultPressureHysteresis = 60;
#define HAL_REVPH_MAX_PULSE_TIME 65534
static constexpr uint32_t APIFromExtId(uint32_t extId) {
return (extId >> 6) & 0x3FF;
}
static constexpr uint32_t PH_STATUS_0_FRAME_API =
APIFromExtId(PH_STATUS_0_FRAME_ID);
static constexpr uint32_t PH_STATUS_1_FRAME_API =
APIFromExtId(PH_STATUS_1_FRAME_ID);
static constexpr uint32_t PH_SET_ALL_FRAME_API =
APIFromExtId(PH_SET_ALL_FRAME_ID);
static constexpr uint32_t PH_PULSE_ONCE_FRAME_API =
APIFromExtId(PH_PULSE_ONCE_FRAME_ID);
static constexpr uint32_t PH_COMPRESSOR_CONFIG_API =
APIFromExtId(PH_COMPRESSOR_CONFIG_FRAME_ID);
static constexpr uint32_t PH_CLEAR_FAULTS_FRAME_API =
APIFromExtId(PH_CLEAR_FAULTS_FRAME_ID);
static constexpr uint32_t PH_VERSION_FRAME_API =
APIFromExtId(PH_VERSION_FRAME_ID);
static constexpr int32_t kPHFrameStatus0Timeout = 50;
static constexpr int32_t kPHFrameStatus1Timeout = 50;
namespace {
struct REV_PHObj {
int32_t controlPeriod;
PH_set_all_t desiredSolenoidsState;
wpi::mutex solenoidLock;
HAL_CANHandle hcan;
std::string previousAllocation;
HAL_REVPHVersion versionInfo;
};
} // namespace
static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, 63,
HAL_HandleEnum::REVPH>* REVPHHandles;
namespace hal::init {
void InitializeREVPH() {
static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, kNumREVPHModules,
HAL_HandleEnum::REVPH>
rH;
REVPHHandles = &rH;
}
} // namespace hal::init
static PH_status_0_t HAL_ReadREVPHStatus0(HAL_CANHandle hcan, int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_status_0_t result = {};
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_0_FRAME_API, packedData, &length,
&timestamp, kPHFrameStatus0Timeout * 2, status);
if (*status != 0) {
return result;
}
PH_status_0_unpack(&result, packedData, PH_STATUS_0_LENGTH);
return result;
}
static PH_status_1_t HAL_ReadREVPHStatus1(HAL_CANHandle hcan, int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_status_1_t result = {};
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_1_FRAME_API, packedData, &length,
&timestamp, kPHFrameStatus1Timeout * 2, status);
if (*status != 0) {
return result;
}
PH_status_1_unpack(&result, packedData, PH_STATUS_1_LENGTH);
return result;
}
enum REV_SolenoidState {
kSolenoidDisabled = 0,
kSolenoidEnabled,
kSolenoidControlledViaPulse
};
static void HAL_UpdateDesiredREVPHSolenoidState(REV_PHObj* hph,
int32_t solenoid,
REV_SolenoidState state) {
switch (solenoid) {
case 0:
hph->desiredSolenoidsState.channel_0 = state;
break;
case 1:
hph->desiredSolenoidsState.channel_1 = state;
break;
case 2:
hph->desiredSolenoidsState.channel_2 = state;
break;
case 3:
hph->desiredSolenoidsState.channel_3 = state;
break;
case 4:
hph->desiredSolenoidsState.channel_4 = state;
break;
case 5:
hph->desiredSolenoidsState.channel_5 = state;
break;
case 6:
hph->desiredSolenoidsState.channel_6 = state;
break;
case 7:
hph->desiredSolenoidsState.channel_7 = state;
break;
case 8:
hph->desiredSolenoidsState.channel_8 = state;
break;
case 9:
hph->desiredSolenoidsState.channel_9 = state;
break;
case 10:
hph->desiredSolenoidsState.channel_10 = state;
break;
case 11:
hph->desiredSolenoidsState.channel_11 = state;
break;
case 12:
hph->desiredSolenoidsState.channel_12 = state;
break;
case 13:
hph->desiredSolenoidsState.channel_13 = state;
break;
case 14:
hph->desiredSolenoidsState.channel_14 = state;
break;
case 15:
hph->desiredSolenoidsState.channel_15 = state;
break;
}
}
static void HAL_SendREVPHSolenoidsState(REV_PHObj* hph, int32_t* status) {
uint8_t packedData[PH_SET_ALL_LENGTH] = {0};
PH_set_all_pack(packedData, &(hph->desiredSolenoidsState), PH_SET_ALL_LENGTH);
HAL_WriteCANPacketRepeating(hph->hcan, packedData, PH_SET_ALL_LENGTH,
PH_SET_ALL_FRAME_API, hph->controlPeriod, status);
}
static HAL_Bool HAL_CheckREVPHPulseTime(int32_t time) {
return ((time > 0) && (time <= HAL_REVPH_MAX_PULSE_TIME)) ? 1 : 0;
}
HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
if (!HAL_CheckREVPHModuleNumber(module)) {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
kNumREVPHModules, module);
return HAL_kInvalidHandle;
}
HAL_REVPHHandle handle;
auto hph = REVPHHandles->Allocate(module, &handle, status);
if (*status != 0) {
if (hph) {
hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module,
hph->previousAllocation);
} else {
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
kNumREVPHModules, module);
}
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
HAL_CANHandle hcan =
HAL_InitializeCAN(manufacturer, module, deviceType, status);
if (*status != 0) {
REVPHHandles->Free(handle);
return HAL_kInvalidHandle;
}
hph->previousAllocation = allocationLocation ? allocationLocation : "";
hph->hcan = hcan;
hph->controlPeriod = kDefaultControlPeriod;
std::memset(&hph->desiredSolenoidsState, 0,
sizeof(hph->desiredSolenoidsState));
std::memset(&hph->versionInfo, 0, sizeof(hph->versionInfo));
// Start closed-loop compressor control by starting solenoid state updates
HAL_SendREVPHSolenoidsState(hph.get(), status);
return handle;
}
void HAL_FreeREVPH(HAL_REVPHHandle handle) {
auto hph = REVPHHandles->Get(handle);
if (hph == nullptr)
return;
HAL_CleanCAN(hph->hcan);
REVPHHandles->Free(handle);
}
HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) {
return module >= 1 && module < kNumREVPDHModules;
}
HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) {
return channel >= 0 && channel < kNumREVPHChannels;
}
HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return false;
}
return status0.compressor_on;
}
void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle,
const HAL_REVPHCompressorConfig* config,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PH_compressor_config_t frameData;
frameData.minimum_tank_pressure =
PH_compressor_config_minimum_tank_pressure_encode(
config->minAnalogVoltage);
frameData.maximum_tank_pressure =
PH_compressor_config_maximum_tank_pressure_encode(
config->maxAnalogVoltage);
frameData.force_disable = config->forceDisable;
frameData.use_digital = config->useDigital;
uint8_t packedData[PH_COMPRESSOR_CONFIG_LENGTH] = {0};
PH_compressor_config_pack(packedData, &frameData,
PH_COMPRESSOR_CONFIG_LENGTH);
HAL_WriteCANPacket(ph->hcan, packedData, PH_COMPRESSOR_CONFIG_LENGTH,
PH_COMPRESSOR_CONFIG_API, status);
}
void HAL_SetREVPHClosedLoopControlDisabled(HAL_REVPHHandle handle,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.forceDisable = true;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
void HAL_SetREVPHClosedLoopControlDigital(HAL_REVPHHandle handle,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.useDigital = true;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
void HAL_SetREVPHClosedLoopControlAnalog(HAL_REVPHHandle handle,
double minAnalogVoltage,
double maxAnalogVoltage,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.minAnalogVoltage = minAnalogVoltage;
config.maxAnalogVoltage = maxAnalogVoltage;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
void HAL_SetREVPHClosedLoopControlHybrid(HAL_REVPHHandle handle,
double minAnalogVoltage,
double maxAnalogVoltage,
int32_t* status) {
HAL_REVPHCompressorConfig config = {0, 0, 0, 0};
config.minAnalogVoltage = minAnalogVoltage;
config.maxAnalogVoltage = maxAnalogVoltage;
config.useDigital = true;
HAL_SetREVPHCompressorConfig(handle, &config, status);
}
HAL_REVPHCompressorConfigType HAL_GetREVPHCompressorConfig(
HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return HAL_REVPHCompressorConfigType_kDisabled;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return HAL_REVPHCompressorConfigType_kDisabled;
}
return static_cast<HAL_REVPHCompressorConfigType>(status0.compressor_config);
}
HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return false;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return false;
}
return status0.digital_sensor;
}
double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_compressor_current_decode(status1.compressor_current);
}
double HAL_GetREVPHAnalogVoltage(HAL_REVPHHandle handle, int32_t channel,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
if (channel < 0 || channel > 1) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastErrorIndexOutOfRange(status, "Invalid REV Analog Index", 0, 2,
channel);
return 0;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return 0;
}
if (channel == 0) {
return PH_status_0_analog_0_decode(status0.analog_0);
}
return PH_status_0_analog_1_decode(status0.analog_1);
}
double HAL_GetREVPHVoltage(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_v_bus_decode(status1.v_bus);
}
double HAL_GetREVPH5VVoltage(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_supply_voltage_5_v_decode(status1.supply_voltage_5_v);
}
double HAL_GetREVPHSolenoidCurrent(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_solenoid_current_decode(status1.solenoid_current);
}
double HAL_GetREVPHSolenoidVoltage(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
if (*status != 0) {
return 0;
}
return PH_status_1_solenoid_voltage_decode(status1.solenoid_voltage);
}
void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
int32_t* status) {
std::memset(version, 0, sizeof(*version));
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_version_t result = {};
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (ph->versionInfo.firmwareMajor > 0) {
version->firmwareMajor = ph->versionInfo.firmwareMajor;
version->firmwareMinor = ph->versionInfo.firmwareMinor;
version->firmwareFix = ph->versionInfo.firmwareFix;
version->hardwareMajor = ph->versionInfo.hardwareMajor;
version->hardwareMinor = ph->versionInfo.hardwareMinor;
version->uniqueId = ph->versionInfo.uniqueId;
*status = 0;
return;
}
HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API,
status);
if (*status != 0) {
return;
}
uint32_t timeoutMs = 100;
for (uint32_t i = 0; i <= timeoutMs; i++) {
HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, packedData, &length,
&timestamp, status);
if (*status == 0) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
if (*status != 0) {
return;
}
PH_version_unpack(&result, packedData, PH_VERSION_LENGTH);
version->firmwareMajor = result.firmware_year;
version->firmwareMinor = result.firmware_minor;
version->firmwareFix = result.firmware_fix;
version->hardwareMinor = result.hardware_minor;
version->hardwareMajor = result.hardware_major;
version->uniqueId = result.unique_id;
ph->versionInfo = *version;
}
int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return 0;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
if (*status != 0) {
return 0;
}
uint32_t result = status0.channel_0;
result |= status0.channel_1 << 1;
result |= status0.channel_2 << 2;
result |= status0.channel_3 << 3;
result |= status0.channel_4 << 4;
result |= status0.channel_5 << 5;
result |= status0.channel_6 << 6;
result |= status0.channel_7 << 7;
result |= status0.channel_8 << 8;
result |= status0.channel_9 << 9;
result |= status0.channel_10 << 10;
result |= status0.channel_11 << 11;
result |= status0.channel_12 << 12;
result |= status0.channel_13 << 13;
result |= status0.channel_14 << 14;
result |= status0.channel_15 << 15;
return result;
}
void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
std::scoped_lock lock{ph->solenoidLock};
for (int solenoid = 0; solenoid < kNumREVPHChannels; solenoid++) {
if (mask & (1 << solenoid)) {
// The mask bit for the solenoid is set, so we update the solenoid state
REV_SolenoidState desiredSolenoidState =
values & (1 << solenoid) ? kSolenoidEnabled : kSolenoidDisabled;
HAL_UpdateDesiredREVPHSolenoidState(ph.get(), solenoid,
desiredSolenoidState);
}
}
HAL_SendREVPHSolenoidsState(ph.get(), status);
}
void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
if (index >= kNumREVPHChannels || index < 0) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Only [0-15] are valid index values. Requested {}", index));
return;
}
if (!HAL_CheckREVPHPulseTime(durMs)) {
*status = PARAMETER_OUT_OF_RANGE;
hal::SetLastError(
status,
fmt::format("Time not within expected range [0-65534]. Requested {}",
durMs));
return;
}
{
std::scoped_lock lock{ph->solenoidLock};
HAL_UpdateDesiredREVPHSolenoidState(ph.get(), index,
kSolenoidControlledViaPulse);
HAL_SendREVPHSolenoidsState(ph.get(), status);
}
if (*status != 0) {
return;
}
PH_pulse_once_t pulse = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
pulse.pulse_length_ms = durMs;
// Specify which solenoid should be pulsed
// The protocol supports specifying any number of solenoids to be pulsed at
// the same time, should that functionality be exposed to users in the future.
switch (index) {
case 0:
pulse.channel_0 = true;
break;
case 1:
pulse.channel_1 = true;
break;
case 2:
pulse.channel_2 = true;
break;
case 3:
pulse.channel_3 = true;
break;
case 4:
pulse.channel_4 = true;
break;
case 5:
pulse.channel_5 = true;
break;
case 6:
pulse.channel_6 = true;
break;
case 7:
pulse.channel_7 = true;
break;
case 8:
pulse.channel_8 = true;
break;
case 9:
pulse.channel_9 = true;
break;
case 10:
pulse.channel_10 = true;
break;
case 11:
pulse.channel_11 = true;
break;
case 12:
pulse.channel_12 = true;
break;
case 13:
pulse.channel_13 = true;
break;
case 14:
pulse.channel_14 = true;
break;
case 15:
pulse.channel_15 = true;
break;
}
// Send pulse command
uint8_t packedData[PH_PULSE_ONCE_LENGTH] = {0};
PH_pulse_once_pack(packedData, &pulse, PH_PULSE_ONCE_LENGTH);
HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH,
PH_PULSE_ONCE_FRAME_API, status);
}
void HAL_GetREVPHFaults(HAL_REVPHHandle handle, HAL_REVPHFaults* faults,
int32_t* status) {
std::memset(faults, 0, sizeof(*faults));
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PH_status_0_t status0 = HAL_ReadREVPHStatus0(ph->hcan, status);
faults->channel0Fault = status0.channel_0_fault;
faults->channel1Fault = status0.channel_1_fault;
faults->channel2Fault = status0.channel_2_fault;
faults->channel3Fault = status0.channel_3_fault;
faults->channel4Fault = status0.channel_4_fault;
faults->channel5Fault = status0.channel_5_fault;
faults->channel6Fault = status0.channel_6_fault;
faults->channel7Fault = status0.channel_7_fault;
faults->channel8Fault = status0.channel_8_fault;
faults->channel9Fault = status0.channel_9_fault;
faults->channel10Fault = status0.channel_10_fault;
faults->channel11Fault = status0.channel_11_fault;
faults->channel12Fault = status0.channel_12_fault;
faults->channel13Fault = status0.channel_13_fault;
faults->channel14Fault = status0.channel_14_fault;
faults->channel15Fault = status0.channel_15_fault;
faults->compressorOverCurrent = status0.compressor_oc_fault;
faults->compressorOpen = status0.compressor_open_fault;
faults->solenoidOverCurrent = status0.solenoid_oc_fault;
faults->brownout = status0.brownout_fault;
faults->canWarning = status0.can_warning_fault;
faults->hardwareFault = status0.hardware_fault;
}
void HAL_GetREVPHStickyFaults(HAL_REVPHHandle handle,
HAL_REVPHStickyFaults* stickyFaults,
int32_t* status) {
std::memset(stickyFaults, 0, sizeof(*stickyFaults));
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
PH_status_1_t status1 = HAL_ReadREVPHStatus1(ph->hcan, status);
stickyFaults->compressorOverCurrent = status1.sticky_compressor_oc_fault;
stickyFaults->compressorOpen = status1.sticky_compressor_open_fault;
stickyFaults->solenoidOverCurrent = status1.sticky_solenoid_oc_fault;
stickyFaults->brownout = status1.sticky_brownout_fault;
stickyFaults->canWarning = status1.sticky_can_warning_fault;
stickyFaults->canBusOff = status1.sticky_can_bus_off_fault;
stickyFaults->hasReset = status1.sticky_has_reset_fault;
}
void HAL_ClearREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status) {
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t packedData[8] = {0};
HAL_WriteCANPacket(ph->hcan, packedData, PH_CLEAR_FAULTS_LENGTH,
PH_CLEAR_FAULTS_FRAME_API, status);
}