/**
* Copy an image into an existing buffer.
* This copies an image into an existing buffer rather than creating a new image
* in memory. That way a new image is only allocated when the image being copied
* is
* larger than the destination.
* This method is called by the PCVideoServer class.
* @param imageData The destination image.
* @param numBytes The size of the destination image.
* @return 0 if failed (no source image or no memory), 1 if success.
*/
int AxisCamera::CopyJPEG(char **destImage, unsigned int &destImageSize,
unsigned int &destImageBufferSize) {
std::lock_guard<priority_mutex> lock(m_imageDataMutex);
if (destImage == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "destImage must not be nullptr");
return 0;
}
if (m_imageData.size() == 0) return 0; // if no source image
if (destImageBufferSize <
m_imageData.size()) // if current destination buffer too small
{
if (*destImage != nullptr) delete[] * destImage;
destImageBufferSize = m_imageData.size() + kImageBufferAllocationIncrement;
*destImage = new char[destImageBufferSize];
if (*destImage == nullptr) return 0;
}
// copy this image into destination buffer
if (*destImage == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "*destImage must not be nullptr");
}
std::copy(m_imageData.begin(), m_imageData.end(), *destImage);
destImageSize = m_imageData.size();
;
return 1;
}
/**
* Initialize PWMs given an module and channel.
*
* This method is private and is the common path for all the constructors for creating PWM
* instances. Checks module and channel value ranges and allocates the appropriate channel.
* The allocation is only done to help users ensure that they don't double assign channels.
*/
void PWM::InitPWM(UINT8 moduleNumber, UINT32 channel)
{
char buf[64];
Resource::CreateResourceObject(&allocated, tDIO::kNumSystems * kPwmChannels);
if (!CheckPWMModule(moduleNumber))
{
snprintf(buf, 64, "Digital Module %d", moduleNumber);
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return;
}
if (!CheckPWMChannel(channel))
{
snprintf(buf, 64, "PWM Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
snprintf(buf, 64, "PWM %d (Module: %d)", channel, moduleNumber);
if (allocated->Allocate((moduleNumber - 1) * kPwmChannels + channel - 1, buf) == ~0ul)
{
CloneError(allocated);
return;
}
m_channel = channel;
m_module = DigitalModule::GetInstance(moduleNumber);
m_module->SetPWM(m_channel, kPwmDisabled);
m_eliminateDeadband = false;
nUsageReporting::report(nUsageReporting::kResourceType_PWM, channel, moduleNumber - 1);
}
/**
* Constructor.
*
* @param moduleNumber The CAN ID of the PCM the solenoid is attached to
* @param channel The channel on the PCM to control (0..7).
*/
Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel)
: SolenoidBase(moduleNumber), m_channel(channel) {
std::stringstream buf;
if (!CheckSolenoidModule(m_moduleNumber)) {
buf << "Solenoid Module " << m_moduleNumber;
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf.str());
return;
}
if (!CheckSolenoidChannel(m_channel)) {
buf << "Solenoid Module " << m_channel;
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
Resource::CreateResourceObject(m_allocated, m_maxModules * m_maxPorts);
buf << "Solenoid " << m_channel << " (Module: " << m_moduleNumber << ")";
if (m_allocated->Allocate(m_moduleNumber * kSolenoidChannels + m_channel,
buf.str()) ==
std::numeric_limits<uint32_t>::max()) {
CloneError(*m_allocated);
return;
}
#if FULL_WPILIB
LiveWindow::GetInstance()->AddActuator("Solenoid", m_moduleNumber, m_channel,
this);
#endif
HALReport(HALUsageReporting::kResourceType_Solenoid, m_channel,
m_moduleNumber);
}
/**
* Print formatted text to the Driver Station LCD text bufer.
*
* Use UpdateLCD() periodically to actually send the test to the Driver Station.
*
* @param line The line on the LCD to print to.
* @param startingColumn The column to start printing to. This is a 1-based number.
* @param writeFmt The printf format string describing how to print.
*/
void DriverStationLCD::Printf(Line line, INT32 startingColumn, const char *writeFmt, ...)
{
va_list args;
UINT32 start = startingColumn - 1;
INT32 maxLength = kLineLength - start;
char lineBuffer[kLineLength + 1];
if (startingColumn < 1 || startingColumn > kLineLength)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "startingColumn");
return;
}
if (line < kMain_Line6 || line > kUser_Line6)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "line");
return;
}
va_start (args, writeFmt);
{
Synchronized sync(m_textBufferSemaphore);
// snprintf appends NULL to its output. Therefore we can't write directly to the buffer.
INT32 length = vsnprintf(lineBuffer, kLineLength + 1, writeFmt, args);
if (length < 0) length = kLineLength;
memcpy(m_textBuffer + start + line * kLineLength + sizeof(UINT16), lineBuffer, std::min(maxLength,length));
}
va_end (args);
}
/**
* Common function to implement constructor behavior.
*/
void Solenoid::InitSolenoid()
{
m_table = NULL;
char buf[64];
if (!CheckSolenoidModule(m_moduleNumber))
{
snprintf(buf, 64, "Solenoid Module %d", m_moduleNumber);
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return;
}
if (!CheckSolenoidChannel(m_channel))
{
snprintf(buf, 64, "Solenoid Channel %d", m_channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
Resource::CreateResourceObject(&m_allocated, tSolenoid::kNumDO7_0Elements * kSolenoidChannels);
snprintf(buf, 64, "Solenoid %d (Module: %d)", m_channel, m_moduleNumber);
if (m_allocated->Allocate((m_moduleNumber - 1) * kSolenoidChannels + m_channel - 1, buf) == ~0ul)
{
CloneError(m_allocated);
return;
}
LiveWindow::GetInstance()->AddActuator("Solenoid", m_moduleNumber, m_channel, this);
nUsageReporting::report(nUsageReporting::kResourceType_Solenoid, m_channel, m_moduleNumber - 1);
}
/**
* Creates a new command with the given name and timeout.
* @param name the name of the command
* @param timeout the time (in seconds) before this command "times out"
* @see Command#isTimedOut() isTimedOut()
*/
Command::Command(const char *name, double timeout)
{
if (name == NULL)
wpi_setWPIErrorWithContext(NullParameter, "name");
if (timeout < 0.0)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
InitCommand(name, timeout);
}
/**
* Handles errors generated by task related code.
*/
bool Task::HandleError(TASK_STATUS results) {
if (results != TASK_ERROR) return true;
int errsv = errno;
if (errsv == TaskLib_ILLEGAL_PRIORITY) {
wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName.c_str());
} else {
std::printf("ERROR: errno=%i", errsv);
wpi_setWPIErrorWithContext(TaskError, m_taskName.c_str());
}
return false;
}
/**
* Puts the given string into the preferences table.
*
* <p>The value may not have quotation marks, nor may the key
* have any whitespace nor an equals sign</p>
*
* <p>This will <b>NOT</b> save the value to memory between power cycles,
* to do that you must call {@link Preferences#Save() Save()} (which must be
* used with care).
* at some point after calling this.</p>
* @param key the key
* @param value the value
*/
void Preferences::PutString(const char *key, const char *value) {
if (value == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "value");
return;
}
if (std::string(value).find_first_of("\"") != std::string::npos) {
wpi_setWPIErrorWithContext(ParameterOutOfRange,
"value contains illegal characters");
return;
}
Put(key, value);
}
/**
* Allocate a specific resource value.
* The user requests a specific resource value, i.e. channel number and it is
* verified
* unallocated, then returned.
*/
uint32_t Resource::Allocate(uint32_t index, const std::string &resourceDesc) {
std::lock_guard<priority_recursive_mutex> sync(m_allocateLock);
if (index >= m_isAllocated.size()) {
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
return std::numeric_limits<uint32_t>::max();
}
if (m_isAllocated[index]) {
wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc);
return std::numeric_limits<uint32_t>::max();
}
m_isAllocated[index] = true;
return index;
}
/**
* Create a Notifier for timer event notification.
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
Notifier::Notifier(TimerEventHandler handler, void *param)
{
if (handler == NULL)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be NULL");
m_handler = handler;
m_param = param;
m_periodic = false;
m_expirationTime = 0;
m_period = 0;
m_nextEvent = NULL;
m_queued = false;
m_handlerSemaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
if (queueSemaphore == NULL)
{
queueSemaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
}
tRioStatusCode localStatus = NiFpga_Status_Success;
{
Synchronized sync(queueSemaphore);
// do the first time intialization of static variables
if (refcount == 0)
{
manager = new tInterruptManager(1 << kTimerInterruptNumber, false, &localStatus);
manager->registerHandler(ProcessQueue, NULL, &localStatus);
manager->enable(&localStatus);
talarm = tAlarm::create(&localStatus);
}
refcount++;
}
wpi_setError(localStatus);
}
/**
* Get the Differential gain of the controller.
*
* @return The differential gain.
*/
double CANJaguar::GetD()
{
uint8_t dataBuffer[8];
uint8_t dataSize;
switch(m_controlMode)
{
case kPercentVbus:
case kVoltage:
wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode");
break;
case kSpeed:
getTransaction(LM_API_SPD_DC, dataBuffer, &dataSize);
if (dataSize == sizeof(int32_t))
{
return unpackFXP16_16(dataBuffer);
}
break;
case kPosition:
getTransaction(LM_API_POS_DC, dataBuffer, &dataSize);
if (dataSize == sizeof(int32_t))
{
return unpackFXP16_16(dataBuffer);
}
break;
case kCurrent:
getTransaction(LM_API_ICTRL_DC, dataBuffer, &dataSize);
if (dataSize == sizeof(int32_t))
{
return unpackFXP16_16(dataBuffer);
}
break;
}
return 0.0;
}
/**
* Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link
* Scheduler} might know if a default {@link Command} needs to be run.
*
* All {@link Subsystem Subsystems} should call this.
*
* @param system the system
*/
void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
if (subsystem == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "subsystem");
return;
}
m_subsystems.insert(subsystem);
}
/**
* Set the source object that causes the counter to count down.
* Set the down counting DigitalSource.
*/
void xCounter::SetDownSource(DigitalSource *source)
{
if (StatusIsFatal()) return;
if (m_allocatedDownSource)
{
delete m_downSource;
m_downSource = NULL;
m_allocatedDownSource = false;
}
m_downSource = source;
if (m_downSource->StatusIsFatal())
{
CloneError(m_downSource);
}
else
{
tRioStatusCode localStatus = NiFpga_Status_Success;
unsigned char mode = m_counter->readConfig_Mode(&localStatus);
if (mode != kTwoPulse && mode != kExternalDirection)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports DownSource in TwoPulse and ExternalDirection modes.");
return;
}
m_counter->writeConfig_DownSource_Module(source->GetModuleForRouting(), &localStatus);
m_counter->writeConfig_DownSource_Channel(source->GetChannelForRouting(), &localStatus);
m_counter->writeConfig_DownSource_AnalogTrigger(source->GetAnalogTriggerForRouting(), &localStatus);
SetDownSourceEdge(true, false);
m_counter->strobeReset(&localStatus);
wpi_setError(localStatus);
}
}
/**
* Returns the value at the specified key.
* @param keyName the key
* @return the value
*/
SmartDashboardData *SmartDashboard::GetData(const char *keyName)
{
if (keyName == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "keyName");
return NULL;
}
NetworkTable *subtable = m_table->GetSubTable(keyName);
SmartDashboardData *data = m_tablesToData[subtable];
if (data == NULL)
{
wpi_setWPIErrorWithContext(SmartDashboardMissingKey, keyName);
return NULL;
}
return data;
}
/**
* Print formatted text to the Driver Station LCD text bufer. This function
* pads the line with empty spaces.
*
* Use UpdateLCD() periodically to actually send the test to the Driver Station.
*
* @param line The line on the LCD to print to.
* @param writeFmt The printf format string describing how to print.
*/
void DriverStationLCD::PrintfLine(Line line, const char *writeFmt, ...)
{
va_list args;
char lineBuffer[kLineLength + 1];
if (line < kMain_Line6 || line > kUser_Line6)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "line");
return;
}
va_start (args, writeFmt);
{
Synchronized sync(m_textBufferSemaphore);
// snprintf appends NULL to its output. Therefore we can't write directly to the buffer.
INT32 length = std::min(vsnprintf(lineBuffer, kLineLength + 1, writeFmt, args), kLineLength);
if (length < 0) length = kLineLength;
// Fill the rest of the buffer
if (length < kLineLength)
{
memset(lineBuffer + length, ' ', kLineLength - length);
}
memcpy(m_textBuffer + line * kLineLength + sizeof(UINT16), lineBuffer, kLineLength);
}
va_end (args);
}
/**
* This will cancel the current command.
* <p>This will cancel the current command eventually. It can be called multiple times.
* And it can be called when the command is not running. If the command is running though,
* then the command will be marked as canceled and eventually removed.</p>
* <p>A command can not be canceled
* if it is a part of a command group, you must cancel the command group instead.</p>
*/
void Command::Cancel()
{
if (m_parent != NULL)
wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not cancel a command that is part of a command group");
_Cancel();
}
/**
* Sets the timeout of this command.
* @param timeout the timeout (in seconds)
* @see Command#isTimedOut() isTimedOut()
*/
void Command::SetTimeout(double timeout)
{
if (timeout < 0.0)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
else
m_timeout = timeout;
}
xCounter::xCounter(EncodingType encodingType, DigitalSource *upSource, DigitalSource *downSource, bool inverted) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL),
m_encodingType(encodingType)
{
if (encodingType != k1X && encodingType != k2X)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports 1X and 2X quadrature decoding.");
return;
}
InitCounter(kExternalDirection);
SetUpSource(upSource);
SetDownSource(downSource);
tRioStatusCode localStatus = NiFpga_Status_Success;
if (encodingType == k1X)
{
SetUpSourceEdge(true, false);
m_counter->writeTimerConfig_AverageSize(1, &localStatus);
}
else
{
SetUpSourceEdge(true, true);
m_counter->writeTimerConfig_AverageSize(2, &localStatus);
}
wpi_setError(localStatus);
SetDownSourceEdge(inverted, true);
}
/**
* Construct an analog input.
*
* @param channel The channel number on the roboRIO to represent. 0-3 are
* on-board 4-7 are on the MXP port.
*/
AnalogInput::AnalogInput(uint32_t channel) {
std::stringstream buf;
buf << "Analog Input " << channel;
Resource::CreateResourceObject(inputs, kAnalogInputs);
if (!checkAnalogInputChannel(channel)) {
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
if (inputs->Allocate(channel, buf.str()) ==
std::numeric_limits<uint32_t>::max()) {
CloneError(*inputs);
return;
}
m_channel = channel;
void *port = getPort(channel);
int32_t status = 0;
m_port = initializeAnalogInputPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
HALReport(HALUsageReporting::kResourceType_AnalogChannel, channel);
}
/**
* Allocate a specific resource value.
* The user requests a specific resource value, i.e. channel number and it is verified
* unallocated, then returned.
*/
uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc)
{
Synchronized sync(m_allocateLock);
if (index >= m_size)
{
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
return ~0ul;
}
if ( m_isAllocated[index] )
{
wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc);
return ~0ul;
}
m_isAllocated[index] = true;
return index;
}
/**
* Initialize the gyro. Calibration is handled by Calibrate().
*/
void AnalogGyro::InitGyro() {
if (StatusIsFatal()) return;
if (!m_analog->IsAccumulatorChannel()) {
wpi_setWPIErrorWithContext(ParameterOutOfRange,
" channel (must be accumulator channel)");
m_analog = nullptr;
return;
}
m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
m_analog->SetAverageBits(kAverageBits);
m_analog->SetOversampleBits(kOversampleBits);
float sampleRate =
kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
m_analog->SetSampleRate(sampleRate);
Wait(0.1);
SetDeadband(0.0f);
SetPIDSourceType(PIDSourceType::kDisplacement);
HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), this);
}
/**
* Starts up the command. Gets the command ready to start.
* <p>Note that the command will eventually start, however it will not necessarily
* do so immediately, and may in fact be canceled before initialize is even called.</p>
*/
void Command::Start()
{
LockChanges();
if (m_parent != NULL)
wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not start a command that is part of a command group");
Scheduler::GetInstance()->AddCommand(this);
}
/**
* Returns the value at the given key.
* @param key the key
* @return the value (or empty if none exists)
*/
std::string Preferences::Get(const char *key) {
std::unique_lock<priority_recursive_mutex> sync(m_tableLock);
if (key == nullptr) {
wpi_setWPIErrorWithContext(NullParameter, "key");
return "";
}
return m_values[key];
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @deprecated Call Set instead.
*
* @param output Write out the PercentVbus value as was computed by the
* PIDController
*/
void CANTalon::PIDWrite(float output) {
if (GetControlMode() == kPercentVbus) {
Set(output);
} else {
wpi_setWPIErrorWithContext(IncompatibleMode,
"PID only supported in PercentVbus mode");
}
}
/**
* Maps the specified key to the specified value in this table.
* The key can not be NULL.
* The value can be retrieved by calling the get method with a key that is equal to the original key.
* @param keyName the key
* @param value the value
*/
void SmartDashboard::PutData(const char *keyName, SmartDashboardData *value)
{
if (keyName == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "keyName");
return;
}
if (value == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "value");
return;
}
NetworkTable *type = new NetworkTable();
type->PutString("~TYPE~", value->GetType());
type->PutSubTable("Data", value->GetTable());
m_table->PutSubTable(keyName, type);
m_tablesToData[type] = value;
}
/**
* Set the Samples to Average which specifies the number of samples of the timer to
* average when calculating the period. Perform averaging to account for
* mechanical imperfections or as oversampling to increase resolution.
* @param samplesToAverage The number of samples to average from 1 to 127.
*/
void Counter::SetSamplesToAverage (int samplesToAverage) {
tRioStatusCode localStatus = NiFpga_Status_Success;
if (samplesToAverage < 1 || samplesToAverage > 127)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Average counter values must be between 1 and 127");
}
m_counter->writeTimerConfig_AverageSize(samplesToAverage, &localStatus);
wpi_setError(localStatus);
}
/**
* Maps the specified key (where the key is the name of the {@link SmartDashboardNamedData}
* to the specified value in this table.
* The value can be retrieved by calling the get method with a key that is equal to the original key.
* @param value the value
*/
void SmartDashboard::PutData(SmartDashboardNamedData *value)
{
if (value == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "value");
return;
}
PutData(value->GetName().c_str(), value);
}
/**
* Maps the specified key to the specified value in this table.
* Neither the key nor the value can be null.
* The value can be retrieved by calling the get method with a key that is equal to the original key.
* @param keyName the key
* @param value the value
*/
void NetworkTable::PutSubTable(const char *keyName, NetworkTable *value)
{
if (value == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "value");
return;
}
Put(keyName, std::auto_ptr<NetworkTables::Entry>(new NetworkTables::TableEntry(value)));
}
void Catapult::PIDWriteSync(double output){
if (jag2->GetControlMode() == CANJaguar::kPercentVbus){
jag2->Set(output, 1);
jag3->Set(output, 1);
jag2->UpdateSyncGroup(1);
}
else
wpi_setWPIErrorWithContext(IncompatibleMode, "PID only supported in PercentVbus mode");
}
/**
* Configure the analog trigger to use a filtered value.
* The analog trigger will operate with a 3 point average rejection filter. This is designed to
* help with 360 degree pot applications for the period where the pot crosses through zero.
*/
void AnalogTrigger::SetFiltered(bool useFilteredValue)
{
if (StatusIsFatal()) return;
tRioStatusCode localStatus = NiFpga_Status_Success;
if (m_trigger->readSourceSelect_Averaged(&localStatus) != 0)
wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not support average and filtering at the same time.");
m_trigger->writeSourceSelect_Filter(useFilteredValue, &localStatus);
wpi_setError(localStatus);
}
