/**
* Get the value of the axis on a joystick.
* This depends on the mapping of the joystick connected to the specified port.
*
* @param stick The joystick to read.
* @param axis The analog axis value to read from the joystick.
* @return The value of the axis on the joystick.
*/
float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
{
if (stick >= kJoystickPorts)
{
wpi_setWPIError(BadJoystickIndex);
return 0;
}
if (axis >= m_joystickAxes[stick].count)
{
if (axis >= kMaxJoystickAxes)
wpi_setWPIError(BadJoystickAxis);
else
ReportJoystickUnpluggedError("WARNING: Joystick Axis missing, check if all controllers are plugged in\n");
return 0.0f;
}
int8_t value = m_joystickAxes[stick].axes[axis];
if(value < 0)
{
return value / 128.0f;
}
else
{
return value / 127.0f;
}
}
/**
* Constructor.
*
* @param moduleNumber The digital module that the sensor is plugged into (1 or 2).
*/
HiTechnicColorSensor::HiTechnicColorSensor(UINT8 moduleNumber)
: m_i2c (NULL)
{
m_table = NULL;
DigitalModule *module = DigitalModule::GetInstance(moduleNumber);
m_mode = kActive;
if (module)
{
m_i2c = module->GetI2C(kAddress);
// Verify Sensor
const UINT8 kExpectedManufacturer[] = "HiTechnc";
const UINT8 kExpectedSensorType[] = "ColorPD ";
if ( ! m_i2c->VerifySensor(kManufacturerBaseRegister, kManufacturerSize, kExpectedManufacturer) )
{
wpi_setWPIError(CompassManufacturerError);
return;
}
if ( ! m_i2c->VerifySensor(kSensorTypeBaseRegister, kSensorTypeSize, kExpectedSensorType) )
{
wpi_setWPIError(CompassTypeError);
}
nUsageReporting::report(nUsageReporting::kResourceType_HiTechnicColorSensor, moduleNumber - 1);
}
}
/**
* Free an allocated resource.
* After a resource is no longer needed, for example a destructor is called for
* a channel assignment
* class, Free will release the resource value so it can be reused somewhere
* else in the program.
*/
void Resource::Free(uint32_t index) {
std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
if (index == std::numeric_limits<uint32_t>::max()) return;
if (index >= m_isAllocated.size()) {
wpi_setWPIError(NotAllocated);
return;
}
if (!m_isAllocated[index]) {
wpi_setWPIError(NotAllocated);
return;
}
m_isAllocated[index] = false;
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the
* object.
*
* When set to kBothDirections, the relay can be any of the four states:
* 0v-0v, 0v-12v, 12v-0v, 12v-12v
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for
* the direction or you can simply specify kOff and kOn. Using only kOff and
* kOn is recommended.
*
* @param value The state to set the relay.
*/
void Relay::Set(Relay::Value value) {
if (StatusIsFatal()) return;
int32_t status = 0;
switch (value) {
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
HAL_SetRelay(m_forwardHandle, false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
HAL_SetRelay(m_reverseHandle, false, &status);
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
HAL_SetRelay(m_forwardHandle, true, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
HAL_SetRelay(m_reverseHandle, true, &status);
}
break;
case kForward:
if (m_direction == kReverseOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
HAL_SetRelay(m_forwardHandle, true, &status);
}
if (m_direction == kBothDirections) {
HAL_SetRelay(m_reverseHandle, false, &status);
}
break;
case kReverse:
if (m_direction == kForwardOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections) {
HAL_SetRelay(m_forwardHandle, false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
HAL_SetRelay(m_reverseHandle, true, &status);
}
break;
}
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the
* object.
*
* When set to kBothDirections, the relay can be any of the four states:
* 0v-0v, 0v-12v, 12v-0v, 12v-12v
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for
* the
* direction or you can simply specify kOff and kOn. Using only kOff and
* kOn is
* recommended.
*
* @param value The state to set the relay.
*/
void Relay::Set(Relay::Value value) {
if (StatusIsFatal()) return;
int32_t status = 0;
switch (value) {
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
setRelayForward(m_relay_ports[m_channel], false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
setRelayReverse(m_relay_ports[m_channel], false, &status);
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
setRelayForward(m_relay_ports[m_channel], true, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
setRelayReverse(m_relay_ports[m_channel], true, &status);
}
break;
case kForward:
if (m_direction == kReverseOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
setRelayForward(m_relay_ports[m_channel], true, &status);
}
if (m_direction == kBothDirections) {
setRelayReverse(m_relay_ports[m_channel], false, &status);
}
break;
case kReverse:
if (m_direction == kForwardOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections) {
setRelayForward(m_relay_ports[m_channel], false, &status);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
setRelayReverse(m_relay_ports[m_channel], true, &status);
}
break;
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Validate that the data being packed will fit in the buffer.
*/
bool Dashboard::ValidateAdd(INT32 size)
{
if ((m_packPtr - m_localBuffer) + size > kMaxDashboardDataSize)
{
wpi_setWPIError(DashboardDataOverflow);
return false;
}
// Make sure printf is not being used at the same time.
if (m_localPrintBuffer[0] != 0)
{
wpi_setWPIError(DashboardDataCollision);
return false;
}
return true;
}
/**
* Returns a boolean indicating if the controller is an xbox controller.
*
* @param stick The joystick port number
* @return A boolean that is true if the controller is an xbox controller.
*/
bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return false;
}
return (bool)m_joystickDescriptor[stick].isXbox;
}
/**
* Set the sample rate on the module.
*
* This is a global setting for the module and effects all channels.
*
* @param samplesPerSecond The number of samples per channel per second.
*/
void AnalogModule::SetSampleRate(float samplesPerSecond)
{
// TODO: This will change when variable size scan lists are implemented.
// TODO: Need float comparison with epsilon.
//wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
m_sampleRateSet = true;
// Compute the convert rate
UINT32 ticksPerSample = (UINT32)((float)kTimebase / samplesPerSecond);
UINT32 ticksPerConversion = ticksPerSample / GetNumChannelsToActivate();
// ticksPerConversion must be at least 80
if (ticksPerConversion < 80)
{
wpi_setWPIError(SampleRateTooHigh);
ticksPerConversion = 80;
}
// Atomically set the scan size and the convert rate so that the sample rate is constant
tAI::tConfig config;
config.ScanSize = GetNumChannelsToActivate();
config.ConvertRate = ticksPerConversion;
tRioStatusCode localStatus = NiFpga_Status_Success;
m_module->writeConfig(config, &localStatus);
wpi_setError(localStatus);
// Indicate that the scan size has been commited to hardware.
SetNumChannelsToActivate(0);
}
/**
* Returns the name of the joystick at the given port
*
* @param stick The joystick port number
* @return The name of the joystick at the given port
*/
std::string DriverStation::GetJoystickName(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
}
std::string retVal(m_joystickDescriptor[stick].name);
return retVal;
}
/**
* Start the task that is responsible for sending images to the PC.
*/
int PCVideoServer::StartServerTask()
{
if (StatusIsFatal())
{
return -1;
}
int id = 0;
m_stopServer = false;
// Check for prior copy of running task
int oldId = taskNameToId((char*)m_serverTask.GetName());
if(oldId != ERROR)
{
// TODO: Report error. You are in a bad state.
taskDelete(oldId);
}
// spawn video server task
// this is done to ensure that the task is spawned with the
// floating point context save parameter.
bool started = m_serverTask.Start((int)this);
id = m_serverTask.GetID();
if (!started)
{
wpi_setWPIError(TaskError);
return id;
}
taskDelay(1);
return id;
}
/**
* Indicate that the packing is complete and commit the buffer to the DriverStation.
*
* The packing of the dashboard packet is complete.
* If you are not using the packed dashboard data, you can call Finalize() to commit the Printf() buffer and the error string buffer.
* In effect, you are packing an empty structure.
* Prepares a packet to go to the dashboard...
* @return The total size of the data packed into the userData field of the status packet.
*/
INT32 Dashboard::Finalize()
{
if (!m_complexTypeStack.empty())
{
wpi_setWPIError(MismatchedComplexTypeClose);
return 0;
}
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_Dashboard, 0);
reported = true;
}
Synchronized sync(m_statusDataSemaphore);
// Sequence number
DriverStation::GetInstance()->IncrementUpdateNumber();
// Packed Dashboard Data
m_userStatusDataSize = m_packPtr - m_localBuffer;
memcpy(m_userStatusData, m_localBuffer, m_userStatusDataSize);
m_packPtr = m_localBuffer;
return m_userStatusDataSize;
}
/**
* Returns the types of Axes on a given joystick port
*
* @param stick The joystick port number and the target axis
* @return What type of axis the axis is reporting to be
*/
int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
return m_joystickDescriptor[stick].axisTypes[axis];
}
/**
* Returns the type of joystick at a given port
*
* @param stick The joystick port number
* @return The HID type of joystick at the given port
*/
int DriverStation::GetJoystickType(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
return (int)m_joystickDescriptor[stick].type;
}
/**
* Free an allocated resource.
* After a resource is no longer needed, for example a destructor is called for a channel assignment
* class, Free will release the resource value so it can be reused somewhere else in the program.
*/
void Resource::Free(uint32_t index)
{
Synchronized sync(m_allocateLock);
if (index == ~0ul) return;
if (index >= m_size)
{
wpi_setWPIError(NotAllocated);
return;
}
if (!m_isAllocated[index])
{
wpi_setWPIError(NotAllocated);
return;
}
m_isAllocated[index] = false;
}
/**
* Convert a voltage to a raw value for a specified channel.
*
* This process depends on the calibration of each channel, so the channel
* must be specified.
*
* @todo This assumes raw values. Oversampling not supported as is.
*
* @param channel The channel to convert for.
* @param voltage The voltage to convert.
* @return The raw value for the channel.
*/
INT32 AnalogModule::VoltsToValue(INT32 channel, float voltage)
{
if (voltage > 10.0)
{
voltage = 10.0;
wpi_setWPIError(VoltageOutOfRange);
}
if (voltage < -10.0)
{
voltage = -10.0;
wpi_setWPIError(VoltageOutOfRange);
}
UINT32 LSBWeight = GetLSBWeight(channel);
INT32 offset = GetOffset(channel);
INT32 value = (INT32) ((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9));
return value;
}
/**
* Provide tank steering using the stored robot configuration.
* Drive the robot using two joystick inputs. The Y-axis will be selected from
* each Joystick object.
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
*/
void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick,
bool squaredInputs) {
if (leftStick == nullptr || rightStick == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
}
/**
* Create a new instance of Accelerometer from an existing AnalogInput.
*
* Make a new instance of accelerometer given an AnalogInput. This is
* particularly useful if the port is going to be read as an analog channel as
* well as through the Accelerometer class.
*
* @param channel The existing AnalogInput object for the analog input the
* accelerometer is connected to
*/
AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
: m_analogInput(channel, NullDeleter<AnalogInput>()) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitAccelerometer();
}
}
/**
* Convert a voltage to a raw value for a specified channel.
*
* This process depends on the calibration of each channel, so the channel
* must be specified.
*
* @todo This assumes raw values. Oversampling not supported as is.
*
* @param channel The channel to convert for.
* @param voltage The voltage to convert.
* @return The raw value for the channel.
*/
int32_t AnalogModule::VoltsToValue(int32_t channel, float voltage)
{
if (voltage > 10.0)
{
voltage = 10.0;
wpi_setWPIError(VoltageOutOfRange);
}
if (voltage < -10.0)
{
voltage = -10.0;
wpi_setWPIError(VoltageOutOfRange);
}
uint32_t LSBWeight = GetLSBWeight(channel);
int32_t offset = GetOffset(channel);
int32_t value = (int32_t) ((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9));
return value;
}
/**
* Get the state of a POV on the joystick.
*
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
*/
int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
if (pov >= m_joystickPOVs[stick].count) {
if (pov >= kMaxJoystickPOVs)
wpi_setWPIError(BadJoystickAxis);
else
ReportJoystickUnpluggedWarning(
"Joystick POV missing, check if all controllers are plugged in");
return -1;
}
return m_joystickPOVs[stick].povs[pov];
}
/**
* Create a new instance of Accelerometer from an existing AnalogInput.
*
* Make a new instance of accelerometer given an AnalogInput. This is
* particularly useful if the port is going to be read as an analog channel as
* well as through the Accelerometer class.
*
* @param channel The existing AnalogInput object for the analog input the
* accelerometer is connected to
*/
AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
: m_analogInput(channel) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitAccelerometer();
}
}
/**
* The state of the buttons on the joystick.
*
* @param stick The joystick to read.
* @return The state of the buttons on the joystick.
*/
uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
return m_joystickButtons[stick].buttons;
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the
* object.
*
* When set to kBothDirections, the relay can be any of the four states:
* 0v-0v, 0v-12v, 12v-0v, 12v-12v
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for
* the direction or you can simply specify kOff and kOn. Using only kOff and
* kOn is recommended.
*
* @param value The state to set the relay.
*/
void Relay::Set(Relay::Value value) {
switch (value) {
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
go_pos = false;
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
go_neg = false;
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
go_pos = true;
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
go_neg = true;
}
break;
case kForward:
if (m_direction == kReverseOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
go_pos = true;
}
if (m_direction == kBothDirections) {
go_neg = false;
}
break;
case kReverse:
if (m_direction == kForwardOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections) {
go_pos = false;
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
go_neg = true;
}
break;
}
impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0));
}
/**
* Enables or disables automatically turning the compressor on when the
* pressure is low.
* @param on Set to true to enable closed loop control of the compressor. False
* to disable.
*/
void Compressor::SetClosedLoopControl(bool on) {
int32_t status = 0;
setClosedLoopControl(m_pcm_pointer, on, &status);
if (status) {
wpi_setWPIError(Timeout);
}
}
/**
* Clear ALL sticky faults inside PCM that Compressor is wired to.
*
* If a sticky fault is set, then it will be persistently cleared. Compressor
* drive
* maybe momentarily disable while flags are being cleared. Care
* should be
* taken to not call this too frequently, otherwise normal
* compressor
* functionality may be prevented.
*
* If no sticky faults are set then this call will have no effect.
*/
void Compressor::ClearAllPCMStickyFaults() {
int32_t status = 0;
clearAllPCMStickyFaults(m_pcm_pointer, &status);
if (status) {
wpi_setWPIError(Timeout);
}
}
Encoder::Encoder(std::shared_ptr<DigitalSource> aSource,
std::shared_ptr<DigitalSource> bSource, bool reverseDirection,
EncodingType encodingType)
: m_aSource(aSource), m_bSource(bSource) {
if (m_aSource == nullptr || m_bSource == nullptr)
wpi_setWPIError(NullParameter);
else
InitEncoder(reverseDirection, encodingType);
}
/**
* Encoder constructor.
*
* Construct a Encoder given a and b channels as digital inputs. This is used in
* the case where the digital inputs are shared. The Encoder class will not
* allocate the digital inputs and assume that they already are counted.
*
* The counter will start counting immediately.
*
* @param aSource The source that should be used for the a channel.
* @param bSource the source that should be used for the b channel.
* @param reverseDirection represents the orientation of the encoder and
* inverts the output values if necessary so forward
* represents positive values.
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
* decoding. If 4X is selected, then an encoder FPGA
* object is used and the returned counts will be 4x
* the encoder spec'd value since all rising and
* falling edges are counted. If 1X or 2X are selected
* then a counter object will be used and the returned
* value will either exactly match the spec'd count or
* be double (2x) the spec'd count.
*/
Encoder::Encoder(DigitalSource* aSource, DigitalSource* bSource,
bool reverseDirection, EncodingType encodingType)
: m_aSource(aSource, NullDeleter<DigitalSource>()),
m_bSource(bSource, NullDeleter<DigitalSource>()) {
if (m_aSource == nullptr || m_bSource == nullptr)
wpi_setWPIError(NullParameter);
else
InitEncoder(reverseDirection, encodingType);
}
/**
* Gyro constructor with a precreated AnalogInput object.
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
: m_analog(channel) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitGyro();
Calibrate();
}
}
/*
* Invert a motor direction.
* This is used when a motor should run in the opposite direction as the drive
* code would normally run it. Motors that are direct drive would be inverted, the
* Drive code assumes that the motors are geared with one reversal.
* @param motor The motor index to invert.
* @param isInverted True if the motor should be inverted when operated.
*/
void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted)
{
if (motor < 0 || motor > 3)
{
wpi_setWPIError(InvalidMotorIndex);
return;
}
m_invertedMotors[motor] = isInverted ? -1 : 1;
}
/**
* Returns the number of buttons on a given joystick port
*
* @param stick The joystick port number
* @return The number of buttons on the indicated joystick
*/
int DriverStation::GetStickButtonCount(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
HALJoystickButtons joystickButtons;
HALGetJoystickButtons(stick, &joystickButtons);
return joystickButtons.count;
}
/**
* Indicate the end of a cluster packed into the dashboard data structure.
*
* After packing data into the cluster, call FinalizeCluster().
* Every call to AddCluster() must have a matching call to FinalizeCluster().
*/
void Dashboard::FinalizeCluster()
{
if (m_complexTypeStack.top() != kCluster)
{
wpi_setWPIError(MismatchedComplexTypeClose);
return;
}
m_complexTypeStack.pop();
AddedElement(kOther);
}
