mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
420 lines
13 KiB
C++
420 lines
13 KiB
C++
// 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/CTREPCM.h"
|
|
|
|
#include <string>
|
|
|
|
#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"
|
|
|
|
using namespace hal;
|
|
|
|
static constexpr HAL_CANManufacturer manufacturer =
|
|
HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
|
|
|
|
static constexpr HAL_CANDeviceType deviceType =
|
|
HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics;
|
|
|
|
static constexpr int32_t Status1 = 0x50;
|
|
static constexpr int32_t StatusSolFaults = 0x51;
|
|
static constexpr int32_t StatusDebug = 0x52;
|
|
|
|
static constexpr int32_t Control1 = 0x70;
|
|
static constexpr int32_t Control2 = 0x71;
|
|
static constexpr int32_t Control3 = 0x72;
|
|
|
|
static constexpr int32_t TimeoutMs = 100;
|
|
static constexpr int32_t SendPeriod = 20;
|
|
|
|
union PcmStatus {
|
|
uint8_t data[8];
|
|
struct Bits {
|
|
/* Byte 0 */
|
|
unsigned SolenoidBits : 8;
|
|
/* Byte 1 */
|
|
unsigned compressorOn : 1;
|
|
unsigned stickyFaultFuseTripped : 1;
|
|
unsigned stickyFaultCompCurrentTooHigh : 1;
|
|
unsigned faultFuseTripped : 1;
|
|
unsigned faultCompCurrentTooHigh : 1;
|
|
unsigned faultHardwareFailure : 1;
|
|
unsigned isCloseloopEnabled : 1;
|
|
unsigned pressureSwitchEn : 1;
|
|
/* Byte 2*/
|
|
unsigned battVoltage : 8;
|
|
/* Byte 3 */
|
|
unsigned solenoidVoltageTop8 : 8;
|
|
/* Byte 4 */
|
|
unsigned compressorCurrentTop6 : 6;
|
|
unsigned solenoidVoltageBtm2 : 2;
|
|
/* Byte 5 */
|
|
unsigned StickyFault_dItooHigh : 1;
|
|
unsigned Fault_dItooHigh : 1;
|
|
unsigned moduleEnabled : 1;
|
|
unsigned closedLoopOutput : 1;
|
|
unsigned compressorCurrentBtm4 : 4;
|
|
/* Byte 6 */
|
|
unsigned tokenSeedTop8 : 8;
|
|
/* Byte 7 */
|
|
unsigned tokenSeedBtm8 : 8;
|
|
} bits;
|
|
};
|
|
|
|
union PcmControl {
|
|
uint8_t data[8];
|
|
struct Bits {
|
|
/* Byte 0 */
|
|
unsigned tokenTop8 : 8;
|
|
/* Byte 1 */
|
|
unsigned tokenBtm8 : 8;
|
|
/* Byte 2 */
|
|
unsigned solenoidBits : 8;
|
|
/* Byte 3*/
|
|
unsigned reserved : 4;
|
|
unsigned closeLoopOutput : 1;
|
|
unsigned compressorOn : 1;
|
|
unsigned closedLoopEnable : 1;
|
|
unsigned clearStickyFaults : 1;
|
|
/* Byte 4 */
|
|
unsigned OneShotField_h8 : 8;
|
|
/* Byte 5 */
|
|
unsigned OneShotField_l8 : 8;
|
|
} bits;
|
|
};
|
|
|
|
struct PcmControlSetOneShotDur {
|
|
uint8_t sol10MsPerUnit[8];
|
|
};
|
|
|
|
union PcmStatusFault {
|
|
uint8_t data[8];
|
|
struct Bits {
|
|
/* Byte 0 */
|
|
unsigned SolenoidDisabledList : 8;
|
|
/* Byte 1 */
|
|
unsigned reserved_bit0 : 1;
|
|
unsigned reserved_bit1 : 1;
|
|
unsigned reserved_bit2 : 1;
|
|
unsigned reserved_bit3 : 1;
|
|
unsigned StickyFault_CompNoCurrent : 1;
|
|
unsigned Fault_CompNoCurrent : 1;
|
|
unsigned StickyFault_SolenoidJumper : 1;
|
|
unsigned Fault_SolenoidJumper : 1;
|
|
} bits;
|
|
};
|
|
|
|
union PcmDebug {
|
|
uint8_t data[8];
|
|
struct Bits {
|
|
unsigned tokFailsTop8 : 8;
|
|
unsigned tokFailsBtm8 : 8;
|
|
unsigned lastFailedTokTop8 : 8;
|
|
unsigned lastFailedTokBtm8 : 8;
|
|
unsigned tokSuccessTop8 : 8;
|
|
unsigned tokSuccessBtm8 : 8;
|
|
} bits;
|
|
};
|
|
|
|
namespace {
|
|
struct PCM {
|
|
HAL_CANHandle canHandle;
|
|
wpi::mutex lock;
|
|
std::string previousAllocation;
|
|
PcmControl control;
|
|
PcmControlSetOneShotDur oneShot;
|
|
};
|
|
} // namespace
|
|
|
|
static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
|
|
HAL_HandleEnum::CTREPCM>* pcmHandles;
|
|
|
|
namespace hal::init {
|
|
void InitializeCTREPCM() {
|
|
static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
|
|
HAL_HandleEnum::CTREPCM>
|
|
pH;
|
|
pcmHandles = &pH;
|
|
}
|
|
} // namespace hal::init
|
|
|
|
#define READ_PACKET(type, frame, failureValue) \
|
|
auto pcm = pcmHandles->Get(handle); \
|
|
if (pcm == nullptr) { \
|
|
*status = HAL_HANDLE_ERROR; \
|
|
return failureValue; \
|
|
} \
|
|
HAL_CANReceiveMessage message; \
|
|
type pcmStatus; \
|
|
HAL_ReadCANPacketTimeout(pcm->canHandle, frame, &message, TimeoutMs, \
|
|
status); \
|
|
if (*status != 0) { \
|
|
return failureValue; \
|
|
} \
|
|
std::memcpy(pcmStatus.data, message.message.data, sizeof(pcmStatus.data));
|
|
|
|
#define READ_STATUS(failureValue) READ_PACKET(PcmStatus, Status1, failureValue)
|
|
#define READ_SOL_FAULTS(failureValue) \
|
|
READ_PACKET(PcmStatusFault, StatusSolFaults, failureValue)
|
|
|
|
static void SendControl(PCM* pcm, int32_t* status) {
|
|
HAL_CANMessage message;
|
|
std::memset(&message, 0, sizeof(message));
|
|
message.dataSize = 8;
|
|
std::memcpy(message.data, pcm->control.data, 8);
|
|
HAL_WriteCANPacketRepeating(pcm->canHandle, Control1, &message, SendPeriod,
|
|
status);
|
|
}
|
|
|
|
extern "C" {
|
|
|
|
HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t busId, int32_t module,
|
|
const char* allocationLocation,
|
|
int32_t* status) {
|
|
hal::init::CheckInit();
|
|
|
|
HAL_CTREPCMHandle handle;
|
|
auto pcm = pcmHandles->Allocate(module, &handle, status);
|
|
|
|
if (*status != 0) {
|
|
if (pcm) {
|
|
hal::SetLastErrorPreviouslyAllocated(status, "CTRE PCM", module,
|
|
pcm->previousAllocation);
|
|
} else {
|
|
hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0,
|
|
kNumCTREPCMModules - 1, module);
|
|
}
|
|
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
|
|
}
|
|
|
|
pcm->canHandle =
|
|
HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
|
|
if (*status != 0) {
|
|
pcmHandles->Free(handle);
|
|
return HAL_kInvalidHandle;
|
|
}
|
|
|
|
std::memset(&pcm->oneShot, 0, sizeof(pcm->oneShot));
|
|
std::memset(&pcm->control, 0, sizeof(pcm->control));
|
|
|
|
pcm->previousAllocation = allocationLocation ? allocationLocation : "";
|
|
|
|
// Enable closed loop control
|
|
HAL_SetCTREPCMClosedLoopControl(handle, true, status);
|
|
if (*status != 0) {
|
|
HAL_FreeCTREPCM(handle);
|
|
return HAL_kInvalidHandle;
|
|
}
|
|
return handle;
|
|
}
|
|
|
|
void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle) {
|
|
auto pcm = pcmHandles->Get(handle);
|
|
if (pcm) {
|
|
HAL_CleanCAN(pcm->canHandle);
|
|
}
|
|
pcmHandles->Free(handle);
|
|
}
|
|
|
|
HAL_Bool HAL_CheckCTREPCMSolenoidChannel(int32_t channel) {
|
|
return channel < kNumCTRESolenoidChannels && channel >= 0;
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMCompressor(HAL_CTREPCMHandle handle, int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.compressorOn;
|
|
}
|
|
|
|
void HAL_SetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, HAL_Bool enabled,
|
|
int32_t* status) {
|
|
auto pcm = pcmHandles->Get(handle);
|
|
if (pcm == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
|
|
int32_t can_status = 0;
|
|
|
|
std::scoped_lock lock{pcm->lock};
|
|
pcm->control.bits.closedLoopEnable = enabled ? 1 : 0;
|
|
SendControl(pcm.get(), &can_status);
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.isCloseloopEnabled;
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMPressureSwitch(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.pressureSwitchEn;
|
|
}
|
|
|
|
double HAL_GetCTREPCMCompressorCurrent(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(0);
|
|
uint32_t result = pcmStatus.bits.compressorCurrentTop6;
|
|
result <<= 4;
|
|
result |= pcmStatus.bits.compressorCurrentBtm4;
|
|
return result * 0.03125; /* 5.5 fixed pt value in Amps */
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighFault(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.faultCompCurrentTooHigh;
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(
|
|
HAL_CTREPCMHandle handle, int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.stickyFaultCompCurrentTooHigh;
|
|
}
|
|
HAL_Bool HAL_GetCTREPCMCompressorShortedStickyFault(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.Fault_dItooHigh;
|
|
}
|
|
HAL_Bool HAL_GetCTREPCMCompressorShortedFault(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.StickyFault_dItooHigh;
|
|
}
|
|
HAL_Bool HAL_GetCTREPCMCompressorNotConnectedStickyFault(
|
|
HAL_CTREPCMHandle handle, int32_t* status) {
|
|
READ_SOL_FAULTS(false);
|
|
return pcmStatus.bits.StickyFault_CompNoCurrent;
|
|
}
|
|
HAL_Bool HAL_GetCTREPCMCompressorNotConnectedFault(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_SOL_FAULTS(false);
|
|
return pcmStatus.bits.Fault_CompNoCurrent;
|
|
}
|
|
|
|
int32_t HAL_GetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t* status) {
|
|
READ_STATUS(0);
|
|
return pcmStatus.bits.SolenoidBits & 0xFF;
|
|
}
|
|
|
|
void HAL_SetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t mask,
|
|
int32_t values, int32_t* status) {
|
|
auto pcm = pcmHandles->Get(handle);
|
|
if (pcm == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
|
|
uint8_t smallMask = mask & 0xFF;
|
|
uint8_t smallValues =
|
|
(values & 0xFF) & smallMask; // Enforce only masked values are set
|
|
uint8_t invertMask = ~smallMask;
|
|
|
|
std::scoped_lock lock{pcm->lock};
|
|
uint8_t existingValue = invertMask & pcm->control.bits.solenoidBits;
|
|
pcm->control.bits.solenoidBits = existingValue | smallValues;
|
|
SendControl(pcm.get(), status);
|
|
}
|
|
|
|
int32_t HAL_GetCTREPCMSolenoidDisabledList(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_SOL_FAULTS(0);
|
|
return pcmStatus.bits.SolenoidDisabledList;
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMSolenoidVoltageStickyFault(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.stickyFaultFuseTripped;
|
|
}
|
|
|
|
HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
READ_STATUS(false);
|
|
return pcmStatus.bits.faultFuseTripped;
|
|
}
|
|
|
|
void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle,
|
|
int32_t* status) {
|
|
auto pcm = pcmHandles->Get(handle);
|
|
if (pcm == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
HAL_CANMessage message;
|
|
std::memset(&message, 0, sizeof(message));
|
|
message.dataSize = 4;
|
|
message.data[3] = 0x80;
|
|
HAL_WriteCANPacket(pcm->canHandle, Control2, &message, status);
|
|
}
|
|
|
|
void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index,
|
|
int32_t* status) {
|
|
if (index > 7 || index < 0) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
hal::SetLastError(
|
|
status,
|
|
fmt::format("Only [0-7] are valid index values. Requested {}", index));
|
|
return;
|
|
}
|
|
|
|
auto pcm = pcmHandles->Get(handle);
|
|
if (pcm == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
|
|
std::scoped_lock lock{pcm->lock};
|
|
uint16_t oneShotField = pcm->control.bits.OneShotField_h8;
|
|
oneShotField <<= 8;
|
|
oneShotField |= pcm->control.bits.OneShotField_l8;
|
|
|
|
uint16_t shift = 2 * index;
|
|
uint16_t mask = 3;
|
|
uint8_t chBits = (oneShotField >> shift) & mask;
|
|
chBits = (chBits % 3) + 1;
|
|
oneShotField &= ~(mask << shift);
|
|
oneShotField |= (chBits << shift);
|
|
pcm->control.bits.OneShotField_h8 = oneShotField >> 8;
|
|
pcm->control.bits.OneShotField_l8 = oneShotField;
|
|
SendControl(pcm.get(), status);
|
|
}
|
|
|
|
void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
|
|
int32_t durMs, int32_t* status) {
|
|
if (index > 7 || index < 0) {
|
|
*status = PARAMETER_OUT_OF_RANGE;
|
|
hal::SetLastError(
|
|
status,
|
|
fmt::format("Only [0-7] are valid index values. Requested {}", index));
|
|
return;
|
|
}
|
|
|
|
auto pcm = pcmHandles->Get(handle);
|
|
if (pcm == nullptr) {
|
|
*status = HAL_HANDLE_ERROR;
|
|
return;
|
|
}
|
|
|
|
HAL_CANMessage message;
|
|
std::memset(&message, 0, sizeof(message));
|
|
message.dataSize = 8;
|
|
|
|
std::scoped_lock lock{pcm->lock};
|
|
pcm->oneShot.sol10MsPerUnit[index] = (std::min)(
|
|
static_cast<uint32_t>(durMs) / 10, static_cast<uint32_t>(0xFF));
|
|
std::memcpy(message.data, pcm->oneShot.sol10MsPerUnit, 8);
|
|
HAL_WriteCANPacketRepeating(pcm->canHandle, Control3, &message, SendPeriod,
|
|
status);
|
|
}
|
|
|
|
} // extern "C"
|