mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
74 lines
2.1 KiB
C++
74 lines
2.1 KiB
C++
|
|
#include "HAL/Solenoid.h"
|
|
|
|
#include "Port.h"
|
|
#include "HAL/Errors.h"
|
|
#include "ChipObject.h"
|
|
#include "HAL/cpp/Synchronized.h"
|
|
#include "NetworkCommunication/LoadOut.h"
|
|
|
|
struct solenoid_port_t {
|
|
Port port;
|
|
tSolenoid *module;
|
|
uint32_t PWMGeneratorID;
|
|
};
|
|
typedef struct solenoid_port_t SolenoidPort;
|
|
|
|
static ReentrantSemaphore solenoidSemaphore;
|
|
static tSolenoid* solenoidModules[2] = {NULL, NULL};
|
|
|
|
bool solenoidModulesInitialized = false;
|
|
|
|
/**
|
|
* Initialize the digital modules.
|
|
*/
|
|
void initializeSolenoid(int32_t *status) {
|
|
if (solenoidModulesInitialized) return;
|
|
|
|
for (unsigned int i = 0; i < (sizeof(solenoidModules)/sizeof(solenoidModules[0])); i++) {
|
|
Synchronized sync(solenoidSemaphore);
|
|
solenoidModules[i] = tSolenoid::create(status);
|
|
}
|
|
solenoidModulesInitialized = true;
|
|
}
|
|
|
|
void* initializeSolenoidPort(void* port_pointer, int32_t *status) {
|
|
initializeSolenoid(status);
|
|
Port* port = (Port*) port_pointer;
|
|
|
|
// Initialize port structure
|
|
SolenoidPort* solenoid_port = new SolenoidPort();
|
|
solenoid_port->port = *port;
|
|
solenoid_port->module = solenoidModules[solenoid_port->port.module-1];
|
|
|
|
return solenoid_port;
|
|
}
|
|
|
|
bool checkSolenoidModule(uint8_t module) {
|
|
if (nLoadOut::getModulePresence(nLoadOut::kModuleType_Solenoid, module - 1))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
bool getSolenoid(void* solenoid_port_pointer, int32_t *status) {
|
|
SolenoidPort* port = (SolenoidPort*) solenoid_port_pointer;
|
|
if (checkSolenoidModule(port->port.module)) {
|
|
uint8_t mask = 1 << (port->port.pin - 1);
|
|
return (mask & port->module->readDO7_0(port->port.module - 1, status));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status) {
|
|
SolenoidPort* port = (SolenoidPort*) solenoid_port_pointer;
|
|
if (checkSolenoidModule(port->port.module)) {
|
|
Synchronized sync(solenoidSemaphore);
|
|
uint8_t currentValue = port->module->readDO7_0(port->port.module - 1, status);
|
|
uint8_t mask = 1 << (port->port.pin - 1);
|
|
if (value) currentValue = currentValue | mask; // Flip the bit on
|
|
else currentValue = currentValue & ~mask; // Flip the bit off
|
|
port->module->writeDO7_0(port->port.module - 1, currentValue, status);
|
|
}
|
|
}
|
|
|