Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

CMake Changes

Browse Source 
This is the changes made by Patrick Plenefisch converting the native
code to use CMake and the CMake Maven Plugin, as opposed to the
native Maven plugin. This is to allow for compatibility with newer
versions of the GCC toolchain. All the cpp sources were moved from
maven style directories to cpp style directories for CMake.

Change-Id: I67f5e3608948f37c83b0990d232105a3784f8593
This commit is contained in:
Brad Miller
2014-03-24 16:13:08 -04:00
parent 33134bef1d
commit 69d9ad70ab
804 changed files with 586 additions and 9377 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
wpilibc/src/main/native/ADXL345_I2C.cpp
View File

@@ -1,94 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "ADXL345_I2C.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "I2C.h"
const uint8_t ADXL345_I2C::kAddress;
const uint8_t ADXL345_I2C::kPowerCtlRegister;
const uint8_t ADXL345_I2C::kDataFormatRegister;
const uint8_t ADXL345_I2C::kDataRegister;
constexpr double ADXL345_I2C::kGsPerLSB;
/**
* Constructor.
*
* @param moduleNumber The digital module that the sensor is plugged into (1 or 2).
* @param range The range (+ or -) that the accelerometer will measure.
*/
ADXL345_I2C::ADXL345_I2C(uint8_t moduleNumber, ADXL345_I2C::DataFormat_Range range)
: m_i2c (NULL)
{
DigitalModule *module = DigitalModule::GetInstance(moduleNumber);
if (module)
{
m_i2c = module->GetI2C(kAddress);
// Turn on the measurements
m_i2c->Write(kPowerCtlRegister, kPowerCtl_Measure);
// Specify the data format to read
m_i2c->Write(kDataFormatRegister, kDataFormat_FullRes | (uint8_t)range);
HALReport(HALUsageReporting::kResourceType_ADXL345, HALUsageReporting::kADXL345_I2C, moduleNumber - 1);
}
}
/**
* Destructor.
*/
ADXL345_I2C::~ADXL345_I2C()
{
delete m_i2c;
m_i2c = NULL;
}
/**
* Get the acceleration of one axis in Gs.
*
* @param axis The axis to read from.
* @return Acceleration of the ADXL345 in Gs.
*/
double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis)
{
int16_t rawAccel = 0;
if(m_i2c)
{
m_i2c->Read(kDataRegister + (uint8_t)axis, sizeof(rawAccel), (uint8_t *)&rawAccel);
// Sensor is little endian... swap bytes
rawAccel = ((rawAccel >> 8) & 0xFF) | (rawAccel << 8);
}
return rawAccel * kGsPerLSB;
}
/**
* Get the acceleration of all axes in Gs.
*
* @return Acceleration measured on all axes of the ADXL345 in Gs.
*/
ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations()
{
AllAxes data = {0.0};
int16_t rawData[3];
if (m_i2c)
{
m_i2c->Read(kDataRegister, sizeof(rawData), (uint8_t*)rawData);
// Sensor is little endian... swap bytes
for (int32_t i=0; i<3; i++)
{
rawData[i] = ((rawData[i] >> 8) & 0xFF) | (rawData[i] << 8);
}
data.XAxis = rawData[0] * kGsPerLSB;
data.YAxis = rawData[1] * kGsPerLSB;
data.ZAxis = rawData[2] * kGsPerLSB;
}
return data;
}

213
wpilibc/src/main/native/ADXL345_SPI.cpp
View File

@@ -1,213 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "ADXL345_SPI.h"
#include "DigitalInput.h"
#include "DigitalOutput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "SPI.h"
const uint8_t ADXL345_SPI::kPowerCtlRegister;
const uint8_t ADXL345_SPI::kDataFormatRegister;
const uint8_t ADXL345_SPI::kDataRegister;
constexpr double ADXL345_SPI::kGsPerLSB;
/**
* Constructor.
*
* @param clk The GPIO the clock signal is wired to.
* @param mosi The GPIO the MOSI (Master Out Slave In) signal is wired to.
* @param miso The GPIO the MISO (Master In Slave Out) signal is wired to.
* @param cs The GPIO the CS (Chip Select) signal is wired to.
* @param range The range (+ or -) that the accelerometer will measure.
*/
ADXL345_SPI::ADXL345_SPI(DigitalOutput &clk, DigitalOutput &mosi, DigitalInput &miso,
DigitalOutput &cs, DataFormat_Range range)
: m_clk (NULL)
, m_mosi (NULL)
, m_miso (NULL)
, m_cs (NULL)
, m_spi (NULL)
{
Init(&clk, &mosi, &miso, &cs, range);
}
/**
* Constructor.
*
* @param clk The GPIO the clock signal is wired to.
* @param mosi The GPIO the MOSI (Master Out Slave In) signal is wired to.
* @param miso The GPIO the MISO (Master In Slave Out) signal is wired to.
* @param cs The GPIO the CS (Chip Select) signal is wired to.
* @param range The range (+ or -) that the accelerometer will measure.
*/
ADXL345_SPI::ADXL345_SPI(DigitalOutput *clk, DigitalOutput *mosi, DigitalInput *miso,
DigitalOutput *cs, DataFormat_Range range)
: m_clk (NULL)
, m_mosi (NULL)
, m_miso (NULL)
, m_cs (NULL)
, m_spi (NULL)
{
Init(clk, mosi, miso, cs, range);
}
/**
* Constructor.
*
* @param moduleNumber The digital module with the sensor attached.
* @param clk The GPIO the clock signal is wired to.
* @param mosi The GPIO the MOSI (Master Out Slave In) signal is wired to.
* @param miso The GPIO the MISO (Master In Slave Out) signal is wired to.
* @param cs The GPIO the CS (Chip Select) signal is wired to.
* @param range The range (+ or -) that the accelerometer will measure.
*/
ADXL345_SPI::ADXL345_SPI(uint8_t moduleNumber, uint32_t clk, uint32_t mosi, uint32_t miso,
uint32_t cs, ADXL345_SPI::DataFormat_Range range)
: m_clk (NULL)
, m_mosi (NULL)
, m_miso (NULL)
, m_cs (NULL)
, m_spi (NULL)
{
m_clk = new DigitalOutput(moduleNumber, clk);
m_mosi = new DigitalOutput(moduleNumber, mosi);
m_miso = new DigitalInput(moduleNumber, miso);
m_cs = new DigitalOutput(moduleNumber, cs);
Init(m_clk, m_mosi, m_miso, m_cs, range);
}
/**
* Internal common init function.
*/
void ADXL345_SPI::Init(DigitalOutput *clk, DigitalOutput *mosi, DigitalInput *miso,
DigitalOutput *cs, DataFormat_Range range)
{
if (clk != NULL && mosi != NULL && miso != NULL && cs != NULL)
{
m_spi = new SPI(clk, mosi, miso);
m_spi->SetMSBFirst();
m_spi->SetSampleDataOnRising();
m_spi->SetSlaveSelect(cs, kChipSelect, false);
m_spi->SetClockActiveLow();
// 8-bit address and 8-bit data
m_spi->SetBitsPerWord(16);
m_spi->ApplyConfig();
m_spi->ClearReceivedData();
// Turn on the measurements
m_spi->Write((kPowerCtlRegister << 8) | kPowerCtl_Measure);
m_spi->Read();
// Specify the data format to read
m_spi->Write((kDataFormatRegister << 8) | kDataFormat_FullRes | (uint8_t)(range & 0x03));
m_spi->Read();
// 8-bit address and 16-bit data
m_spi->SetBitsPerWord(24);
m_spi->ApplyConfig();
HALReport(HALUsageReporting::kResourceType_ADXL345, HALUsageReporting::kADXL345_SPI);
}
}
/**
* Destructor.
*/
ADXL345_SPI::~ADXL345_SPI()
{
delete m_spi;
m_spi = NULL;
delete m_cs;
m_cs = NULL;
delete m_miso;
m_miso = NULL;
delete m_mosi;
m_mosi = NULL;
delete m_clk;
m_clk = NULL;
}
/**
* Get the acceleration of one axis in Gs.
*
* @param axis The axis to read from.
* @return Acceleration of the ADXL345 in Gs.
*/
double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis)
{
int16_t rawAccel = 0;
if(m_spi)
{
m_spi->Write(((kAddress_Read | kAddress_MultiByte | kDataRegister) + (uint8_t)axis) << 16);
rawAccel = (uint16_t)m_spi->Read();
// Sensor is little endian... swap bytes
rawAccel = ((rawAccel >> 8) & 0xFF) | (rawAccel << 8);
}
return rawAccel * kGsPerLSB;
}
/**
* Get the acceleration of all axes in Gs.
*
* @return Acceleration measured on all axes of the ADXL345 in Gs.
*/
ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations()
{
AllAxes data = {0.0};
int16_t rawData[3];
if (m_spi)
{
tFrameMode mode;
bool activeLow;
// Backup original settings.
DigitalOutput *cs = m_spi->GetSlaveSelect(&mode, &activeLow);
uint32_t bitsPerWord = m_spi->GetBitsPerWord();
// Initialize the chip select to inactive.
cs->Set(activeLow);
// Control the chip select manually.
m_spi->SetSlaveSelect(NULL);
// 8-bit address
m_spi->SetBitsPerWord(8);
m_spi->ApplyConfig();
// Assert chip select.
cs->Set(!activeLow);
// Select the data address.
m_spi->Write(kAddress_Read | kAddress_MultiByte | kDataRegister);
m_spi->Read();
// 16-bits for each axis
m_spi->SetBitsPerWord(16);
m_spi->ApplyConfig();
for (int32_t i=0; i<3; i++)
{
// SPI Interface can't read enough data in a single transaction to read all axes at once.
rawData[i] = (uint16_t)m_spi->Read(true);
// Sensor is little endian... swap bytes
rawData[i] = ((rawData[i] >> 8) & 0xFF) | (rawData[i] << 8);
}
// Deassert chip select.
cs->Set(activeLow);
// Restore original settings.
m_spi->SetSlaveSelect(cs, mode, activeLow);
m_spi->SetBitsPerWord(bitsPerWord);
m_spi->ApplyConfig();
data.XAxis = rawData[0] * kGsPerLSB;
data.YAxis = rawData[1] * kGsPerLSB;
data.ZAxis = rawData[2] * kGsPerLSB;
}
return data;
}

156
wpilibc/src/main/native/Accelerometer.cpp
View File

@@ -1,156 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Accelerometer.h"
#include "AnalogModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
/**
* Common function for initializing the accelerometer.
*/
void Accelerometer::InitAccelerometer()
{
m_table = NULL;
m_voltsPerG = 1.0;
m_zeroGVoltage = 2.5;
HALReport(HALUsageReporting::kResourceType_Accelerometer, m_analogChannel->GetChannel(), m_analogChannel->GetModuleNumber() - 1);
LiveWindow::GetInstance()->AddSensor("Accelerometer", m_analogChannel->GetModuleNumber(), m_analogChannel->GetChannel(), this);
}
/**
* Create a new instance of an accelerometer.
*
* The accelerometer is assumed to be in the first analog module in the given analog channel. The
* constructor allocates desired analog channel.
*/
Accelerometer::Accelerometer(uint32_t channel)
{
m_analogChannel = new AnalogChannel(channel);
m_allocatedChannel = true;
InitAccelerometer();
}
/**
* Create new instance of accelerometer.
*
* Make a new instance of the accelerometer given a module and channel. The constructor allocates
* the desired analog channel from the specified module
*
* @param moduleNumber The analog module (1 or 2).
* @param channel The analog channel (1..8)
*/
Accelerometer::Accelerometer(uint8_t moduleNumber, uint32_t channel)
{
m_analogChannel = new AnalogChannel(moduleNumber, channel);
m_allocatedChannel = true;
InitAccelerometer();
}
/**
* Create a new instance of Accelerometer from an existing AnalogChannel.
* Make a new instance of accelerometer given an AnalogChannel. This is particularly
* useful if the port is going to be read as an analog channel as well as through
* the Accelerometer class.
*/
Accelerometer::Accelerometer(AnalogChannel *channel)
{
if (channel == NULL)
{
wpi_setWPIError(NullParameter);
}
else
{
m_analogChannel = channel;
InitAccelerometer();
}
m_allocatedChannel = false;
}
/**
* Delete the analog components used for the accelerometer.
*/
Accelerometer::~Accelerometer()
{
if (m_allocatedChannel)
{
delete m_analogChannel;
}
}
/**
* Return the acceleration in Gs.
*
* The acceleration is returned units of Gs.
*
* @return The current acceleration of the sensor in Gs.
*/
float Accelerometer::GetAcceleration()
{
return (m_analogChannel->GetAverageVoltage() - m_zeroGVoltage) / m_voltsPerG;
}
/**
* Set the accelerometer sensitivity.
*
* This sets the sensitivity of the accelerometer used for calculating the acceleration.
* The sensitivity varys by accelerometer model. There are constants defined for various models.
*
* @param sensitivity The sensitivity of accelerometer in Volts per G.
*/
void Accelerometer::SetSensitivity(float sensitivity)
{
m_voltsPerG = sensitivity;
}
/**
* Set the voltage that corresponds to 0 G.
*
* The zero G voltage varys by accelerometer model. There are constants defined for various models.
*
* @param zero The zero G voltage.
*/
void Accelerometer::SetZero(float zero)
{
m_zeroGVoltage = zero;
}
/**
* Get the Acceleration for the PID Source parent.
*
* @return The current acceleration in Gs.
*/
double Accelerometer::PIDGet()
{
return GetAcceleration();
}
void Accelerometer::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetAcceleration());
}
}
void Accelerometer::StartLiveWindowMode() {
}
void Accelerometer::StopLiveWindowMode() {
}
std::string Accelerometer::GetSmartDashboardType() {
return "Accelerometer";
}
void Accelerometer::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Accelerometer::GetTable() {
return m_table;
}

449
wpilibc/src/main/native/AnalogChannel.cpp
View File

@@ -1,449 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "AnalogChannel.h"
#include "AnalogModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
static Resource *channels = NULL;
const uint8_t AnalogChannel::kAccumulatorModuleNumber;
const uint32_t AnalogChannel::kAccumulatorNumChannels;
const uint32_t AnalogChannel::kAccumulatorChannels[] = {1, 2};
/**
* Common initialization.
*/
void AnalogChannel::InitChannel(uint8_t moduleNumber, uint32_t channel)
{
m_table = NULL;
char buf[64];
Resource::CreateResourceObject(&channels, kAnalogModules * kAnalogChannels);
if (!checkAnalogModule(moduleNumber))
{
snprintf(buf, 64, "Analog Module %d", moduleNumber);
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return;
}
if (!checkAnalogChannel(channel))
{
snprintf(buf, 64, "Analog Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
snprintf(buf, 64, "Analog Input %d (Module: %d)", channel, moduleNumber);
if (channels->Allocate((moduleNumber - 1) * kAnalogChannels + channel - 1, buf) == ~0ul)
{
CloneError(channels);
return;
}
m_channel = channel;
m_module = moduleNumber;
void* port = getPortWithModule(moduleNumber, channel);
int32_t status = 0;
m_port = initializeAnalogPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance()->AddSensor("AnalogChannel",channel, GetModuleNumber(), this);
HALReport(HALUsageReporting::kResourceType_AnalogChannel, channel, GetModuleNumber() - 1);
}
/**
* Construct an analog channel on a specified module.
*
* @param moduleNumber The analog module (1 or 2).
* @param channel The channel number to represent.
*/
AnalogChannel::AnalogChannel(uint8_t moduleNumber, uint32_t channel)
{
InitChannel(moduleNumber, channel);
}
/**
* Construct an analog channel on the default module.
*
* @param channel The channel number to represent.
*/
AnalogChannel::AnalogChannel(uint32_t channel)
{
InitChannel(GetDefaultAnalogModule(), channel);
}
/**
* Channel destructor.
*/
AnalogChannel::~AnalogChannel()
{
channels->Free((m_module - 1) * kAnalogChannels + m_channel - 1);
}
/**
* Get the analog module that this channel is on.
* @return A pointer to the AnalogModule that this channel is on.
*/
AnalogModule *AnalogChannel::GetModule()
{
if (StatusIsFatal()) return NULL;
return AnalogModule::GetInstance(m_module);
}
/**
* Get a sample straight from this channel on the module.
* The sample is a 12-bit value representing the -10V to 10V range of the A/D converter in the module.
* The units are in A/D converter codes. Use GetVoltage() to get the analog value in calibrated units.
* @return A sample straight from this channel on the module.
*/
int16_t AnalogChannel::GetValue()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int16_t value = getAnalogValue(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Get a sample from the output of the oversample and average engine for this channel.
* The sample is 12-bit + the value configured in SetOversampleBits().
* The value configured in SetAverageBits() will cause this value to be averaged 2**bits number of samples.
* This is not a sliding window. The sample will not change until 2**(OversamplBits + AverageBits) samples
* have been acquired from the module on this channel.
* Use GetAverageVoltage() to get the analog value in calibrated units.
* @return A sample from the oversample and average engine for this channel.
*/
int32_t AnalogChannel::GetAverageValue()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int32_t value = getAnalogAverageValue(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Get a scaled sample straight from this channel on the module.
* The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
* @return A scaled sample straight from this channel on the module.
*/
float AnalogChannel::GetVoltage()
{
if (StatusIsFatal()) return 0.0f;
int32_t status = 0;
float voltage = getAnalogVoltage(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return voltage;
}
/**
* Get a scaled sample from the output of the oversample and average engine for this channel.
* The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
* Using oversampling will cause this value to be higher resolution, but it will update more slowly.
* Using averaging will cause this value to be more stable, but it will update more slowly.
* @return A scaled sample from the output of the oversample and average engine for this channel.
*/
float AnalogChannel::GetAverageVoltage()
{
if (StatusIsFatal()) return 0.0f;
int32_t status = 0;
float voltage = getAnalogAverageVoltage(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return voltage;
}
/**
* Get the factory scaling least significant bit weight constant.
* The least significant bit weight constant for the channel that was calibrated in
* manufacturing and stored in an eeprom in the module.
*
* Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
*
* @return Least significant bit weight.
*/
uint32_t AnalogChannel::GetLSBWeight()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int32_t lsbWeight = getAnalogLSBWeight(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return lsbWeight;
}
/**
* Get the factory scaling offset constant.
* The offset constant for the channel that was calibrated in manufacturing and stored
* in an eeprom in the module.
*
* Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
*
* @return Offset constant.
*/
int32_t AnalogChannel::GetOffset()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int32_t offset = getAnalogOffset(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return offset;
}
/**
* Get the channel number.
* @return The channel number.
*/
uint32_t AnalogChannel::GetChannel()
{
if (StatusIsFatal()) return 0;
return m_channel;
}
/**
* Get the module number.
* @return The module number.
*/
uint8_t AnalogChannel::GetModuleNumber()
{
if (StatusIsFatal()) return 0;
return m_module;
}
/**
* Set the number of averaging bits.
* This sets the number of averaging bits. The actual number of averaged samples is 2**bits.
* Use averaging to improve the stability of your measurement at the expense of sampling rate.
* The averaging is done automatically in the FPGA.
*
* @param bits Number of bits of averaging.
*/
void AnalogChannel::SetAverageBits(uint32_t bits)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAnalogAverageBits(m_port, bits, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the number of averaging bits previously configured.
* This gets the number of averaging bits from the FPGA. The actual number of averaged samples is 2**bits.
* The averaging is done automatically in the FPGA.
*
* @return Number of bits of averaging previously configured.
*/
uint32_t AnalogChannel::GetAverageBits()
{
int32_t status = 0;
int32_t averageBits = getAnalogAverageBits(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return averageBits;
}
/**
* Set the number of oversample bits.
* This sets the number of oversample bits. The actual number of oversampled values is 2**bits.
* Use oversampling to improve the resolution of your measurements at the expense of sampling rate.
* The oversampling is done automatically in the FPGA.
*
* @param bits Number of bits of oversampling.
*/
void AnalogChannel::SetOversampleBits(uint32_t bits)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAnalogOversampleBits(m_port, bits, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the number of oversample bits previously configured.
* This gets the number of oversample bits from the FPGA. The actual number of oversampled values is
* 2**bits. The oversampling is done automatically in the FPGA.
*
* @return Number of bits of oversampling previously configured.
*/
uint32_t AnalogChannel::GetOversampleBits()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int32_t oversampleBits = getAnalogOversampleBits(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return oversampleBits;
}
/**
* Is the channel attached to an accumulator.
*
* @return The analog channel is attached to an accumulator.
*/
bool AnalogChannel::IsAccumulatorChannel()
{
if (StatusIsFatal()) return false;
int32_t status = 0;
bool isAccum = isAccumulatorChannel(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return isAccum;
}
/**
* Initialize the accumulator.
*/
void AnalogChannel::InitAccumulator()
{
if (StatusIsFatal()) return;
m_accumulatorOffset = 0;
int32_t status = 0;
initAccumulator(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set an inital value for the accumulator.
*
* This will be added to all values returned to the user.
* @param initialValue The value that the accumulator should start from when reset.
*/
void AnalogChannel::SetAccumulatorInitialValue(int64_t initialValue)
{
if (StatusIsFatal()) return;
m_accumulatorOffset = initialValue;
}
/**
* Resets the accumulator to the initial value.
*/
void AnalogChannel::ResetAccumulator()
{
if (StatusIsFatal()) return;
int32_t status = 0;
resetAccumulator(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the center value of the accumulator.
*
* The center value is subtracted from each A/D value before it is added to the accumulator. This
* is used for the center value of devices like gyros and accelerometers to make integration work
* and to take the device offset into account when integrating.
*
* This center value is based on the output of the oversampled and averaged source from channel 1.
* Because of this, any non-zero oversample bits will affect the size of the value for this field.
*/
void AnalogChannel::SetAccumulatorCenter(int32_t center)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAccumulatorCenter(m_port, center, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the accumulator's deadband.
*/
void AnalogChannel::SetAccumulatorDeadband(int32_t deadband)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAccumulatorDeadband(m_port, deadband, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Read the accumulated value.
*
* Read the value that has been accumulating on channel 1.
* The accumulator is attached after the oversample and average engine.
*
* @return The 64-bit value accumulated since the last Reset().
*/
int64_t AnalogChannel::GetAccumulatorValue()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int64_t value = getAccumulatorValue(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value + m_accumulatorOffset;
}
/**
* Read the number of accumulated values.
*
* Read the count of the accumulated values since the accumulator was last Reset().
*
* @return The number of times samples from the channel were accumulated.
*/
uint32_t AnalogChannel::GetAccumulatorCount()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
uint32_t count = getAccumulatorCount(m_port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return count;
}
/**
* Read the accumulated value and the number of accumulated values atomically.
*
* This function reads the value and count from the FPGA atomically.
* This can be used for averaging.
*
* @param value Pointer to the 64-bit accumulated output.
* @param count Pointer to the number of accumulation cycles.
*/
void AnalogChannel::GetAccumulatorOutput(int64_t *value, uint32_t *count)
{
if (StatusIsFatal()) return;
int32_t status = 0;
getAccumulatorOutput(m_port, value, count, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
*value += m_accumulatorOffset;
}
/**
* Get the Average value for the PID Source base object.
*
* @return The average voltage.
*/
double AnalogChannel::PIDGet()
{
if (StatusIsFatal()) return 0.0;
return GetAverageValue();
}
void AnalogChannel::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetAverageVoltage());
}
}
void AnalogChannel::StartLiveWindowMode() {
}
void AnalogChannel::StopLiveWindowMode() {
}
std::string AnalogChannel::GetSmartDashboardType() {
return "Analog Input";
}
void AnalogChannel::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * AnalogChannel::GetTable() {
return m_table;
}

299
wpilibc/src/main/native/AnalogModule.cpp
View File

@@ -1,299 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "AnalogModule.h"
#include "Timer.h"
#include "WPIErrors.h"
const long AnalogModule::kTimebase; ///< 40 MHz clock
const long AnalogModule::kDefaultOversampleBits;
const long AnalogModule::kDefaultAverageBits;
constexpr float AnalogModule::kDefaultSampleRate;
/**
* Get an instance of an Analog Module.
*
* Singleton analog module creation where a module is allocated on the first use
* and the same module is returned on subsequent uses.
*
* @param moduleNumber The analog module to get (1 or 2).
* @return A pointer to the AnalogModule.
*/
AnalogModule* AnalogModule::GetInstance(uint8_t moduleNumber)
{
if (checkAnalogModule(moduleNumber))
{
return (AnalogModule*)GetModule(nLoadOut::kModuleType_Analog, moduleNumber);
}
// If this wasn't caught before now, make sure we say what's wrong before we crash
char buf[64];
snprintf(buf, 64, "Analog Module %d", moduleNumber);
wpi_setGlobalWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return NULL;
}
/**
* Create a new instance of an analog module.
*
* Create an instance of the analog module object. Initialize all the parameters
* to reasonable values on start.
* Setting a global value on an analog module can be done only once unless subsequent
* values are set the previously set value.
* Analog modules are a singleton, so the constructor is never called outside of this class.
*
* @param moduleNumber The analog module to create (1 or 2).
*/
AnalogModule::AnalogModule(uint8_t moduleNumber)
: Module(nLoadOut::kModuleType_Analog, moduleNumber)
, m_moduleNumber(moduleNumber)
, m_ports()
{
for (uint32_t i = 0; i < kAnalogChannels; i++)
{
void* port = getPortWithModule(moduleNumber, i+1);
int32_t status = 0;
m_ports[i] = initializeAnalogPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
AddToSingletonList();
}
/**
* Destructor for AnalogModule.
*/
AnalogModule::~AnalogModule()
{
}
/**
* 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)
{
int32_t status = 0;
setAnalogSampleRateWithModule(m_moduleNumber, samplesPerSecond, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the current sample rate on the module.
*
* This assumes one entry in the scan list.
* This is a global setting for the module and effects all channels.
*
* @return Sample rate.
*/
float AnalogModule::GetSampleRate()
{
int32_t status = 0;
float sampleRate = getAnalogSampleRateWithModule(m_moduleNumber, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return sampleRate;
}
/**
* Set the number of averaging bits.
*
* This sets the number of averaging bits. The actual number of averaged samples is 2**bits.
* Use averaging to improve the stability of your measurement at the expense of sampling rate.
* The averaging is done automatically in the FPGA.
*
* @param channel Analog channel to configure.
* @param bits Number of bits to average.
*/
void AnalogModule::SetAverageBits(uint32_t channel, uint32_t bits)
{
int32_t status = 0;
setAnalogAverageBits(m_ports[channel-1], bits, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the number of averaging bits.
*
* This gets the number of averaging bits from the FPGA. The actual number of averaged samples is 2**bits.
* The averaging is done automatically in the FPGA.
*
* @param channel Channel to address.
* @return Bits to average.
*/
uint32_t AnalogModule::GetAverageBits(uint32_t channel)
{
int32_t status = 0;
int32_t averageBits = getAnalogAverageBits(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return averageBits;
}
/**
* Set the number of oversample bits.
*
* This sets the number of oversample bits. The actual number of oversampled values is 2**bits.
* Use oversampling to improve the resolution of your measurements at the expense of sampling rate.
* The oversampling is done automatically in the FPGA.
*
* @param channel Analog channel to configure.
* @param bits Number of bits to oversample.
*/
void AnalogModule::SetOversampleBits(uint32_t channel, uint32_t bits)
{
int32_t status = 0;
setAnalogOversampleBits(m_ports[channel-1], bits, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the number of oversample bits.
*
* This gets the number of oversample bits from the FPGA. The actual number of oversampled values is
* 2**bits. The oversampling is done automatically in the FPGA.
*
* @param channel Channel to address.
* @return Bits to oversample.
*/
uint32_t AnalogModule::GetOversampleBits(uint32_t channel)
{
int32_t status = 0;
int32_t oversampleBits = getAnalogOversampleBits(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return oversampleBits;
}
/**
* Get a sample straight from the channel on this module.
*
* The sample is a 12-bit value representing the -10V to 10V range of the A/D converter in the module.
* The units are in A/D converter codes. Use GetVoltage() to get the analog value in calibrated units.
*
* @return A sample straight from the channel on this module.
*/
int16_t AnalogModule::GetValue(uint32_t channel)
{
int32_t status = 0;
int16_t value = getAnalogValue(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Get a sample from the output of the oversample and average engine for the channel.
*
* The sample is 12-bit + the value configured in SetOversampleBits().
* The value configured in SetAverageBits() will cause this value to be averaged 2**bits number of samples.
* This is not a sliding window. The sample will not change until 2**(OversamplBits + AverageBits) samples
* have been acquired from the module on this channel.
* Use GetAverageVoltage() to get the analog value in calibrated units.
*
* @param channel Channel number to read.
* @return A sample from the oversample and average engine for the channel.
*/
int32_t AnalogModule::GetAverageValue(uint32_t channel)
{
int32_t status = 0;
int32_t value = getAnalogAverageValue(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* 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)
{
int32_t status = 0;
int32_t value = getAnalogVoltsToValue(m_ports[channel-1], voltage, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Get a scaled sample straight from the channel on this module.
*
* The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
*
* @param channel The channel to read.
* @return A scaled sample straight from the channel on this module.
*/
float AnalogModule::GetVoltage(uint32_t channel)
{
int32_t status = 0;
float voltage = getAnalogVoltage(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return voltage;
}
/**
* Get a scaled sample from the output of the oversample and average engine for the channel.
*
* The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
* Using oversampling will cause this value to be higher resolution, but it will update more slowly.
* Using averaging will cause this value to be more stable, but it will update more slowly.
*
* @param channel The channel to read.
* @return A scaled sample from the output of the oversample and average engine for the channel.
*/
float AnalogModule::GetAverageVoltage(uint32_t channel)
{
int32_t status = 0;
float voltage = getAnalogAverageVoltage(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return voltage;
}
/**
* Get the factory scaling least significant bit weight constant.
* The least significant bit weight constant for the channel that was calibrated in
* manufacturing and stored in an eeprom in the module.
*
* Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
*
* @param channel The channel to get calibration data for.
* @return Least significant bit weight.
*/
uint32_t AnalogModule::GetLSBWeight(uint32_t channel)
{
int32_t status = 0;
int32_t lsbWeight = getAnalogLSBWeight(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return lsbWeight;
}
/**
* Get the factory scaling offset constant.
* The offset constant for the channel that was calibrated in manufacturing and stored
* in an eeprom in the module.
*
* Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
*
* @param channel The channel to get calibration data for.
* @return Offset constant.
*/
int32_t AnalogModule::GetOffset(uint32_t channel)
{
int32_t status = 0;
int32_t offset = getAnalogOffset(m_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return offset;
}

174
wpilibc/src/main/native/AnalogTrigger.cpp
View File

@@ -1,174 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "AnalogTrigger.h"
#include "AnalogChannel.h"
#include "AnalogModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
/**
* Initialize an analog trigger from a slot and channel.
* This is the common code for the two constructors that use a slot and channel.
*/
void AnalogTrigger::InitTrigger(uint8_t moduleNumber, uint32_t channel)
{
void* port = getPortWithModule(moduleNumber, channel);
int32_t status = 0;
uint32_t index = 0;
m_trigger = initializeAnalogTrigger(port, &index, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_index = index;
HALReport(HALUsageReporting::kResourceType_AnalogTrigger, channel, moduleNumber - 1);
}
/**
* Constructor for an analog trigger given a channel number.
* The default module is used in this case.
*
* @param channel The analog channel (1..8).
*/
AnalogTrigger::AnalogTrigger(uint32_t channel)
{
InitTrigger(GetDefaultAnalogModule(), channel);
}
/**
* Constructor for an analog trigger given both the slot and channel.
*
* @param moduleNumber The analog module (1 or 2).
* @param channel The analog channel (1..8).
*/
AnalogTrigger::AnalogTrigger(uint8_t moduleNumber, uint32_t channel)
{
InitTrigger(moduleNumber, channel);
}
/**
* Construct an analog trigger given an analog channel.
* This should be used in the case of sharing an analog channel between the trigger
* and an analog input object.
*/
AnalogTrigger::AnalogTrigger(AnalogChannel *channel)
{
InitTrigger(channel->GetModuleNumber(), channel->GetChannel());
}
AnalogTrigger::~AnalogTrigger()
{
int32_t status = 0;
cleanAnalogTrigger(m_trigger, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the upper and lower limits of the analog trigger.
* The limits are given in ADC codes. If oversampling is used, the units must be scaled
* appropriately.
*/
void AnalogTrigger::SetLimitsRaw(int32_t lower, int32_t upper)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the upper and lower limits of the analog trigger.
* The limits are given as floating point voltage values.
*/
void AnalogTrigger::SetLimitsVoltage(float lower, float upper)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the analog trigger to use the averaged vs. raw values.
* If the value is true, then the averaged value is selected for the analog trigger, otherwise
* the immediate value is used.
*/
void AnalogTrigger::SetAveraged(bool useAveragedValue)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* 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;
int32_t status = 0;
setAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Return the index of the analog trigger.
* This is the FPGA index of this analog trigger instance.
* @return The index of the analog trigger.
*/
uint32_t AnalogTrigger::GetIndex()
{
if (StatusIsFatal()) return ~0ul;
return m_index;
}
/**
* Return the InWindow output of the analog trigger.
* True if the analog input is between the upper and lower limits.
* @return The InWindow output of the analog trigger.
*/
bool AnalogTrigger::GetInWindow()
{
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = getAnalogTriggerInWindow(m_trigger, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* Return the TriggerState output of the analog trigger.
* True if above upper limit.
* False if below lower limit.
* If in Hysteresis, maintain previous state.
* @return The TriggerState output of the analog trigger.
*/
bool AnalogTrigger::GetTriggerState()
{
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = getAnalogTriggerTriggerState(m_trigger, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* Creates an AnalogTriggerOutput object.
* Gets an output object that can be used for routing.
* Caller is responsible for deleting the AnalogTriggerOutput object.
* @param type An enum of the type of output object to create.
* @return A pointer to a new AnalogTriggerOutput object.
*/
AnalogTriggerOutput *AnalogTrigger::CreateOutput(AnalogTriggerType type)
{
if (StatusIsFatal()) return NULL;
return new AnalogTriggerOutput(this, type);
}

83
wpilibc/src/main/native/AnalogTriggerOutput.cpp
View File

@@ -1,83 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "AnalogTriggerOutput.h"
#include "AnalogTrigger.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
/**
* Create an object that represents one of the four outputs from an analog trigger.
*
* Because this class derives from DigitalSource, it can be passed into routing functions
* for Counter, Encoder, etc.
*
* @param trigger A pointer to the trigger for which this is an output.
* @param outputType An enum that specifies the output on the trigger to represent.
*/
AnalogTriggerOutput::AnalogTriggerOutput(AnalogTrigger *trigger, AnalogTriggerType outputType)
: m_trigger (trigger)
, m_outputType (outputType)
{
HALReport(HALUsageReporting::kResourceType_AnalogTriggerOutput, trigger->GetIndex(), outputType);
}
AnalogTriggerOutput::~AnalogTriggerOutput()
{
}
/**
* Get the state of the analog trigger output.
* @return The state of the analog trigger output.
*/
bool AnalogTriggerOutput::Get()
{
int32_t status = 0;
bool result = getAnalogTriggerOutput(m_trigger->m_trigger, m_outputType, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* @return The value to be written to the channel field of a routing mux.
*/
uint32_t AnalogTriggerOutput::GetChannelForRouting()
{
return (m_trigger->m_index << 2) + m_outputType;
}
/**
* @return The value to be written to the module field of a routing mux.
*/
uint32_t AnalogTriggerOutput::GetModuleForRouting()
{
return m_trigger->m_index >> 2;
}
/**
* @return The value to be written to the module field of a routing mux.
*/
bool AnalogTriggerOutput::GetAnalogTriggerForRouting()
{
return true;
}
/**
* Request interrupts asynchronously on this analog trigger output.
* TODO: Hardware supports interrupts on Analog Trigger outputs... WPILib should too
*/
void AnalogTriggerOutput::RequestInterrupts(InterruptHandlerFunction handler, void *param)
{
}
/**
* Request interrupts synchronously on this analog trigger output.
* TODO: Hardware supports interrupts on Analog Trigger outputs... WPILib should too
*/
void AnalogTriggerOutput::RequestInterrupts()
{
}

20
wpilibc/src/main/native/Buttons/AnalogIOButton.cpp
View File

@@ -1,20 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/AnalogIOButton.h"
#include "DriverStation.h"
const double AnalogIOButton::kThreshold = 0.5;
AnalogIOButton::AnalogIOButton(int port) :
m_port(port)
{
}
bool AnalogIOButton::Get()
{
return DriverStation::GetInstance()->GetEnhancedIO().GetAnalogIn(m_port) < kThreshold;
}

50
wpilibc/src/main/native/Buttons/Button.cpp
View File

@@ -1,50 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/Button.h"
/**
* Specifies the command to run when a button is first pressed
* @param command The pointer to the command to run
*/
void Button::WhenPressed(Command *command) {
WhenActive(command);
}
/**
* Specifies the command to be scheduled while the button is pressed
* The command will be scheduled repeatedly while the button is pressed and will
* be canceled when the button is released.
* @param command The pointer to the command to run
*/
void Button::WhileHeld(Command *command) {
WhileActive(command);
}
/**
* Specifies the command to run when the button is released
* The command will be scheduled a single time.
* @param The pointer to the command to run
*/
void Button::WhenReleased(Command *command) {
WhenInactive(command);
}
/**
* Cancels the specificed command when the button is pressed
* @param The command to be canceled
*/
void Button::CancelWhenPressed(Command *command) {
CancelWhenActive(command);
}
/**
* Toggle the specified command when the button is pressed
* @param The command to be toggled
*/
void Button::ToggleWhenPressed(Command *command) {
ToggleWhenActive(command);
}

21
wpilibc/src/main/native/Buttons/ButtonScheduler.cpp
View File

@@ -1,21 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/ButtonScheduler.h"
#include "Commands/Scheduler.h"
ButtonScheduler::ButtonScheduler(bool last, Trigger *button, Command *orders) :
m_pressedLast(last),
m_button(button),
m_command(orders)
{
}
void ButtonScheduler::Start()
{
Scheduler::GetInstance()->AddButton(this);
}

29
wpilibc/src/main/native/Buttons/CancelButtonScheduler.cpp
View File

@@ -1,29 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/CancelButtonScheduler.h"
#include "Buttons/Button.h"
#include "Commands/Command.h"
CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger *button, Command *orders) :
ButtonScheduler(last, button, orders)
{
pressedLast = m_button->Grab();
}
void CancelButtonScheduler::Execute()
{
if (m_button->Grab()) {
if (!pressedLast) {
pressedLast = true;
m_command->Cancel();
}
} else {
pressedLast = false;
}
}

20
wpilibc/src/main/native/Buttons/DigitalIOButton.cpp
View File

@@ -1,20 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/DigitalIOButton.h"
#include "DriverStation.h"
const bool DigitalIOButton::kActiveState = false;
DigitalIOButton::DigitalIOButton(int port) :
m_port(port)
{
}
bool DigitalIOButton::Get()
{
return DriverStation::GetInstance()->GetEnhancedIO().GetDigital(m_port) == kActiveState;
}

32
wpilibc/src/main/native/Buttons/HeldButtonScheduler.cpp
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/HeldButtonScheduler.h"
#include "Buttons/Button.h"
#include "Commands/Command.h"
HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger *button, Command *orders) :
ButtonScheduler(last, button, orders)
{
}
void HeldButtonScheduler::Execute()
{
if (m_button->Grab())
{
m_pressedLast = true;
m_command->Start();
}
else
{
if (m_pressedLast)
{
m_pressedLast = false;
m_command->Cancel();
}
}
}

34
wpilibc/src/main/native/Buttons/InternalButton.cpp
View File

@@ -1,34 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/InternalButton.h"
InternalButton::InternalButton() :
m_pressed(false),
m_inverted(false)
{
}
InternalButton::InternalButton(bool inverted) :
m_pressed(inverted),
m_inverted(inverted)
{
}
void InternalButton::SetInverted(bool inverted)
{
m_inverted = inverted;
}
void InternalButton::SetPressed(bool pressed)
{
m_pressed = pressed;
}
bool InternalButton::Get()
{
return m_pressed ^ m_inverted;
}

18
wpilibc/src/main/native/Buttons/JoystickButton.cpp
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/JoystickButton.h"
JoystickButton::JoystickButton(GenericHID *joystick, int buttonNumber) :
m_joystick(joystick),
m_buttonNumber(buttonNumber)
{
}
bool JoystickButton::Get()
{
return m_joystick->GetRawButton(m_buttonNumber);
}

29
wpilibc/src/main/native/Buttons/NetworkButton.cpp
View File

@@ -1,29 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/NetworkButton.h"
#include "networktables/NetworkTable.h"
NetworkButton::NetworkButton(const char *tableName, const char *field) ://TODO how is this supposed to work???
m_netTable(NetworkTable::GetTable(tableName)),
m_field(field)
{
}
NetworkButton::NetworkButton(ITable *table, const char *field) :
m_netTable(table),
m_field(field)
{
}
bool NetworkButton::Get()
{
/*if (m_netTable->isConnected())
return m_netTable->GetBoolean(m_field.c_str());
else
return false;*/
return m_netTable->GetBoolean(m_field);
}

31
wpilibc/src/main/native/Buttons/PressedButtonScheduler.cpp
View File

@@ -1,31 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/PressedButtonScheduler.h"
#include "Buttons/Button.h"
#include "Commands/Command.h"
PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger *button, Command *orders) :
ButtonScheduler(last, button, orders)
{
}
void PressedButtonScheduler::Execute()
{
if (m_button->Grab())
{
if (!m_pressedLast)
{
m_pressedLast = true;
m_command->Start();
}
}
else
{
m_pressedLast = false;
}
}

31
wpilibc/src/main/native/Buttons/ReleasedButtonScheduler.cpp
View File

@@ -1,31 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/ReleasedButtonScheduler.h"
#include "Buttons/Button.h"
#include "Commands/Command.h"
ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger *button, Command *orders) :
ButtonScheduler(last, button, orders)
{
}
void ReleasedButtonScheduler::Execute()
{
if (m_button->Grab())
{
m_pressedLast = true;
}
else
{
if (m_pressedLast)
{
m_pressedLast = false;
m_command->Start();
}
}
}

32
wpilibc/src/main/native/Buttons/ToggleButtonScheduler.cpp
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/ToggleButtonScheduler.h"
#include "Buttons/Button.h"
#include "Commands/Command.h"
ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger *button,
Command *orders) :
ButtonScheduler(last, button, orders) {
pressedLast = m_button->Grab();
}
void ToggleButtonScheduler::Execute() {
if (m_button->Grab()) {
if (!pressedLast) {
pressedLast = true;
if (m_command->IsRunning()) {
m_command->Cancel();
} else {
m_command->Start();
}
}
} else {
pressedLast = false;
}
}

75
wpilibc/src/main/native/Buttons/Trigger.cpp
View File

@@ -1,75 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Buttons/Button.h"
#include "Buttons/HeldButtonScheduler.h"
#include "Buttons/PressedButtonScheduler.h"
#include "Buttons/ReleasedButtonScheduler.h"
#include "Buttons/ToggleButtonScheduler.h"
#include "Buttons/CancelButtonScheduler.h"
Trigger::Trigger() {
m_table = NULL;
}
bool Trigger::Grab()
{
if (Get())
return true;
else if (m_table != NULL)
{
//if (m_table->isConnected())//TODO is connected on button?
return m_table->GetBoolean("pressed");
/*else
return false;*/
}
else
return false;
}
void Trigger::WhenActive(Command *command)
{
PressedButtonScheduler *pbs = new PressedButtonScheduler(Grab(), this, command);
pbs->Start();
}
void Trigger::WhileActive(Command *command)
{
HeldButtonScheduler *hbs = new HeldButtonScheduler(Grab(), this, command);
hbs->Start();
}
void Trigger::WhenInactive(Command *command)
{
ReleasedButtonScheduler *rbs = new ReleasedButtonScheduler(Grab(), this, command);
rbs->Start();
}
void Trigger::CancelWhenActive(Command *command) {
CancelButtonScheduler *cbs = new CancelButtonScheduler(Grab(), this, command);
cbs->Start();
}
void Trigger::ToggleWhenActive(Command *command) {
ToggleButtonScheduler *tbs = new ToggleButtonScheduler(Grab(), this, command);
tbs->Start();
}
std::string Trigger::GetSmartDashboardType(){
return "Button";
}
void Trigger::InitTable(ITable* table){
m_table = table;
if(m_table!=NULL){
m_table->PutBoolean("pressed", Get());
}
}
ITable* Trigger::GetTable(){
return m_table;
}

1278
wpilibc/src/main/native/CANJaguar.cpp
View File

File diff suppressed because it is too large Load Diff

475
wpilibc/src/main/native/Commands/Command.cpp
View File

@@ -1,475 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/Command.h"
#include "Commands/CommandGroup.h"
#include "Commands/Scheduler.h"
#include "DriverStation.h"
#include "Timer.h"
#include "WPIErrors.h"
static const char *kName = "name";
static const char *kRunning = "running";
static const char *kIsParented = "isParented";
int Command::m_commandCounter = 0;
void Command::InitCommand(const char *name, double timeout)
{
m_commandID = m_commandCounter++;
m_timeout = timeout;
m_locked = false;
m_startTime = -1;
m_initialized = false;
m_running = false;
m_interruptible = true;
m_canceled = false;
m_runWhenDisabled = false;
m_parent = NULL;
if (name == NULL)
{
// Don't have a way to find the subclass name like java, so use the address
char buf[32];
snprintf(buf, 32, "Command_%p", this);
m_name = buf;
}
else
{
m_name = name;
}
m_table = NULL;
}
/**
* Creates a new command.
* The name of this command will be default.
*/
Command::Command()
{
InitCommand(NULL, -1.0);
}
/**
* Creates a new command with the given name and no timeout.
* @param name the name for this command
*/
Command::Command(const char *name)
{
if (name == NULL)
wpi_setWPIErrorWithContext(NullParameter, "name");
InitCommand(name, -1.0);
}
/**
* Creates a new command with the given timeout and a default name.
* @param timeout the time (in seconds) before this command "times out"
* @see Command#isTimedOut() isTimedOut()
*/
Command::Command(double timeout)
{
if (timeout < 0.0)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
InitCommand(NULL, timeout);
}
/**
* 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);
}
Command::~Command()
{//TODO deal with cleaning up all listeners
/*if (m_table != NULL){
m_table->RemoveChangeListener(kRunning, this);
}*/
}
/**
* Get the ID (sequence number) for this command
* The ID is a unique sequence number that is incremented for each command.
* @return the ID of this command
*/
int Command::GetID() {
return m_commandID;
}
/**
* 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;
}
/**
* Returns the time since this command was initialized (in seconds).
* This function will work even if there is no specified timeout.
* @return the time since this command was initialized (in seconds).
*/
double Command::TimeSinceInitialized()
{
if (m_startTime < 0.0)
return 0.0;
else
return Timer::GetFPGATimestamp() - m_startTime;
}
/**
* This method specifies that the given {@link Subsystem} is used by this command.
* This method is crucial to the functioning of the Command System in general.
*
* <p>Note that the recommended way to call this method is in the constructor.</p>
*
* @param subsystem the {@link Subsystem} required
* @see Subsystem
*/
void Command::Requires(Subsystem *subsystem)
{
if (!AssertUnlocked("Can not add new requirement to command"))
return;
if (subsystem != NULL)
m_requirements.insert(subsystem);
else
wpi_setWPIErrorWithContext(NullParameter, "subsystem");
}
/**
* Called when the command has been removed.
* This will call {@link Command#interrupted() interrupted()} or {@link Command#end() end()}.
*/
void Command::Removed()
{
if (m_initialized)
{
if (IsCanceled())
{
Interrupted();
_Interrupted();
}
else
{
End();
_End();
}
}
m_initialized = false;
m_canceled = false;
m_running = false;
if (m_table != NULL)
m_table->PutBoolean(kRunning, false);
}
/**
* 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);
}
/**
* The run method is used internally to actually run the commands.
* @return whether or not the command should stay within the {@link Scheduler}.
*/
bool Command::Run()
{
if (!m_runWhenDisabled && m_parent == NULL && DriverStation::GetInstance()->IsDisabled())
Cancel();
if (IsCanceled())
return false;
if (!m_initialized)
{
m_initialized = true;
StartTiming();
_Initialize();
Initialize();
}
_Execute();
Execute();
return !IsFinished();
}
void Command::_Initialize()
{
}
void Command::_Interrupted()
{
}
void Command::_Execute()
{
}
void Command::_End()
{
}
/**
* Called to indicate that the timer should start.
* This is called right before {@link Command#initialize() initialize()} is, inside the
* {@link Command#run() run()} method.
*/
void Command::StartTiming()
{
m_startTime = Timer::GetFPGATimestamp();
}
/**
* Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()}
* method returns a number which is greater than or equal to the timeout for the command.
* If there is no timeout, this will always return false.
* @return whether the time has expired
*/
bool Command::IsTimedOut()
{
return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
}
/**
* Returns the requirements (as an std::set of {@link Subsystem Subsystems} pointers) of this command
* @return the requirements (as an std::set of {@link Subsystem Subsystems} pointers) of this command
*/
Command::SubsystemSet Command::GetRequirements()
{
return m_requirements;
}
/**
* Prevents further changes from being made
*/
void Command::LockChanges()
{
m_locked = true;
}
/**
* If changes are locked, then this will generate a CommandIllegalUse error.
* @param message the message to report on error (it is appended by a default message)
* @return true if assert passed, false if assert failed
*/
bool Command::AssertUnlocked(const char *message)
{
if (m_locked)
{
char buf[128];
snprintf(buf, 128, "%s after being started or being added to a command group", message);
wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
return false;
}
return true;
}
/**
* Sets the parent of this command. No actual change is made to the group.
* @param parent the parent
*/
void Command::SetParent(CommandGroup *parent)
{
if (parent == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "parent");
}
else if (m_parent != NULL)
{
wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not give command to a command group after already being put in a command group");
}
else
{
LockChanges();
m_parent = parent;
if (m_table != NULL)
{
m_table->PutBoolean(kIsParented, true);
}
}
}
/**
* This is used internally to mark that the command has been started.
* The lifecycle of a command is:
*
* startRunning() is called.
* run() is called (multiple times potentially)
* removed() is called
*
* It is very important that startRunning and removed be called in order or some assumptions
* of the code will be broken.
*/
void Command::StartRunning()
{
m_running = true;
m_startTime = -1;
if (m_table != NULL)
m_table->PutBoolean(kRunning, true);
}
/**
* Returns whether or not the command is running.
* This may return true even if the command has just been canceled, as it may
* not have yet called {@link Command#interrupted()}.
* @return whether or not the command is running
*/
bool Command::IsRunning()
{
return m_running;
}
/**
* 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();
}
/**
* This works like cancel(), except that it doesn't throw an exception if it is a part
* of a command group. Should only be called by the parent command group.
*/
void Command::_Cancel()
{
if (IsRunning())
m_canceled = true;
}
/**
* Returns whether or not this has been canceled.
* @return whether or not this has been canceled
*/
bool Command::IsCanceled()
{
return m_canceled;
}
/**
* Returns whether or not this command can be interrupted.
* @return whether or not this command can be interrupted
*/
bool Command::IsInterruptible()
{
return m_interruptible;
}
/**
* Sets whether or not this command can be interrupted.
* @param interruptible whether or not this command can be interrupted
*/
void Command::SetInterruptible(bool interruptible)
{
m_interruptible = interruptible;
}
/**
* Checks if the command requires the given {@link Subsystem}.
* @param system the system
* @return whether or not the subsystem is required (false if given NULL)
*/
bool Command::DoesRequire(Subsystem *system)
{
return m_requirements.count(system) > 0;
}
/**
* Returns the {@link CommandGroup} that this command is a part of.
* Will return null if this {@link Command} is not in a group.
* @return the {@link CommandGroup} that this command is a part of (or null if not in group)
*/
CommandGroup *Command::GetGroup()
{
return m_parent;
}
/**
* Sets whether or not this {@link Command} should run when the robot is disabled.
*
* <p>By default a command will not run when the robot is disabled, and will in fact be canceled.</p>
* @param run whether or not this command should run when the robot is disabled
*/
void Command::SetRunWhenDisabled(bool run)
{
m_runWhenDisabled = run;
}
/**
* Returns whether or not this {@link Command} will run when the robot is disabled, or if it will cancel itself.
* @return whether or not this {@link Command} will run when the robot is disabled, or if it will cancel itself
*/
bool Command::WillRunWhenDisabled()
{
return m_runWhenDisabled;
}
std::string Command::GetName()
{
return m_name;
}
std::string Command::GetSmartDashboardType()
{
return "Command";
}
void Command::InitTable(ITable* table)
{
if(m_table!=NULL)
m_table->RemoveTableListener(this);
m_table = table;
if(m_table!=NULL){
m_table->PutString(kName, GetName());
m_table->PutBoolean(kRunning, IsRunning());
m_table->PutBoolean(kIsParented, m_parent != NULL);
m_table->AddTableListener(kRunning, this, false);
}
}
ITable* Command::GetTable(){
return m_table;
}
void Command::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew)
{
if (value.b){
if(!IsRunning())
Start();
}
else{
if(IsRunning())
Cancel();
}
}

383
wpilibc/src/main/native/Commands/CommandGroup.cpp
View File

@@ -1,383 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/CommandGroup.h"
#include "WPIErrors.h"
/**
* Creates a new {@link CommandGroup CommandGroup}.
*/
CommandGroup::CommandGroup()
{
m_currentCommandIndex = -1;
}
/**
* Creates a new {@link CommandGroup CommandGroup} with the given name.
* @param name the name for this command group
*/
CommandGroup::CommandGroup(const char *name) :
Command(name)
{
m_currentCommandIndex = -1;
}
CommandGroup::~CommandGroup()
{
}
/**
* Adds a new {@link Command Command} to the group. The {@link Command Command} will be started after
* all the previously added {@link Command Commands}.
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after
* being added to a group.</p>
*
* <p>It is recommended that this method be called in the constructor.</p>
*
* @param command The {@link Command Command} to be added
*/
void CommandGroup::AddSequential(Command *command)
{
if (command == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group"))
return;
command->SetParent(this);
m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence));
// Iterate through command->GetRequirements() and call Requires() on each required subsystem
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++)
Requires(*iter);
}
/**
* Adds a new {@link Command Command} to the group with a given timeout.
* The {@link Command Command} will be started after all the previously added commands.
*
* <p>Once the {@link Command Command} is started, it will be run until it finishes or the time
* expires, whichever is sooner. Note that the given {@link Command Command} will have no
* knowledge that it is on a timer.</p>
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after
* being added to a group.</p>
*
* <p>It is recommended that this method be called in the constructor.</p>
*
* @param command The {@link Command Command} to be added
* @param timeout The timeout (in seconds)
*/
void CommandGroup::AddSequential(Command *command, double timeout)
{
if (command == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group"))
return;
if (timeout < 0.0)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
return;
}
command->SetParent(this);
m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence, timeout));
// Iterate through command->GetRequirements() and call Requires() on each required subsystem
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++)
Requires(*iter);
}
/**
* Adds a new child {@link Command} to the group. The {@link Command} will be started after
* all the previously added {@link Command Commands}.
*
* <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it
* run at the same time as the subsequent {@link Command Commands}. The child will run until either
* it finishes, a new child with conflicting requirements is started, or
* the main sequence runs a {@link Command} with conflicting requirements. In the latter
* two cases, the child will be canceled even if it says it can't be
* interrupted.</p>
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after
* being added to a group.</p>
*
* <p>It is recommended that this method be called in the constructor.</p>
*
* @param command The command to be added
*/
void CommandGroup::AddParallel(Command *command)
{
if (command == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group"))
return;
command->SetParent(this);
m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild));
// Iterate through command->GetRequirements() and call Requires() on each required subsystem
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++)
Requires(*iter);
}
/**
* Adds a new child {@link Command} to the group with the given timeout. The {@link Command} will be started after
* all the previously added {@link Command Commands}.
*
* <p>Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
* or the time expires, whichever is sooner. Note that the given {@link Command Command} will have no
* knowledge that it is on a timer.</p>
*
* <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it
* run at the same time as the subsequent {@link Command Commands}. The child will run until either
* it finishes, the timeout expires, a new child with conflicting requirements is started, or
* the main sequence runs a {@link Command} with conflicting requirements. In the latter
* two cases, the child will be canceled even if it says it can't be
* interrupted.</p>
*
* <p>Note that any requirements the given {@link Command Command} has will be added to the
* group. For this reason, a {@link Command Command's} requirements can not be changed after
* being added to a group.</p>
*
* <p>It is recommended that this method be called in the constructor.</p>
*
* @param command The command to be added
* @param timeout The timeout (in seconds)
*/
void CommandGroup::AddParallel(Command *command, double timeout)
{
if (command == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!AssertUnlocked("Cannot add new command to command group"))
return;
if (timeout < 0.0)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
return;
}
command->SetParent(this);
m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild, timeout));
// Iterate through command->GetRequirements() and call Requires() on each required subsystem
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++)
Requires(*iter);
}
void CommandGroup::_Initialize()
{
m_currentCommandIndex = -1;
}
void CommandGroup::_Execute()
{
CommandGroupEntry entry;
Command *cmd = NULL;
bool firstRun = false;
if (m_currentCommandIndex == -1)
{
firstRun = true;
m_currentCommandIndex = 0;
}
while ((unsigned)m_currentCommandIndex < m_commands.size())
{
if (cmd != NULL)
{
if (entry.IsTimedOut())
cmd->_Cancel();
if (cmd->Run())
{
break;
}
else
{
cmd->Removed();
m_currentCommandIndex++;
firstRun = true;
cmd = NULL;
continue;
}
}
entry = m_commands[m_currentCommandIndex];
cmd = NULL;
switch (entry.m_state)
{
case CommandGroupEntry::kSequence_InSequence:
cmd = entry.m_command;
if (firstRun)
{
cmd->StartRunning();
CancelConflicts(cmd);
firstRun = false;
}
break;
case CommandGroupEntry::kSequence_BranchPeer:
m_currentCommandIndex++;
entry.m_command->Start();
break;
case CommandGroupEntry::kSequence_BranchChild:
m_currentCommandIndex++;
CancelConflicts(entry.m_command);
entry.m_command->StartRunning();
m_children.push_back(entry);
break;
}
}
// Run Children
CommandList::iterator iter = m_children.begin();
for (; iter != m_children.end();)
{
entry = *iter;
Command *child = entry.m_command;
if (entry.IsTimedOut())
child->_Cancel();
if (!child->Run())
{
child->Removed();
iter = m_children.erase(iter);
}
else
{
iter++;
}
}
}
void CommandGroup::_End()
{
// Theoretically, we don't have to check this, but we do if teams override the IsFinished method
if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size())
{
Command *cmd = m_commands[m_currentCommandIndex].m_command;
cmd->_Cancel();
cmd->Removed();
}
CommandList::iterator iter = m_children.begin();
for (; iter != m_children.end(); iter++)
{
Command *cmd = iter->m_command;
cmd->_Cancel();
cmd->Removed();
}
m_children.clear();
}
void CommandGroup::_Interrupted()
{
_End();
}
// Can be overwritten by teams
void CommandGroup::Initialize()
{
}
// Can be overwritten by teams
void CommandGroup::Execute()
{
}
// Can be overwritten by teams
void CommandGroup::End()
{
}
// Can be overwritten by teams
void CommandGroup::Interrupted()
{
}
bool CommandGroup::IsFinished()
{
return (unsigned)m_currentCommandIndex >= m_commands.size() && m_children.empty();
}
bool CommandGroup::IsInterruptible()
{
if (!Command::IsInterruptible())
return false;
if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size())
{
Command *cmd = m_commands[m_currentCommandIndex].m_command;
if (!cmd->IsInterruptible())
return false;
}
CommandList::iterator iter = m_children.begin();
for (; iter != m_children.end(); iter++)
{
if (!iter->m_command->IsInterruptible())
return false;
}
return true;
}
void CommandGroup::CancelConflicts(Command *command)
{
CommandList::iterator childIter = m_children.begin();
for (; childIter != m_children.end();)
{
Command *child = childIter->m_command;
bool erased = false;
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator requirementIter = requirements.begin();
for (; requirementIter != requirements.end(); requirementIter++)
{
if (child->DoesRequire(*requirementIter))
{
child->_Cancel();
child->Removed();
childIter = m_children.erase(childIter);
erased = true;
break;
}
}
if (!erased)
childIter++;
}
}
int CommandGroup::GetSize()
{
return m_children.size();
}

40
wpilibc/src/main/native/Commands/CommandGroupEntry.cpp
View File

@@ -1,40 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/CommandGroupEntry.h"
#include "Commands/Command.h"
CommandGroupEntry::CommandGroupEntry() :
m_timeout(-1.0),
m_command(NULL),
m_state(kSequence_InSequence)
{
}
CommandGroupEntry::CommandGroupEntry(Command *command, Sequence state) :
m_timeout(-1.0),
m_command(command),
m_state(state)
{
}
CommandGroupEntry::CommandGroupEntry(Command *command, Sequence state, double timeout) :
m_timeout(timeout),
m_command(command),
m_state(state)
{
}
bool CommandGroupEntry::IsTimedOut()
{
if (m_timeout < 0.0)
return false;
double time = m_command->TimeSinceInitialized();
if (time == 0.0)
return false;
return time >= m_timeout;
}

106
wpilibc/src/main/native/Commands/PIDCommand.cpp
View File

@@ -1,106 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/PIDCommand.h"
#include "PIDController.h"
#include "float.h"
PIDCommand::PIDCommand(const char *name, double p, double i, double d, double f, double period) :
Command(name)
{
m_controller = new PIDController(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(double p, double i, double d, double f, double period)
{
m_controller = new PIDController(p, i, d, f, this, this, period);
}
PIDCommand::PIDCommand(const char *name, double p, double i, double d) :
Command(name)
{
m_controller = new PIDController(p, i, d, this, this);
}
PIDCommand::PIDCommand(const char *name, double p, double i, double d, double period) :
Command(name)
{
m_controller = new PIDController(p, i, d, this, this, period);
}
PIDCommand::PIDCommand(double p, double i, double d)
{
m_controller = new PIDController(p, i, d, this, this);
}
PIDCommand::PIDCommand(double p, double i, double d, double period)
{
m_controller = new PIDController(p, i, d, this, this, period);
}
PIDCommand::~PIDCommand()
{
delete m_controller;
}
void PIDCommand::_Initialize()
{
m_controller->Enable();
}
void PIDCommand::_End()
{
m_controller->Disable();
}
void PIDCommand::_Interrupted()
{
_End();
}
void PIDCommand::SetSetpointRelative(double deltaSetpoint)
{
SetSetpoint(GetSetpoint() + deltaSetpoint);
}
void PIDCommand::PIDWrite(float output)
{
UsePIDOutput(output);
}
double PIDCommand::PIDGet()
{
return ReturnPIDInput();
}
PIDController *PIDCommand::GetPIDController()
{
return m_controller;
}
void PIDCommand::SetSetpoint(double setpoint)
{
m_controller->SetSetpoint(setpoint);
}
double PIDCommand::GetSetpoint()
{
return m_controller->GetSetpoint();
}
double PIDCommand::GetPosition()
{
return ReturnPIDInput();
}
std::string PIDCommand::GetSmartDashboardType(){
return "PIDCommand";
}
void PIDCommand::InitTable(ITable* table){
m_controller->InitTable(table);
Command::InitTable(table);
}

237
wpilibc/src/main/native/Commands/PIDSubsystem.cpp
View File

@@ -1,237 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/PIDSubsystem.h"
#include "PIDController.h"
#include "float.h"
// XXX max and min are not used?
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d) :
Subsystem(name)
{
m_controller = new PIDController(p, i, d, this, this);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feedforward value
*/
PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d, double f) :
Subsystem(name)
{
m_controller = new PIDController(p, i, d, f, this, this);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will also space the time
* between PID loop calculations to be equal to the given period.
* @param name the name
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feedfoward value
* @param period the time (in seconds) between calculations
*/
PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d, double f,
double period) :
Subsystem(name)
{
m_controller = new PIDController(p, i, d, f, this, this, period);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
* It will use the class name as its name.
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
*/
PIDSubsystem::PIDSubsystem(double p, double i, double d) :
Subsystem("PIDSubsystem")
{
m_controller = new PIDController(p, i, d, this, this);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
* It will use the class name as its name.
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feedforward value
*/
PIDSubsystem::PIDSubsystem(double p, double i, double d, double f) :
Subsystem("PIDSubsystem")
{
m_controller = new PIDController(p, i, d, f, this, this);
}
/**
* Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
* It will use the class name as its name.
* It will also space the time
* between PID loop calculations to be equal to the given period.
* @param p the proportional value
* @param i the integral value
* @param d the derivative value
* @param f the feedforward value
* @param period the time (in seconds) between calculations
*/
PIDSubsystem::PIDSubsystem(double p, double i, double d, double f, double period) :
Subsystem("PIDSubsystem")
{
m_controller = new PIDController(p, i, d, f, this, this, period);
}
PIDSubsystem::~PIDSubsystem()
{
delete m_controller;
}
/**
* Enables the internal {@link PIDController}
*/
void PIDSubsystem::Enable()
{
m_controller->Enable();
}
/**
* Disables the internal {@link PIDController}
*/
void PIDSubsystem::Disable()
{
m_controller->Disable();
}
/**
* Returns the {@link PIDController} used by this {@link PIDSubsystem}.
* Use this if you would like to fine tune the pid loop.
*
* @return the {@link PIDController} used by this {@link PIDSubsystem}
*/
PIDController *PIDSubsystem::GetPIDController()
{
return m_controller;
}
/**
* Sets the setpoint to the given value. If {@link PIDCommand#SetRange(double, double) SetRange(...)}
* was called,
* then the given setpoint
* will be trimmed to fit within the range.
* @param setpoint the new setpoint
*/
void PIDSubsystem::SetSetpoint(double setpoint)
{
m_controller->SetSetpoint(setpoint);
}
/**
* Adds the given value to the setpoint.
* If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used,
* then the bounds will still be honored by this method.
* @param deltaSetpoint the change in the setpoint
*/
void PIDSubsystem::SetSetpointRelative(double deltaSetpoint)
{
SetSetpoint(GetSetpoint() + deltaSetpoint);
}
/**
* Return the current setpoint
* @return The current setpoint
*/
double PIDSubsystem::GetSetpoint()
{
return m_controller->GetSetpoint();
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
void PIDSubsystem::SetInputRange(float minimumInput, float maximumInput)
{
m_controller->SetInputRange(minimumInput, maximumInput);
}
/*
* Set the absolute error which is considered tolerable for use with
* OnTarget.
* @param percentage error which is tolerable
*/
void PIDSubsystem::SetAbsoluteTolerance(float absValue) {
m_controller->SetAbsoluteTolerance(absValue);
}
/*
* Set the percentage error which is considered tolerable for use with
* OnTarget.
* @param percentage error which is tolerable
*/
void PIDSubsystem::SetPercentTolerance(float percent) {
m_controller->SetPercentTolerance(percent);
}
/*
* Return true if the error is within the percentage of the total input range,
* determined by SetTolerance. This asssumes that the maximum and minimum input
* were set using SetInput. Use OnTarget() in the IsFinished() method of commands
* that use this subsystem.
*
* Currently this just reports on target as the actual value passes through the setpoint.
* Ideally it should be based on being within the tolerance for some period of time.
*
* @return true if the error is within the percentage tolerance of the input range
*/
bool PIDSubsystem::OnTarget()
{
return m_controller->OnTarget();
}
/**
* Returns the current position
* @return the current position
*/
double PIDSubsystem::GetPosition()
{
return ReturnPIDInput();
}
void PIDSubsystem::PIDWrite(float output)
{
UsePIDOutput(output);
}
double PIDSubsystem::PIDGet()
{
return ReturnPIDInput();
}
std::string PIDSubsystem::GetSmartDashboardType(){
return "PIDCommand";
}
void PIDSubsystem::InitTable(ITable* table){
m_controller->InitTable(table);
Subsystem::InitTable(table);
}

37
wpilibc/src/main/native/Commands/PrintCommand.cpp
View File

@@ -1,37 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/PrintCommand.h"
#include "stdio.h"
#include <sstream>
PrintCommand::PrintCommand(const char *message) :
Command(((std::stringstream&)(std::stringstream("Print \"") << message << "\"")).str().c_str())
{
m_message = message;
}
void PrintCommand::Initialize()
{
printf(m_message.c_str());
}
void PrintCommand::Execute()
{
}
bool PrintCommand::IsFinished()
{
return true;
}
void PrintCommand::End()
{
}
void PrintCommand::Interrupted()
{
}

285
wpilibc/src/main/native/Commands/Scheduler.cpp
View File

@@ -1,285 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/Scheduler.h"
#include "Buttons/ButtonScheduler.h"
#include "Commands/Subsystem.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <iostream>
#include <set>
#include <algorithm>
Scheduler *Scheduler::_instance = NULL;
Scheduler::Scheduler() :
m_buttonsLock(NULL), m_additionsLock(NULL), m_adding(false) {
m_buttonsLock = initializeMutexNormal();
m_additionsLock = initializeMutexNormal();
HALReport(HALUsageReporting::kResourceType_Command,
HALUsageReporting::kCommand_Scheduler);
m_table = NULL;
m_enabled = true;
}
Scheduler::~Scheduler() {
takeMutex(m_additionsLock);
deleteMutex(m_additionsLock);
takeMutex(m_buttonsLock);
deleteMutex(m_buttonsLock);
}
/**
* Returns the {@link Scheduler}, creating it if one does not exist.
* @return the {@link Scheduler}
*/
Scheduler *Scheduler::GetInstance() {
if (_instance == NULL)
_instance = new Scheduler();
return _instance;
}
void Scheduler::SetEnabled(bool enabled) {
m_enabled = enabled;
}
/**
* Add a command to be scheduled later.
* In any pass through the scheduler, all commands are added to the additions list, then
* at the end of the pass, they are all scheduled.
* @param command The command to be scheduled
*/
void Scheduler::AddCommand(Command *command) {
Synchronized sync(m_additionsLock);
if (std::find(m_additions.begin(), m_additions.end(), command)
!= m_additions.end())
return;
m_additions.push_back(command);
}
void Scheduler::AddButton(ButtonScheduler *button) {
Synchronized sync(m_buttonsLock);
m_buttons.push_back(button);
}
void Scheduler::ProcessCommandAddition(Command *command) {
if (command == NULL)
return;
// Check to make sure no adding during adding
if (m_adding) {
wpi_setWPIErrorWithContext(IncompatibleState, "Can not start command from cancel method");
return;
}
// Only add if not already in
CommandSet::iterator found = m_commands.find(command);
if (found == m_commands.end()) {
// Check that the requirements can be had
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter;
for (iter = requirements.begin(); iter != requirements.end(); iter++) {
Subsystem *lock = *iter;
if (lock->GetCurrentCommand() != NULL
&& !lock->GetCurrentCommand()->IsInterruptible())
return;
}
// Give it the requirements
m_adding = true;
for (iter = requirements.begin(); iter != requirements.end(); iter++) {
Subsystem *lock = *iter;
if (lock->GetCurrentCommand() != NULL) {
lock->GetCurrentCommand()->Cancel();
Remove(lock->GetCurrentCommand());
}
lock->SetCurrentCommand(command);
}
m_adding = false;
m_commands.insert(command);
command->StartRunning();
m_runningCommandsChanged = true;
}
}
/**
* Runs a single iteration of the loop. This method should be called often in order to have a functioning
* {@link Command} system. The loop has five stages:
*
* <ol>
* <li> Poll the Buttons </li>
* <li> Execute/Remove the Commands </li>
* <li> Send values to SmartDashboard </li>
* <li> Add Commands </li>
* <li> Add Defaults </li>
* </ol>
*/
void Scheduler::Run() {
// Get button input (going backwards preserves button priority)
{
if (!m_enabled)
return;
Synchronized sync(m_buttonsLock);
ButtonVector::reverse_iterator rButtonIter = m_buttons.rbegin();
for (; rButtonIter != m_buttons.rend(); rButtonIter++) {
(*rButtonIter)->Execute();
}
}
m_runningCommandsChanged = false;
// Loop through the commands
CommandSet::iterator commandIter = m_commands.begin();
for (; commandIter != m_commands.end();) {
Command *command = *commandIter;
// Increment before potentially removing to keep the iterator valid
commandIter++;
if (!command->Run()) {
Remove(command);
m_runningCommandsChanged = true;
}
}
// Add the new things
{
Synchronized sync(m_additionsLock);
CommandVector::iterator additionsIter = m_additions.begin();
for (; additionsIter != m_additions.end(); additionsIter++) {
ProcessCommandAddition(*additionsIter);
}
m_additions.clear();
}
// Add in the defaults
Command::SubsystemSet::iterator subsystemIter = m_subsystems.begin();
for (; subsystemIter != m_subsystems.end(); subsystemIter++) {
Subsystem *lock = *subsystemIter;
if (lock->GetCurrentCommand() == NULL) {
ProcessCommandAddition(lock->GetDefaultCommand());
}
lock->ConfirmCommand();
}
UpdateTable();
}
/**
* 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 == NULL) {
wpi_setWPIErrorWithContext(NullParameter, "subsystem");
return;
}
m_subsystems.insert(subsystem);
}
/**
* Removes the {@link Command} from the {@link Scheduler}.
* @param command the command to remove
*/
void Scheduler::Remove(Command *command) {
if (command == NULL) {
wpi_setWPIErrorWithContext(NullParameter, "command");
return;
}
if (!m_commands.erase(command))
return;
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++) {
Subsystem *lock = *iter;
lock->SetCurrentCommand(NULL);
}
command->Removed();
}
void Scheduler::RemoveAll() {
while (m_commands.size() > 0) {
Remove(*m_commands.begin());
}
}
/**
* Update the network tables associated with the Scheduler object on the SmartDashboard
*/
void Scheduler::UpdateTable() {
CommandSet::iterator commandIter;
if (m_table != NULL) {
// Get the list of possible commands to cancel
m_table->RetrieveValue("Cancel", *toCancel);
// m_table->RetrieveValue("Ids", *ids);
// cancel commands that have had the cancel buttons pressed
// on the SmartDashboad
if (toCancel->size() > 0) {
for (commandIter = m_commands.begin(); commandIter
!= m_commands.end(); ++commandIter) {
for (unsigned i = 0; i < toCancel->size(); i++) {
Command *c = *commandIter;
if (c->GetID() == toCancel->get(i)) {
c->Cancel();
}
}
}
toCancel->setSize(0);
m_table->PutValue("Cancel", *toCancel);
}
// Set the running commands
if (m_runningCommandsChanged) {
commands->setSize(0);
ids->setSize(0);
for (commandIter = m_commands.begin(); commandIter != m_commands.end(); ++commandIter) {
Command *c = *commandIter;
commands->add(c->GetName());
ids->add(c->GetID());
}
m_table->PutValue("Names", *commands);
m_table->PutValue("Ids", *ids);
}
}
}
std::string Scheduler::GetName() {
return "Scheduler";
}
std::string Scheduler::GetType() {
return "Scheduler";
}
std::string Scheduler::GetSmartDashboardType() {
return "Scheduler";
}
void Scheduler::InitTable(ITable *subTable) {
m_table = subTable;
commands = new StringArray();
ids = new NumberArray();
toCancel = new NumberArray();
m_table->PutValue("Names", *commands);
m_table->PutValue("Ids", *ids);
m_table->PutValue("Cancel", *toCancel);
}
ITable * Scheduler::GetTable() {
return m_table;
}

35
wpilibc/src/main/native/Commands/StartCommand.cpp
View File

@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/StartCommand.h"
StartCommand::StartCommand(Command *commandToStart) :
Command("StartCommand")
{
m_commandToFork = commandToStart;
}
void StartCommand::Initialize()
{
m_commandToFork->Start();
}
void StartCommand::Execute()
{
}
void StartCommand::End()
{
}
void StartCommand::Interrupted()
{
}
bool StartCommand::IsFinished()
{
return true;
}

179
wpilibc/src/main/native/Commands/Subsystem.cpp
View File

@@ -1,179 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/Subsystem.h"
#include "Commands/Command.h"
#include "Commands/Scheduler.h"
#include "WPIErrors.h"
/**
* Creates a subsystem with the given name
* @param name the name of the subsystem
*/
Subsystem::Subsystem(const char *name) :
m_currentCommand(NULL),
m_defaultCommand(NULL),
m_initializedDefaultCommand(false)
{
m_name = name;
Scheduler::GetInstance()->RegisterSubsystem(this);
m_table = NULL;
m_currentCommandChanged = true;
}
/**
* Initialize the default command for this subsystem
* This is meant to be the place to call SetDefaultCommand in a subsystem and will be called
* on all the subsystems by the CommandBase method before the program starts running by using
* the list of all registered Subsystems inside the Scheduler.
*
* This should be overridden by a Subsystem that has a default Command
*/
void Subsystem::InitDefaultCommand() {
}
/**
* Sets the default command. If this is not called or is called with null,
* then there will be no default command for the subsystem.
*
* <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
* singleton.</p>
*
* @param command the default command (or null if there should be none)
*/
void Subsystem::SetDefaultCommand(Command *command)
{
if (command == NULL)
{
m_defaultCommand = NULL;
}
else
{
bool found = false;
Command::SubsystemSet requirements = command->GetRequirements();
Command::SubsystemSet::iterator iter = requirements.begin();
for (; iter != requirements.end(); iter++)
{
if (*iter == this)
{
found = true;
break;
}
}
if (!found)
{
wpi_setWPIErrorWithContext(CommandIllegalUse, "A default command must require the subsystem");
return;
}
m_defaultCommand = command;
}
if (m_table != NULL)
{
if (m_defaultCommand != NULL)
{
m_table->PutBoolean("hasDefault", true);
m_table->PutString("default", m_defaultCommand->GetName());
}
else
{
m_table->PutBoolean("hasDefault", false);
}
}
}
/**
* Returns the default command (or null if there is none).
* @return the default command
*/
Command *Subsystem::GetDefaultCommand()
{
if (!m_initializedDefaultCommand) {
m_initializedDefaultCommand = true;
InitDefaultCommand();
}
return m_defaultCommand;
}
/**
* Sets the current command
* @param command the new current command
*/
void Subsystem::SetCurrentCommand(Command *command)
{
m_currentCommand = command;
m_currentCommandChanged = true;
}
/**
* Returns the command which currently claims this subsystem.
* @return the command which currently claims this subsystem
*/
Command *Subsystem::GetCurrentCommand()
{
return m_currentCommand;
}
/**
* Call this to alert Subsystem that the current command is actually the command.
* Sometimes, the {@link Subsystem} is told that it has no command while the {@link Scheduler}
* is going through the loop, only to be soon after given a new one. This will avoid that situation.
*/
void Subsystem::ConfirmCommand()
{
if (m_currentCommandChanged) {
if (m_table != NULL)
{
if (m_currentCommand != NULL)
{
m_table->PutBoolean("hasCommand", true);
m_table->PutString("command", m_currentCommand->GetName());
}
else
{
m_table->PutBoolean("hasCommand", false);
}
}
m_currentCommandChanged = false;
}
}
std::string Subsystem::GetName()
{
return m_name;
}
std::string Subsystem::GetSmartDashboardType()
{
return "Subsystem";
}
void Subsystem::InitTable(ITable* table)
{
m_table = table;
if(m_table!=NULL){
if (m_defaultCommand != NULL) {
m_table->PutBoolean("hasDefault", true);
m_table->PutString("default", m_defaultCommand->GetName());
} else {
m_table->PutBoolean("hasDefault", false);
}
if (m_currentCommand != NULL) {
m_table->PutBoolean("hasCommand", true);
m_table->PutString("command", m_currentCommand->GetName());
} else {
m_table->PutBoolean("hasCommand", false);
}
}
}
ITable* Subsystem::GetTable(){
return m_table;
}

39
wpilibc/src/main/native/Commands/WaitCommand.cpp
View File

@@ -1,39 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/WaitCommand.h"
#include <sstream>
WaitCommand::WaitCommand(double timeout) :
Command(((std::stringstream&)(std::stringstream("Wait(") << timeout << ")")).str().c_str(), timeout)
{
}
WaitCommand::WaitCommand(const char *name, double timeout) :
Command(name, timeout)
{
}
void WaitCommand::Initialize()
{
}
void WaitCommand::Execute()
{
}
bool WaitCommand::IsFinished()
{
return IsTimedOut();
}
void WaitCommand::End()
{
}
void WaitCommand::Interrupted()
{
}

39
wpilibc/src/main/native/Commands/WaitForChildren.cpp
View File

@@ -1,39 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/WaitForChildren.h"
#include "Commands/CommandGroup.h"
WaitForChildren::WaitForChildren(double timeout) :
Command("WaitForChildren", timeout)
{
}
WaitForChildren::WaitForChildren(const char *name, double timeout) :
Command(name, timeout)
{
}
void WaitForChildren::Initialize()
{
}
void WaitForChildren::Execute()
{
}
void WaitForChildren::End()
{
}
void WaitForChildren::Interrupted()
{
}
bool WaitForChildren::IsFinished()
{
return GetGroup() == NULL || GetGroup()->GetSize() == 0;
}

51
wpilibc/src/main/native/Commands/WaitUntilCommand.cpp
View File

@@ -1,51 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Commands/WaitUntilCommand.h"
#include "DriverStation.h"
#include "Timer.h"
/**
* A {@link WaitCommand} will wait until a certain match time before finishing.
* This will wait until the game clock reaches some value, then continue to the
* next command.
* @see CommandGroup
*/
WaitUntilCommand::WaitUntilCommand(double time) :
Command("WaitUntilCommand", time)
{
m_time = time;
}
WaitUntilCommand::WaitUntilCommand(const char *name, double time) :
Command(name, time)
{
m_time = time;
}
void WaitUntilCommand::Initialize()
{
}
void WaitUntilCommand::Execute()
{
}
/**
* Check if we've reached the actual finish time.
*/
bool WaitUntilCommand::IsFinished()
{
return DriverStation::GetInstance()->GetMatchTime() >= m_time;
}
void WaitUntilCommand::End()
{
}
void WaitUntilCommand::Interrupted()
{
}

195
wpilibc/src/main/native/Compressor.cpp
View File

@@ -1,195 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Compressor.h"
#include "DigitalInput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Timer.h"
#include "WPIErrors.h"
/**
* Internal task.
*
* Task which checks the compressor pressure switch and operates the relay as necessary
* depending on the pressure.
*
* Do not call this function directly.
*/
static void CompressorChecker(Compressor *c)
{
while (1)
{
if (c->Enabled())
{
c->SetRelayValue( c->GetPressureSwitchValue() == 0 ? Relay::kOn : Relay::kOff );
}
else
{
c->SetRelayValue(Relay::kOff);
}
Wait(0.5);
}
}
/**
* Initialize the Compressor object.
* This method is the common initialization code for all the constructors for the Compressor
* object. It takes the relay channel and pressure switch channel and spawns a task that polls the
* compressor and sensor.
*
* You MUST start the compressor by calling the Start() method.
*/
void Compressor::InitCompressor(uint8_t pressureSwitchModuleNumber,
uint32_t pressureSwitchChannel,
uint8_t compresssorRelayModuleNumber,
uint32_t compressorRelayChannel)
{
m_table = NULL;
m_enabled = false;
m_pressureSwitch = new DigitalInput(pressureSwitchModuleNumber, pressureSwitchChannel);
m_relay = new Relay(compresssorRelayModuleNumber, compressorRelayChannel, Relay::kForwardOnly);
HALReport(HALUsageReporting::kResourceType_Compressor, 0);
if (!m_task.Start((int32_t)this))
{
wpi_setWPIError(CompressorTaskError);
}
}
/**
* Compressor constructor.
* Given a fully specified relay channel and pressure switch channel, initialize the Compressor object.
*
* You MUST start the compressor by calling the Start() method.
*
* @param pressureSwitchModuleNumber The digital module that the pressure switch is attached to.
* @param pressureSwitchChannel The GPIO channel that the pressure switch is attached to.
* @param compresssorRelayModuleNumber The digital module that the compressor relay is attached to.
* @param compressorRelayChannel The relay channel that the compressor relay is attached to.
*/
Compressor::Compressor(uint8_t pressureSwitchModuleNumber,
uint32_t pressureSwitchChannel,
uint8_t compresssorRelayModuleNumber,
uint32_t compressorRelayChannel)
: m_task ("Compressor", (FUNCPTR)CompressorChecker)
{
InitCompressor(pressureSwitchModuleNumber,
pressureSwitchChannel,
compresssorRelayModuleNumber,
compressorRelayChannel);
}
/**
* Compressor constructor.
* Given a relay channel and pressure switch channel (both in the default digital module), initialize
* the Compressor object.
*
* You MUST start the compressor by calling the Start() method.
*
* @param pressureSwitchChannel The GPIO channel that the pressure switch is attached to.
* @param compressorRelayChannel The relay channel that the compressor relay is attached to.
*/
Compressor::Compressor(uint32_t pressureSwitchChannel, uint32_t compressorRelayChannel)
: m_task ("Compressor", (FUNCPTR)CompressorChecker)
{
InitCompressor(GetDefaultDigitalModule(),
pressureSwitchChannel,
GetDefaultDigitalModule(),
compressorRelayChannel);
}
/**
* Delete the Compressor object.
* Delete the allocated resources for the compressor and kill the compressor task that is polling
* the pressure switch.
*/
Compressor::~Compressor()
{
delete m_pressureSwitch;
delete m_relay;
}
/**
* Operate the relay for the compressor.
* Change the value of the relay output that is connected to the compressor motor.
* This is only intended to be called by the internal polling thread.
*/
void Compressor::SetRelayValue(Relay::Value relayValue)
{
m_relay->Set(relayValue);
}
/**
* Get the pressure switch value.
* Read the pressure switch digital input.
*
* @return The current state of the pressure switch.
*/
uint32_t Compressor::GetPressureSwitchValue()
{
return m_pressureSwitch->Get();
}
/**
* Start the compressor.
* This method will allow the polling loop to actually operate the compressor. The
* is stopped by default and won't operate until starting it.
*/
void Compressor::Start()
{
m_enabled = true;
}
/**
* Stop the compressor.
* This method will stop the compressor from turning on.
*/
void Compressor::Stop()
{
m_enabled = false;
}
/**
* Get the state of the enabled flag.
* Return the state of the enabled flag for the compressor and pressure switch
* combination.
*
* @return The state of the compressor thread's enable flag.
*/
bool Compressor::Enabled()
{
return m_enabled;
}
void Compressor::UpdateTable() {
if (m_table != NULL) {
m_table->PutBoolean("Enabled", m_enabled);
m_table->PutBoolean("Pressure switch", GetPressureSwitchValue());
}
}
void Compressor::StartLiveWindowMode() {
}
void Compressor::StopLiveWindowMode() {
}
std::string Compressor::GetSmartDashboardType() {
return "Compressor";
}
void Compressor::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Compressor::GetTable() {
return m_table;
}

667
wpilibc/src/main/native/Counter.cpp
View File

@@ -1,667 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Counter.h"
#include "AnalogTrigger.h"
#include "DigitalInput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
/**
* Create an instance of a counter object.
* This creates a ChipObject counter and initializes status variables appropriately
*/
void Counter::InitCounter(Mode mode)
{
m_table = NULL;
int32_t status = 0;
uint32_t index = 0;
m_counter = initializeCounter(mode, &index, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_upSource = NULL;
m_downSource = NULL;
m_allocatedUpSource = false;
m_allocatedDownSource = false;
HALReport(HALUsageReporting::kResourceType_Counter, index, mode);
}
/**
* Create an instance of a counter where no sources are selected.
* Then they all must be selected by calling functions to specify the upsource and the downsource
* independently.
*/
Counter::Counter() :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
}
/**
* Create an instance of a counter from a Digital Input.
* This is used if an existing digital input is to be shared by multiple other objects such
* as encoders.
*/
Counter::Counter(DigitalSource *source) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
SetUpSource(source);
ClearDownSource();
}
Counter::Counter(DigitalSource &source) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
SetUpSource(&source);
ClearDownSource();
}
/**
* Create an instance of a Counter object.
* Create an up-Counter instance given a channel. The default digital module is assumed.
*/
Counter::Counter(uint32_t channel) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
SetUpSource(channel);
ClearDownSource();
}
/**
* Create an instance of a Counter object.
* Create an instance of an up-Counter given a digital module and a channel.
* @param moduleNumber The digital module (1 or 2).
* @param channel The channel in the digital module
*/
Counter::Counter(uint8_t moduleNumber, uint32_t channel) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
SetUpSource(moduleNumber, channel);
ClearDownSource();
}
/**
* Create an instance of a Counter object.
* Create an instance of a simple up-Counter given an analog trigger.
* Use the trigger state output from the analog trigger.
*/
Counter::Counter(AnalogTrigger *trigger) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
SetUpSource(trigger->CreateOutput(kState));
ClearDownSource();
m_allocatedUpSource = true;
}
Counter::Counter(AnalogTrigger &trigger) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
InitCounter();
SetUpSource(trigger.CreateOutput(kState));
ClearDownSource();
m_allocatedUpSource = true;
}
Counter::Counter(EncodingType encodingType, DigitalSource *upSource, DigitalSource *downSource, bool inverted) :
m_upSource(NULL),
m_downSource(NULL),
m_counter(NULL)
{
if (encodingType != k1X && encodingType != k2X)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports 1X and 2X quadrature decoding.");
return;
}
InitCounter(kExternalDirection);
SetUpSource(upSource);
SetDownSource(downSource);
int32_t status = 0;
if (encodingType == k1X)
{
SetUpSourceEdge(true, false);
setCounterAverageSize(m_counter, 1, &status);
}
else
{
SetUpSourceEdge(true, true);
setCounterAverageSize(m_counter, 2, &status);
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
SetDownSourceEdge(inverted, true);
}
/**
* Delete the Counter object.
*/
Counter::~Counter()
{
SetUpdateWhenEmpty(true);
if (m_allocatedUpSource)
{
delete m_upSource;
m_upSource = NULL;
}
if (m_allocatedDownSource)
{
delete m_downSource;
m_downSource = NULL;
}
int32_t status = 0;
freeCounter(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_counter = NULL;
}
/**
* Set the up source for the counter as digital input channel and slot.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The digital channel (1..14).
*/
void Counter::SetUpSource(uint8_t moduleNumber, uint32_t channel)
{
if (StatusIsFatal()) return;
SetUpSource(new DigitalInput(moduleNumber, channel));
m_allocatedUpSource = true;
}
/**
* Set the upsource for the counter as a digital input channel.
* The slot will be the default digital module slot.
*/
void Counter::SetUpSource(uint32_t channel)
{
if (StatusIsFatal()) return;
SetUpSource(GetDefaultDigitalModule(), channel);
m_allocatedUpSource = true;
}
/**
* Set the up counting source to be an analog trigger.
* @param analogTrigger The analog trigger object that is used for the Up Source
* @param triggerType The analog trigger output that will trigger the counter.
*/
void Counter::SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType)
{
if (StatusIsFatal()) return;
SetUpSource(analogTrigger->CreateOutput(triggerType));
m_allocatedUpSource = true;
}
/**
* Set the up counting source to be an analog trigger.
* @param analogTrigger The analog trigger object that is used for the Up Source
* @param triggerType The analog trigger output that will trigger the counter.
*/
void Counter::SetUpSource(AnalogTrigger &analogTrigger, AnalogTriggerType triggerType)
{
SetUpSource(&analogTrigger, triggerType);
}
/**
* Set the source object that causes the counter to count up.
* Set the up counting DigitalSource.
*/
void Counter::SetUpSource(DigitalSource *source)
{
if (StatusIsFatal()) return;
if (m_allocatedUpSource)
{
delete m_upSource;
m_upSource = NULL;
m_allocatedUpSource = false;
}
m_upSource = source;
if (m_upSource->StatusIsFatal())
{
CloneError(m_upSource);
}
else
{
int32_t status = 0;
setCounterUpSourceWithModule(m_counter, source->GetModuleForRouting(), source->GetChannelForRouting(),
source->GetAnalogTriggerForRouting(), &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Set the source object that causes the counter to count up.
* Set the up counting DigitalSource.
*/
void Counter::SetUpSource(DigitalSource &source)
{
SetUpSource(&source);
}
/**
* Set the edge sensitivity on an up counting source.
* Set the up source to either detect rising edges or falling edges.
*/
void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_upSource == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "Must set non-NULL UpSource before setting UpSourceEdge");
}
int32_t status = 0;
setCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Disable the up counting source to the counter.
*/
void Counter::ClearUpSource()
{
if (StatusIsFatal()) return;
if (m_allocatedUpSource)
{
delete m_upSource;
m_upSource = NULL;
m_allocatedUpSource = false;
}
int32_t status = 0;
clearCounterUpSource(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the down counting source to be a digital input channel.
* The slot will be set to the default digital module slot.
*/
void Counter::SetDownSource(uint32_t channel)
{
if (StatusIsFatal()) return;
SetDownSource(new DigitalInput(channel));
m_allocatedDownSource = true;
}
/**
* Set the down counting source to be a digital input slot and channel.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The digital channel (1..14).
*/
void Counter::SetDownSource(uint8_t moduleNumber, uint32_t channel)
{
if (StatusIsFatal()) return;
SetDownSource(new DigitalInput(moduleNumber, channel));
m_allocatedDownSource = true;
}
/**
* Set the down counting source to be an analog trigger.
* @param analogTrigger The analog trigger object that is used for the Down Source
* @param triggerType The analog trigger output that will trigger the counter.
*/
void Counter::SetDownSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType)
{
if (StatusIsFatal()) return;
SetDownSource(analogTrigger->CreateOutput(triggerType));
m_allocatedDownSource = true;
}
/**
* Set the down counting source to be an analog trigger.
* @param analogTrigger The analog trigger object that is used for the Down Source
* @param triggerType The analog trigger output that will trigger the counter.
*/
void Counter::SetDownSource(AnalogTrigger &analogTrigger, AnalogTriggerType triggerType)
{
SetDownSource(&analogTrigger, triggerType);
}
/**
* Set the source object that causes the counter to count down.
* Set the down counting DigitalSource.
*/
void Counter::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
{
int32_t status = 0;
setCounterDownSourceWithModule(m_counter, source->GetModuleForRouting(),source->GetChannelForRouting(),
source->GetAnalogTriggerForRouting(), &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Set the source object that causes the counter to count down.
* Set the down counting DigitalSource.
*/
void Counter::SetDownSource(DigitalSource &source)
{
SetDownSource(&source);
}
/**
* Set the edge sensitivity on a down counting source.
* Set the down source to either detect rising edges or falling edges.
*/
void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_downSource == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "Must set non-NULL DownSource before setting DownSourceEdge");
}
int32_t status = 0;
setCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Disable the down counting source to the counter.
*/
void Counter::ClearDownSource()
{
if (StatusIsFatal()) return;
if (m_allocatedDownSource)
{
delete m_downSource;
m_downSource = NULL;
m_allocatedDownSource = false;
}
int32_t status = 0;
clearCounterDownSource(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set standard up / down counting mode on this counter.
* Up and down counts are sourced independently from two inputs.
*/
void Counter::SetUpDownCounterMode()
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterUpDownMode(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set external direction mode on this counter.
* Counts are sourced on the Up counter input.
* The Down counter input represents the direction to count.
*/
void Counter::SetExternalDirectionMode()
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterExternalDirectionMode(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set Semi-period mode on this counter.
* Counts up on both rising and falling edges.
*/
void Counter::SetSemiPeriodMode(bool highSemiPeriod)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the counter to count in up or down based on the length of the input pulse.
* This mode is most useful for direction sensitive gear tooth sensors.
* @param threshold The pulse length beyond which the counter counts the opposite direction. Units are seconds.
*/
void Counter::SetPulseLengthMode(float threshold)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterPulseLengthMode(m_counter, threshold, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get 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.
* @return SamplesToAverage The number of samples being averaged (from 1 to 127)
*/
int Counter::GetSamplesToAverage()
{
int32_t status = 0;
int32_t samples = getCounterSamplesToAverage(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return samples;
}
/**
* 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) {
if (samplesToAverage < 1 || samplesToAverage > 127)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Average counter values must be between 1 and 127");
}
int32_t status = 0;
setCounterSamplesToAverage(m_counter, samplesToAverage, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Start the Counter counting.
* This enables the counter and it starts accumulating counts from the associated
* input channel. The counter value is not reset on starting, and still has the previous value.
*/
void Counter::Start()
{
if (StatusIsFatal()) return;
int32_t status = 0;
startCounter(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Read the current counter value.
* Read the value at this instant. It may still be running, so it reflects the current value. Next
* time it is read, it might have a different value.
*/
int32_t Counter::Get()
{
if (StatusIsFatal()) return 0;
int32_t status = 0;
int32_t value = getCounter(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Reset the Counter to zero.
* Set the counter value to zero. This doesn't effect the running state of the counter, just sets
* the current value to zero.
*/
void Counter::Reset()
{
if (StatusIsFatal()) return;
int32_t status = 0;
resetCounter(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Stop the Counter.
* Stops the counting but doesn't effect the current value.
*/
void Counter::Stop()
{
if (StatusIsFatal()) return;
int32_t status = 0;
stopCounter(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/*
* Get the Period of the most recent count.
* Returns the time interval of the most recent count. This can be used for velocity calculations
* to determine shaft speed.
* @returns The period of the last two pulses in units of seconds.
*/
double Counter::GetPeriod()
{
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = getCounterPeriod(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Set the maximum period where the device is still considered "moving".
* Sets the maximum period where the device is considered moving. This value is used to determine
* the "stopped" state of the counter using the GetStopped method.
* @param maxPeriod The maximum period where the counted device is considered moving in
* seconds.
*/
void Counter::SetMaxPeriod(double maxPeriod)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterMaxPeriod(m_counter, maxPeriod, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Select whether you want to continue updating the event timer output when there are no samples captured.
* The output of the event timer has a buffer of periods that are averaged and posted to
* a register on the FPGA. When the timer detects that the event source has stopped
* (based on the MaxPeriod) the buffer of samples to be averaged is emptied. If you
* enable the update when empty, you will be notified of the stopped source and the event
* time will report 0 samples. If you disable update when empty, the most recent average
* will remain on the output until a new sample is acquired. You will never see 0 samples
* output (except when there have been no events since an FPGA reset) and you will likely not
* see the stopped bit become true (since it is updated at the end of an average and there are
* no samples to average).
*/
void Counter::SetUpdateWhenEmpty(bool enabled)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterUpdateWhenEmpty(m_counter, enabled, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Determine if the clock is stopped.
* Determine if the clocked input is stopped based on the MaxPeriod value set using the
* SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and counter) are
* assumed to be stopped and it returns true.
* @return Returns true if the most recent counter period exceeds the MaxPeriod value set by
* SetMaxPeriod.
*/
bool Counter::GetStopped()
{
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = getCounterStopped(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* The last direction the counter value changed.
* @return The last direction the counter value changed.
*/
bool Counter::GetDirection()
{
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = getCounterDirection(m_counter, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Set the Counter to return reversed sensing on the direction.
* This allows counters to change the direction they are counting in the case of 1X and 2X
* quadrature encoding only. Any other counter mode isn't supported.
* @param reverseDirection true if the value counted should be negated.
*/
void Counter::SetReverseDirection(bool reverseDirection)
{
if (StatusIsFatal()) return;
int32_t status = 0;
setCounterReverseDirection(m_counter, reverseDirection, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
void Counter::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", Get());
}
}
void Counter::StartLiveWindowMode() {
}
void Counter::StopLiveWindowMode() {
}
std::string Counter::GetSmartDashboardType() {
return "Counter";
}
void Counter::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Counter::GetTable() {
return m_table;
}

400
wpilibc/src/main/native/Dashboard.cpp
View File

@@ -1,400 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Dashboard.h"
#include "DriverStation.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <string.h>
#include <stdarg.h>
const int32_t Dashboard::kMaxDashboardDataSize;
/**
* Dashboard contructor.
*
* This is only called once when the DriverStation constructor is called.
*/
Dashboard::Dashboard(MUTEX_ID statusDataSem)
: m_userStatusData (NULL)
, m_userStatusDataSize (0)
, m_localBuffer (NULL)
, m_localPrintBuffer (NULL)
, m_packPtr (NULL)
, m_printSemaphore (0)
, m_statusDataSemaphore (statusDataSem)
{
m_userStatusData = new char[kMaxDashboardDataSize];
m_localBuffer = new char[kMaxDashboardDataSize];
m_localPrintBuffer = new char[kMaxDashboardDataSize * 2];
m_localPrintBuffer[0] = 0;
m_packPtr = m_localBuffer;
m_printSemaphore = initializeMutexNormal();
}
/**
* Dashboard destructor.
*
* Called only when the DriverStation class is destroyed.
*/
Dashboard::~Dashboard()
{
deleteMutex(m_printSemaphore);
m_packPtr = NULL;
delete [] m_localPrintBuffer;
m_localPrintBuffer = NULL;
delete [] m_localBuffer;
m_localBuffer = NULL;
delete [] m_userStatusData;
m_userStatusData = NULL;
}
/**
* Pack a signed 8-bit int into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddI8(int8_t value)
{
if (!ValidateAdd(sizeof(int8_t))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kI8);
}
/**
* Pack a signed 16-bit int into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddI16(int16_t value)
{
if (!ValidateAdd(sizeof(int16_t))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kI16);
}
/**
* Pack a signed 32-bit int into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddI32(int32_t value)
{
if (!ValidateAdd(sizeof(int32_t))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kI32);
}
/**
* Pack an unsigned 8-bit int into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddU8(uint8_t value)
{
if (!ValidateAdd(sizeof(uint8_t))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kU8);
}
/**
* Pack an unsigned 16-bit int into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddU16(uint16_t value)
{
if (!ValidateAdd(sizeof(uint16_t))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kU16);
}
/**
* Pack an unsigned 32-bit int into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddU32(uint32_t value)
{
if (!ValidateAdd(sizeof(uint32_t))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kU32);
}
/**
* Pack a 32-bit floating point number into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddFloat(float value)
{
if (!ValidateAdd(sizeof(float))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kFloat);
}
/**
* Pack a 64-bit floating point number into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddDouble(double value)
{
if (!ValidateAdd(sizeof(double))) return;
memcpy(m_packPtr, (char*)&value, sizeof(value));
m_packPtr += sizeof(value);
AddedElement(kDouble);
}
/**
* Pack a boolean into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddBoolean(bool value)
{
if (!ValidateAdd(sizeof(char))) return;
*m_packPtr = value ? 1 : 0;
m_packPtr += sizeof(char);
AddedElement(kBoolean);
}
/**
* Pack a NULL-terminated string of 8-bit characters into the dashboard data structure.
* @param value Data to be packed into the structure.
*/
void Dashboard::AddString(char* value)
{
AddString(value, strlen(value));
}
/**
* Pack a string of 8-bit characters of specified length into the dashboard data structure.
* @param value Data to be packed into the structure.
* @param length The number of bytes in the string to pack.
*/
void Dashboard::AddString(char* value, int32_t length)
{
if (!ValidateAdd(length + sizeof(length))) return;
memcpy(m_packPtr, (char*)&length, sizeof(length));
m_packPtr += sizeof(length);
memcpy(m_packPtr, value, length);
m_packPtr += length;
AddedElement(kString);
}
/**
* Start an array in the packed dashboard data structure.
*
* After calling AddArray(), call the appropriate Add method for each element of the array.
* Make sure you call the same add each time. An array must contain elements of the same type.
* You can use clusters inside of arrays to make each element of the array contain a structure of values.
* You can also nest arrays inside of other arrays.
* Every call to AddArray() must have a matching call to FinalizeArray().
*/
void Dashboard::AddArray()
{
if (!ValidateAdd(sizeof(int32_t))) return;
m_complexTypeStack.push(kArray);
m_arrayElementCount.push_back(0);
m_arraySizePtr.push_back((int32_t*)m_packPtr);
m_packPtr += sizeof(int32_t);
}
/**
* Indicate the end of an array packed into the dashboard data structure.
*
* After packing data into the array, call FinalizeArray().
* Every call to AddArray() must have a matching call to FinalizeArray().
*/
void Dashboard::FinalizeArray()
{
if (m_complexTypeStack.top() != kArray)
{
wpi_setWPIError(MismatchedComplexTypeClose);
return;
}
m_complexTypeStack.pop();
*(m_arraySizePtr.back()) = m_arrayElementCount.back();
m_arraySizePtr.pop_back();
if (m_arrayElementCount.back() != 0)
{
m_expectedArrayElementType.pop_back();
}
m_arrayElementCount.pop_back();
AddedElement(kOther);
}
/**
* Start a cluster in the packed dashboard data structure.
*
* After calling AddCluster(), call the appropriate Add method for each element of the cluster.
* You can use clusters inside of arrays to make each element of the array contain a structure of values.
* Every call to AddCluster() must have a matching call to FinalizeCluster().
*/
void Dashboard::AddCluster()
{
m_complexTypeStack.push(kCluster);
}
/**
* 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);
}
/**
* Print a string to the UserData text on the Dashboard.
*
* This will add text to the buffer to send to the dashboard.
* You must call Finalize() periodically to actually send the buffer to the dashboard if you are not using the packed dashboard data.
*/
void Dashboard::Printf(const char *writeFmt, ...)
{
va_list args;
int32_t size;
// Check if the buffer has already been used for packing.
if (m_packPtr != m_localBuffer)
{
wpi_setWPIError(DashboardDataCollision);
return;
}
va_start (args, writeFmt);
{
Synchronized sync(m_printSemaphore);
vsprintf(m_localPrintBuffer + strlen(m_localPrintBuffer), writeFmt, args);
size = strlen(m_localPrintBuffer);
}
if (size > kMaxDashboardDataSize)
{
wpi_setWPIError(DashboardDataOverflow);
}
va_end (args);
}
/**
* 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_t Dashboard::Finalize()
{
if (!m_complexTypeStack.empty())
{
wpi_setWPIError(MismatchedComplexTypeClose);
return 0;
}
static bool reported = false;
if (!reported)
{
HALReport(HALUsageReporting::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;
}
/**
* Called by the DriverStation class to retrieve buffers, sizes, etc. for writing
* to the NetworkCommunication task.
* This function is called while holding the m_statusDataSemaphore.
*/
void Dashboard::GetStatusBuffer(char **userStatusData, int32_t* userStatusDataSize)
{
// User printed strings
if (m_localPrintBuffer[0] != 0)
{
// Sequence number
DriverStation::GetInstance()->IncrementUpdateNumber();
int32_t printSize;
Synchronized syncPrint(m_printSemaphore);
printSize = strlen(m_localPrintBuffer);
m_userStatusDataSize = printSize;
memcpy(m_userStatusData, m_localPrintBuffer, m_userStatusDataSize);
m_localPrintBuffer[0] = 0;
}
*userStatusData = m_userStatusData;
*userStatusDataSize = m_userStatusDataSize;
}
/**
* Validate that the data being packed will fit in the buffer.
*/
bool Dashboard::ValidateAdd(int32_t 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;
}
/**
* Check for consistent types when adding elements to an array and keep track of the number of elements in the array.
*/
void Dashboard::AddedElement(Type type)
{
if(IsArrayRoot())
{
if (m_arrayElementCount.back() == 0)
{
m_expectedArrayElementType.push_back(type);
}
else
{
if (type != m_expectedArrayElementType.back())
{
wpi_setWPIError(InconsistentArrayValueAdded);
}
}
m_arrayElementCount.back() = m_arrayElementCount.back() + 1;
}
}
/**
* If the top of the type stack an array?
*/
bool Dashboard::IsArrayRoot()
{
return !m_complexTypeStack.empty() && m_complexTypeStack.top() == kArray;
}

225
wpilibc/src/main/native/DigitalInput.cpp
View File

@@ -1,225 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DigitalInput.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
// TODO: This is not a good place for this...
Resource *interruptsResource = NULL;
/**
* Create an instance of a DigitalInput.
* Creates a digital input given a slot and channel. Common creation routine
* for all constructors.
*/
void DigitalInput::InitDigitalInput(uint8_t moduleNumber, uint32_t channel)
{
m_table = NULL;
char buf[64];
Resource::CreateResourceObject(&interruptsResource, interrupt_kNumSystems);
if (!CheckDigitalModule(moduleNumber))
{
snprintf(buf, 64, "Digital Module %d", moduleNumber);
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return;
}
if (!CheckDigitalChannel(channel))
{
snprintf(buf, 64, "Digital Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
m_channel = channel;
m_module = DigitalModule::GetInstance(moduleNumber);
m_module->AllocateDIO(channel, true);
HALReport(HALUsageReporting::kResourceType_DigitalInput, channel, moduleNumber - 1);
}
/**
* Create an instance of a Digital Input class.
* Creates a digital input given a channel and uses the default module.
*
* @param channel The digital channel (1..14).
*/
DigitalInput::DigitalInput(uint32_t channel)
{
InitDigitalInput(GetDefaultDigitalModule(), channel);
}
/**
* Create an instance of a Digital Input class.
* Creates a digital input given an channel and module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The digital channel (1..14).
*/
DigitalInput::DigitalInput(uint8_t moduleNumber, uint32_t channel)
{
InitDigitalInput(moduleNumber, channel);
}
/**
* Free resources associated with the Digital Input class.
*/
DigitalInput::~DigitalInput()
{
if (StatusIsFatal()) return;
if (m_interrupt != NULL)
{
int32_t status = 0;
cleanInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_interrupt = NULL;
interruptsResource->Free(m_interruptIndex);
}
m_module->FreeDIO(m_channel);
}
/*
* Get the value from a digital input channel.
* Retrieve the value of a single digital input channel from the FPGA.
*/
uint32_t DigitalInput::Get()
{
if (StatusIsFatal()) return 0;
return m_module->GetDIO(m_channel);
}
/**
* @return The GPIO channel number that this object represents.
*/
uint32_t DigitalInput::GetChannel()
{
return m_channel;
}
/**
* @return The value to be written to the channel field of a routing mux.
*/
uint32_t DigitalInput::GetChannelForRouting()
{
return DigitalModule::RemapDigitalChannel(GetChannel() - 1);
}
/**
* @return The value to be written to the module field of a routing mux.
*/
uint32_t DigitalInput::GetModuleForRouting()
{
if (StatusIsFatal()) return 0;
return m_module->GetNumber() - 1;
}
/**
* @return The value to be written to the analog trigger field of a routing mux.
*/
bool DigitalInput::GetAnalogTriggerForRouting()
{
return false;
}
/**
* Request interrupts asynchronously on this digital input.
* @param handler The address of the interrupt handler function of type tInterruptHandler that
* will be called whenever there is an interrupt on the digitial input port.
* Request interrupts in synchronus mode where the user program interrupt handler will be
* called when an interrupt occurs.
* The default is interrupt on rising edges only.
*/
void DigitalInput::RequestInterrupts(InterruptHandlerFunction handler, void *param)
{
if (StatusIsFatal()) return;
uint32_t index = interruptsResource->Allocate("Async Interrupt");
if (index == ~0ul)
{
CloneError(interruptsResource);
return;
}
m_interruptIndex = index;
// Creates a manager too
AllocateInterrupts(false);
int32_t status = 0;
requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
GetAnalogTriggerForRouting(), &status);
SetUpSourceEdge(true, false);
attachInterruptHandler(m_interrupt, handler, param, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Request interrupts synchronously on this digital input.
* Request interrupts in synchronus mode where the user program will have to explicitly
* wait for the interrupt to occur.
* The default is interrupt on rising edges only.
*/
void DigitalInput::RequestInterrupts()
{
if (StatusIsFatal()) return;
uint32_t index = interruptsResource->Allocate("Sync Interrupt");
if (index == ~0ul)
{
CloneError(interruptsResource);
return;
}
m_interruptIndex = index;
AllocateInterrupts(true);
int32_t status = 0;
requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
GetAnalogTriggerForRouting(), &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
SetUpSourceEdge(true, false);
}
void DigitalInput::SetUpSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_interrupt == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "You must call RequestInterrupts before SetUpSourceEdge");
return;
}
if (m_interrupt != NULL)
{
int32_t status = 0;
setInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
void DigitalInput::UpdateTable() {
if (m_table != NULL) {
m_table->PutBoolean("Value", Get());
}
}
void DigitalInput::StartLiveWindowMode() {
}
void DigitalInput::StopLiveWindowMode() {
}
std::string DigitalInput::GetSmartDashboardType() {
return "DigitalInput";
}
void DigitalInput::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * DigitalInput::GetTable() {
return m_table;
}

425
wpilibc/src/main/native/DigitalModule.cpp
View File

@@ -1,425 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DigitalModule.h"
#include "I2C.h"
#include "PWM.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <math.h>
/**
* Get an instance of an Digital Module.
* Singleton digital module creation where a module is allocated on the first use
* and the same module is returned on subsequent uses.
*
* @param moduleNumber The digital module to get (1 or 2).
*/
DigitalModule* DigitalModule::GetInstance(uint8_t moduleNumber)
{
if (checkDigitalModule(moduleNumber))
{
return (DigitalModule *)GetModule(nLoadOut::kModuleType_Digital, moduleNumber);
}
// If this wasn't caught before now, make sure we say what's wrong before we crash
char buf[64];
snprintf(buf, 64, "Digital Module %d", moduleNumber);
wpi_setGlobalWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return NULL;
}
/**
* Create a new instance of an digital module.
* Create an instance of the digital module object. Initialize all the parameters
* to reasonable values on start.
* Setting a global value on an digital module can be done only once unless subsequent
* values are set the previously set value.
* Digital modules are a singleton, so the constructor is never called outside of this class.
*
* @param moduleNumber The digital module to create (1 or 2).
*/
DigitalModule::DigitalModule(uint8_t moduleNumber)
: Module(nLoadOut::kModuleType_Digital, moduleNumber)
{
// TODO: Refactor out common logic
m_module = moduleNumber;
for (uint32_t i = 0; i < kDigitalChannels; i++)
{
void* port = getPortWithModule(moduleNumber, i+1);
int32_t status = 0;
m_digital_ports[i] = initializeDigitalPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
for (uint32_t i = 0; i < kRelayChannels; i++)
{
void* port = getPortWithModule(moduleNumber, i+1);
int32_t status = 0;
m_relay_ports[i] = initializeDigitalPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
for (uint32_t i = 0; i < kPwmChannels; i++)
{
void* port = getPortWithModule(moduleNumber, i+1);
int32_t status = 0;
m_pwm_ports[i] = initializeDigitalPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
AddToSingletonList();
}
DigitalModule::~DigitalModule()
{
}
/**
* Set a PWM channel to the desired value. The values range from 0 to 255 and the period is controlled
* by the PWM Period and MinHigh registers.
*
* @param channel The PWM channel to set.
* @param value The PWM value to set.
*/
void DigitalModule::SetPWM(uint32_t channel, unsigned short value)
{
int32_t status = 0;
setPWM(m_pwm_ports[channel-1], value, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get a value from a PWM channel. The values range from 0 to 255.
*
* @param channel The PWM channel to read from.
* @return The raw PWM value.
*/
unsigned short DigitalModule::GetPWM(uint32_t channel)
{
int32_t status = 0;
uint8_t value = getPWM(m_pwm_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Set how how often the PWM signal is squelched, thus scaling the period.
*
* @param channel The PWM channel to configure.
* @param squelchMask The 2-bit mask of outputs to squelch.
*/
void DigitalModule::SetPWMPeriodScale(uint32_t channel, uint32_t squelchMask)
{
int32_t status = 0;
setPWMPeriodScale(m_pwm_ports[channel-1], squelchMask, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the state of a relay.
* Set the state of a relay output to be forward. Relays have two outputs and each is
* independently set to 0v or 12v.
*/
void DigitalModule::SetRelayForward(uint32_t channel, bool on)
{
int32_t status = 0;
setRelayForward(m_relay_ports[channel-1], on, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Set the state of a relay.
* Set the state of a relay output to be reverse. Relays have two outputs and each is
* independently set to 0v or 12v.
*/
void DigitalModule::SetRelayReverse(uint32_t channel, bool on)
{
int32_t status = 0;
setRelayReverse(m_relay_ports[channel-1], on, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the current state of the forward relay channel
*/
bool DigitalModule::GetRelayForward(uint32_t channel)
{
int32_t status = 0;
bool on = getRelayForward(m_relay_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return on;
}
/**
* Get the current state of all of the forward relay channels on this module.
*/
uint8_t DigitalModule::GetRelayForward()
{
uint8_t value = 0;
for (unsigned int i = 0; i < kRelayChannels; i++) {
value |= GetRelayForward(i+1) << i;
}
return value;
}
/**
* Get the current state of the reverse relay channel
*/
bool DigitalModule::GetRelayReverse(uint32_t channel)
{
int32_t status = 0;
bool on = getRelayReverse(m_relay_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return on;
}
/**
* Get the current state of all of the reverse relay channels on this module.
*/
uint8_t DigitalModule::GetRelayReverse()
{
uint8_t value = 0;
for (unsigned int i = 0; i < kRelayChannels; i++) {
value |= GetRelayReverse(i+1) << i;
}
return value;
}
/**
* Allocate Digital I/O channels.
* Allocate channels so that they are not accidently reused. Also the direction is set at the
* time of the allocation.
*
* @param channel The Digital I/O channel
* @param input If true open as input; if false open as output
* @return Was successfully allocated
*/
bool DigitalModule::AllocateDIO(uint32_t channel, bool input)
{
int32_t status = 0;
bool allocated = allocateDIO(m_digital_ports[channel-1], input, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return allocated;
}
/**
* Free the resource associated with a digital I/O channel.
*
* @param channel The Digital I/O channel to free
*/
void DigitalModule::FreeDIO(uint32_t channel)
{
int32_t status = 0;
freeDIO(m_digital_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Write a digital I/O bit to the FPGA.
* Set a single value on a digital I/O channel.
*
* @param channel The Digital I/O channel
* @param value The state to set the digital channel (if it is configured as an output)
*/
void DigitalModule::SetDIO(uint32_t channel, short value)
{
int32_t status = 0;
setDIO(m_digital_ports[channel-1], value, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Read a digital I/O bit from the FPGA.
* Get a single value from a digital I/O channel.
*
* @param channel The digital I/O channel
* @return The state of the specified channel
*/
bool DigitalModule::GetDIO(uint32_t channel)
{
int32_t status = 0;
bool value = getDIO(m_digital_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Read the state of all the Digital I/O lines from the FPGA
* These are not remapped to logical order. They are still in hardware order.
*/
uint16_t DigitalModule::GetDIO()
{
uint16_t value = 0;
for (unsigned int i = 0; i < kDigitalChannels; i++) {
value |= GetDIO(i+1) << i;
}
return value;
}
/**
* Read the direction of a the Digital I/O lines
* A 1 bit means output and a 0 bit means input.
*
* @param channel The digital I/O channel
* @return The direction of the specified channel
*/
bool DigitalModule::GetDIODirection(uint32_t channel)
{
int32_t status = 0;
bool value = getDIODirection(m_digital_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Read the direction of all the Digital I/O lines from the FPGA
* A 1 bit means output and a 0 bit means input.
* These are not remapped to logical order. They are still in hardware order.
*/
uint16_t DigitalModule::GetDIODirection()
{
uint16_t value = 0;
for (unsigned int i = 0; i < kDigitalChannels; i++) {
value |= GetDIODirection(i+1) << i;
}
return value;
}
/**
* Generate a single pulse.
* Write a pulse to the specified digital output channel. There can only be a single pulse going at any time.
*
* @param channel The Digital Output channel that the pulse should be output on
* @param pulseLength The active length of the pulse (in seconds)
*/
void DigitalModule::Pulse(uint32_t channel, float pulseLength)
{
int32_t status = 0;
pulse(m_digital_ports[channel-1], pulseLength, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Check a DIO line to see if it is currently generating a pulse.
*
* @return A pulse is in progress
*/
bool DigitalModule::IsPulsing(uint32_t channel)
{
int32_t status = 0;
bool value = isPulsing(m_digital_ports[channel-1], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Check if any DIO line is currently generating a pulse.
*
* @return A pulse on some line is in progress
*/
bool DigitalModule::IsPulsing()
{
int32_t status = 0;
bool value = isAnyPulsing(&status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Allocate a DO PWM Generator.
* Allocate PWM generators so that they are not accidently reused.
*
* @return PWM Generator refnum
*/
uint32_t DigitalModule::AllocateDO_PWM()
{
int32_t status = 0;
uint32_t value = *((uint32_t*) allocatePWMWithModule(m_module, &status)); // TODO: Hacky
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Free the resource associated with a DO PWM generator.
*
* @param pwmGenerator The pwmGen to free that was allocated with AllocateDO_PWM()
*/
void DigitalModule::FreeDO_PWM(uint32_t pwmGenerator) // Note: should become void*
{
int32_t status = 0;
void* generator = &pwmGenerator;
freePWMWithModule(m_module, generator, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Change the frequency of the DO PWM generator.
*
* The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic.
*
* @param rate The frequency to output all digital output PWM signals on this module.
*/
void DigitalModule::SetDO_PWMRate(float rate)
{
int32_t status = 0;
setPWMRateWithModule(m_module, rate, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure which DO channel the PWM siganl is output on
*
* @param pwmGenerator The generator index reserved by AllocateDO_PWM()
* @param channel The Digital Output channel to output on
*/
void DigitalModule::SetDO_PWMOutputChannel(uint32_t pwmGenerator, uint32_t channel) // pwmGenerator should become void*
{
int32_t status = 0;
void* generator = &pwmGenerator;
setPWMOutputChannelWithModule(m_module, generator, channel, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the duty-cycle of the PWM generator
*
* @param pwmGenerator The generator index reserved by AllocateDO_PWM()
* @param dutyCycle The percent duty cycle to output [0..1].
*/
void DigitalModule::SetDO_PWMDutyCycle(uint32_t pwmGenerator, float dutyCycle)
{
int32_t status = 0;
void* generator = &pwmGenerator;
setPWMDutyCycleWithModule(m_module, generator, dutyCycle, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the loop timing of the Digital Module
*
* @return The loop time
*/
uint16_t DigitalModule::GetLoopTiming()
{
int32_t status = 0;
uint16_t timing = getLoopTimingWithModule(m_module, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return timing;
}
/**
* Return a pointer to an I2C object for this digital module
* The caller is responsible for deleting the pointer.
*
* @param address The address of the device on the I2C bus
* @return A pointer to an I2C object to talk to the device at address
*/
I2C* DigitalModule::GetI2C(uint32_t address)
{
return new I2C(this, address);
}

311
wpilibc/src/main/native/DigitalOutput.cpp
View File

@@ -1,311 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DigitalOutput.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
extern Resource *interruptsResource;
/**
* Create an instance of a DigitalOutput.
* Creates a digital output given a slot and channel. Common creation routine
* for all constructors.
*/
void DigitalOutput::InitDigitalOutput(uint8_t moduleNumber, uint32_t channel)
{
m_table = NULL;
char buf[64];
if (!CheckDigitalModule(moduleNumber))
{
snprintf(buf, 64, "Digital Module %d", moduleNumber);
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return;
}
if (!CheckDigitalChannel(channel))
{
snprintf(buf, 64, "Digital Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
m_channel = channel;
m_pwmGenerator = ~0ul;
m_module = DigitalModule::GetInstance(moduleNumber);
m_module->AllocateDIO(m_channel, false);
HALReport(HALUsageReporting::kResourceType_DigitalOutput, channel, moduleNumber - 1);
}
/**
* Create an instance of a digital output.
* Create a digital output given a channel. The default module is used.
*
* @param channel The digital channel (1..14).
*/
DigitalOutput::DigitalOutput(uint32_t channel)
{
InitDigitalOutput(GetDefaultDigitalModule(), channel);
}
/**
* Create an instance of a digital output.
* Create an instance of a digital output given a module number and channel.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The digital channel (1..14).
*/
DigitalOutput::DigitalOutput(uint8_t moduleNumber, uint32_t channel)
{
InitDigitalOutput(moduleNumber, channel);
}
/**
* Free the resources associated with a digital output.
*/
DigitalOutput::~DigitalOutput()
{
if (StatusIsFatal()) return;
// Disable the PWM in case it was running.
DisablePWM();
m_module->FreeDIO(m_channel);
}
/**
* Set the value of a digital output.
* Set the value of a digital output to either one (true) or zero (false).
*/
void DigitalOutput::Set(uint32_t value)
{
if (StatusIsFatal()) return;
m_module->SetDIO(m_channel, value);
}
/**
* @return The GPIO channel number that this object represents.
*/
uint32_t DigitalOutput::GetChannel()
{
return m_channel;
}
/**
* Output a single pulse on the digital output line.
* Send a single pulse on the digital output line where the pulse diration is specified in seconds.
* Maximum pulse length is 0.0016 seconds.
* @param length The pulselength in seconds
*/
void DigitalOutput::Pulse(float length)
{
if (StatusIsFatal()) return;
m_module->Pulse(m_channel, length);
}
/**
* Determine if the pulse is still going.
* Determine if a previously started pulse is still going.
*/
bool DigitalOutput::IsPulsing()
{
if (StatusIsFatal()) return false;
return m_module->IsPulsing(m_channel);
}
/**
* Change the PWM frequency of the PWM output on a Digital Output line.
*
* The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic.
*
* There is only one PWM frequency per digital module.
*
* @param rate The frequency to output all digital output PWM signals on this module.
*/
void DigitalOutput::SetPWMRate(float rate)
{
if (StatusIsFatal()) return;
m_module->SetDO_PWMRate(rate);
}
/**
* Enable a PWM Output on this line.
*
* Allocate one of the 4 DO PWM generator resources from this module.
*
* Supply the initial duty-cycle to output so as to avoid a glitch when first starting.
*
* The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less)
* but is reduced the higher the frequency of the PWM signal is.
*
* @param initialDutyCycle The duty-cycle to start generating. [0..1]
*/
void DigitalOutput::EnablePWM(float initialDutyCycle)
{
if (StatusIsFatal()) return;
if (m_pwmGenerator != ~0ul) return;
m_pwmGenerator = m_module->AllocateDO_PWM();
m_module->SetDO_PWMDutyCycle(m_pwmGenerator, initialDutyCycle);
m_module->SetDO_PWMOutputChannel(m_pwmGenerator, m_channel);
}
/**
* Change this line from a PWM output back to a static Digital Output line.
*
* Free up one of the 4 DO PWM generator resources that were in use.
*/
void DigitalOutput::DisablePWM()
{
if (StatusIsFatal()) return;
// Disable the output by routing to a dead bit.
m_module->SetDO_PWMOutputChannel(m_pwmGenerator, kDigitalChannels);
m_module->FreeDO_PWM(m_pwmGenerator);
m_pwmGenerator = ~0ul;
}
/**
* Change the duty-cycle that is being generated on the line.
*
* The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less)
* but is reduced the higher the frequency of the PWM signal is.
*
* @param dutyCycle The duty-cycle to change to. [0..1]
*/
void DigitalOutput::UpdateDutyCycle(float dutyCycle)
{
if (StatusIsFatal()) return;
m_module->SetDO_PWMDutyCycle(m_pwmGenerator, dutyCycle);
}
/**
* @return The value to be written to the channel field of a routing mux.
*/
uint32_t DigitalOutput::GetChannelForRouting()
{
return DigitalModule::RemapDigitalChannel(GetChannel() - 1);
}
/**
* @return The value to be written to the module field of a routing mux.
*/
uint32_t DigitalOutput::GetModuleForRouting()
{
if (StatusIsFatal()) return 0;
return m_module->GetNumber() - 1;
}
/**
* @return The value to be written to the analog trigger field of a routing mux.
*/
bool DigitalOutput::GetAnalogTriggerForRouting()
{
return false;
}
/**
* Request interrupts asynchronously on this digital output.
* @param handler The address of the interrupt handler function of type tInterruptHandler that
* will be called whenever there is an interrupt on the digitial output port.
* Request interrupts in synchronus mode where the user program interrupt handler will be
* called when an interrupt occurs.
* The default is interrupt on rising edges only.
*/
void DigitalOutput::RequestInterrupts(InterruptHandlerFunction handler, void *param)
{
if (StatusIsFatal()) return;
uint32_t index = interruptsResource->Allocate("Sync Interrupt");
if (index == ~0ul)
{
CloneError(interruptsResource);
return;
}
m_interruptIndex = index;
// Creates a manager too
AllocateInterrupts(false);
int32_t status = 0;
requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
GetAnalogTriggerForRouting(), &status);
SetUpSourceEdge(true, false);
attachInterruptHandler(m_interrupt, handler, param, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Request interrupts synchronously on this digital output.
* Request interrupts in synchronus mode where the user program will have to explicitly
* wait for the interrupt to occur.
* The default is interrupt on rising edges only.
*/
void DigitalOutput::RequestInterrupts()
{
if (StatusIsFatal()) return;
uint32_t index = interruptsResource->Allocate("Sync Interrupt");
if (index == ~0ul)
{
CloneError(interruptsResource);
return;
}
m_interruptIndex = index;
AllocateInterrupts(true);
int32_t status = 0;
requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
GetAnalogTriggerForRouting(), &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
SetUpSourceEdge(true, false);
}
void DigitalOutput::SetUpSourceEdge(bool risingEdge, bool fallingEdge)
{
if (StatusIsFatal()) return;
if (m_interrupt == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "You must call RequestInterrupts before SetUpSourceEdge");
return;
}
if (m_interrupt != NULL)
{
int32_t status = 0;
setInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
void DigitalOutput::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
Set(value.b);
}
void DigitalOutput::UpdateTable() {
}
void DigitalOutput::StartLiveWindowMode() {
if (m_table != NULL) {
m_table->AddTableListener("Value", this, true);
}
}
void DigitalOutput::StopLiveWindowMode() {
if (m_table != NULL) {
m_table->RemoveTableListener(this);
}
}
std::string DigitalOutput::GetSmartDashboardType() {
return "Digital Output";
}
void DigitalOutput::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * DigitalOutput::GetTable() {
return m_table;
}

14
wpilibc/src/main/native/DigitalSource.cpp
View File

@@ -1,14 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DigitalSource.h"
/**
* DigitalSource destructor.
*/
DigitalSource::~DigitalSource()
{
}

184
wpilibc/src/main/native/DoubleSolenoid.cpp
View File

@@ -1,184 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DoubleSolenoid.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include <string.h>
#include "LiveWindow/LiveWindow.h"
/**
* Common function to implement constructor behavior.
*/
void DoubleSolenoid::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_forwardChannel))
{
snprintf(buf, 64, "Solenoid Channel %d", m_forwardChannel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
if (!CheckSolenoidChannel(m_reverseChannel))
{
snprintf(buf, 64, "Solenoid Channel %d", m_reverseChannel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
Resource::CreateResourceObject(&m_allocated, solenoid_kNumDO7_0Elements * kSolenoidChannels);
snprintf(buf, 64, "Solenoid %d (Module %d)", m_forwardChannel, m_moduleNumber);
if (m_allocated->Allocate((m_moduleNumber - 1) * kSolenoidChannels + m_forwardChannel - 1, buf) == ~0ul)
{
CloneError(m_allocated);
return;
}
snprintf(buf, 64, "Solenoid %d (Module %d)", m_reverseChannel, m_moduleNumber);
if (m_allocated->Allocate((m_moduleNumber - 1) * kSolenoidChannels + m_reverseChannel - 1, buf) == ~0ul)
{
CloneError(m_allocated);
return;
}
m_forwardMask = 1 << (m_forwardChannel - 1);
m_reverseMask = 1 << (m_reverseChannel - 1);
HALReport(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel, m_moduleNumber - 1);
HALReport(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel, m_moduleNumber - 1);
LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", m_moduleNumber, m_forwardChannel, this);
}
/**
* Constructor.
*
* @param forwardChannel The forward channel on the module to control.
* @param reverseChannel The reverse channel on the module to control.
*/
DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
: SolenoidBase (GetDefaultSolenoidModule())
, m_forwardChannel (forwardChannel)
, m_reverseChannel (reverseChannel)
{
InitSolenoid();
}
/**
* Constructor.
*
* @param moduleNumber The solenoid module (1 or 2).
* @param forwardChannel The forward channel on the module to control.
* @param reverseChannel The reverse channel on the module to control.
*/
DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel)
: SolenoidBase (moduleNumber)
, m_forwardChannel (forwardChannel)
, m_reverseChannel (reverseChannel)
{
InitSolenoid();
}
/**
* Destructor.
*/
DoubleSolenoid::~DoubleSolenoid()
{
if (CheckSolenoidModule(m_moduleNumber))
{
m_allocated->Free((m_moduleNumber - 1) * kSolenoidChannels + m_forwardChannel - 1);
m_allocated->Free((m_moduleNumber - 1) * kSolenoidChannels + m_reverseChannel - 1);
}
}
/**
* Set the value of a solenoid.
*
* @param value Move the solenoid to forward, reverse, or don't move it.
*/
void DoubleSolenoid::Set(Value value)
{
if (StatusIsFatal()) return;
uint8_t rawValue = 0x00;
switch(value)
{
case kOff:
rawValue = 0x00;
break;
case kForward:
rawValue = m_forwardMask;
break;
case kReverse:
rawValue = m_reverseMask;
break;
}
SolenoidBase::Set(rawValue, m_forwardMask | m_reverseMask);
}
/**
* Read the current value of the solenoid.
*
* @return The current value of the solenoid.
*/
DoubleSolenoid::Value DoubleSolenoid::Get()
{
if (StatusIsFatal()) return kOff;
uint8_t value = GetAll();
if (value & m_forwardMask) return kForward;
if (value & m_reverseMask) return kReverse;
return kOff;
}
void DoubleSolenoid::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
Value lvalue = kOff;
std::string *val = (std::string *)value.ptr;
if (*val == "Forward")
lvalue = kForward;
else if (*val == "Reverse")
lvalue = kReverse;
Set(lvalue);
}
void DoubleSolenoid::UpdateTable() {
if (m_table != NULL) {
m_table->PutString("Value", (Get() == kForward ? "Forward" : (Get() == kReverse ? "Reverse" : "Off")));
}
}
void DoubleSolenoid::StartLiveWindowMode() {
Set(kOff);
if (m_table != NULL) {
m_table->AddTableListener("Value", this, true);
}
}
void DoubleSolenoid::StopLiveWindowMode() {
Set(kOff);
if (m_table != NULL) {
m_table->RemoveTableListener(this);
}
}
std::string DoubleSolenoid::GetSmartDashboardType() {
return "Double Solenoid";
}
void DoubleSolenoid::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * DoubleSolenoid::GetTable() {
return m_table;
}

516
wpilibc/src/main/native/DriverStation.cpp
View File

@@ -1,516 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DriverStation.h"
#include "AnalogChannel.h"
#include "HAL/cpp/Synchronized.h"
#include "Timer.h"
//#include "NetworkCommunication/FRCComm.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <string.h>
#include "Log.h"
// set the logging level
TLogLevel dsLogLevel = logDEBUG;
#define DS_LOG(level) \
if (level > dsLogLevel) ; \
else Log().Get(level)
const uint32_t DriverStation::kBatteryModuleNumber;
const uint32_t DriverStation::kBatteryChannel;
const uint32_t DriverStation::kJoystickPorts;
const uint32_t DriverStation::kJoystickAxes;
constexpr float DriverStation::kUpdatePeriod;
DriverStation* DriverStation::m_instance = NULL;
uint8_t DriverStation::m_updateNumber = 0;
/**
* DriverStation contructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation()
: m_controlData (NULL)
, m_digitalOut (0)
, m_batteryChannel (NULL)
, m_statusDataSemaphore (initializeMutexNormal())
, m_task ("DriverStation", (FUNCPTR)DriverStation::InitTask)
, m_dashboardHigh(m_statusDataSemaphore)
, m_dashboardLow(m_statusDataSemaphore)
, m_dashboardInUseHigh(&m_dashboardHigh)
, m_dashboardInUseLow(&m_dashboardLow)
, m_newControlData(0)
, m_packetDataAvailableSem (0)
, m_enhancedIO()
, m_waitForDataSem(0)
, m_approxMatchTimeOffset(-1.0)
, m_userInDisabled(false)
, m_userInAutonomous(false)
, m_userInTeleop(false)
, m_userInTest(false)
{
// Create a new semaphore
m_packetDataAvailableSem = initializeMutexNormal();
m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY);
// Register that semaphore with the network communications task.
// It will signal when new packet data is available.
HALSetNewDataSem(m_packetDataAvailableSem);
m_waitForDataSem = initializeMultiWait();
m_controlData = new HALCommonControlData;
// initialize packet number and control words to zero;
m_controlData->packetIndex = 0;
m_controlData->control = 0;
// set all joystick axis values to neutral; buttons to OFF
m_controlData->stick0Axis1 = m_controlData->stick0Axis2 = m_controlData->stick0Axis3 = 0;
m_controlData->stick1Axis1 = m_controlData->stick1Axis2 = m_controlData->stick1Axis3 = 0;
m_controlData->stick2Axis1 = m_controlData->stick2Axis2 = m_controlData->stick2Axis3 = 0;
m_controlData->stick3Axis1 = m_controlData->stick3Axis2 = m_controlData->stick3Axis3 = 0;
m_controlData->stick0Axis4 = m_controlData->stick0Axis5 = m_controlData->stick0Axis6 = 0;
m_controlData->stick1Axis4 = m_controlData->stick1Axis5 = m_controlData->stick1Axis6 = 0;
m_controlData->stick2Axis4 = m_controlData->stick2Axis5 = m_controlData->stick2Axis6 = 0;
m_controlData->stick3Axis4 = m_controlData->stick3Axis5 = m_controlData->stick3Axis6 = 0;
m_controlData->stick0Buttons = 0;
m_controlData->stick1Buttons = 0;
m_controlData->stick2Buttons = 0;
m_controlData->stick3Buttons = 0;
// initialize the analog and digital data.
m_controlData->analog1 = 0;
m_controlData->analog2 = 0;
m_controlData->analog3 = 0;
m_controlData->analog4 = 0;
m_controlData->dsDigitalIn = 0;
m_batteryChannel = new AnalogChannel(kBatteryModuleNumber, kBatteryChannel);
AddToSingletonList();
if (!m_task.Start((int32_t)this))
{
wpi_setWPIError(DriverStationTaskError);
}
}
DriverStation::~DriverStation()
{
m_task.Stop();
deleteMutex(m_statusDataSemaphore);
delete m_batteryChannel;
delete m_controlData;
m_instance = NULL;
deleteMultiWait(m_waitForDataSem);
// Unregister our semaphore.
HALSetNewDataSem(0);
deleteMutex(m_packetDataAvailableSem);
}
void DriverStation::InitTask(DriverStation *ds)
{
ds->Run();
}
void DriverStation::Run()
{
int period = 0;
while (true)
{
takeMutex(m_packetDataAvailableSem);
SetData();
m_enhancedIO.UpdateData();
GetData();
giveMultiWait(m_waitForDataSem);
if (++period >= 4)
{
MotorSafetyHelper::CheckMotors();
period = 0;
}
if (m_userInDisabled)
HALNetworkCommunicationObserveUserProgramDisabled();
if (m_userInAutonomous)
HALNetworkCommunicationObserveUserProgramAutonomous();
if (m_userInTeleop)
HALNetworkCommunicationObserveUserProgramTeleop();
if (m_userInTest)
HALNetworkCommunicationObserveUserProgramTest();
}
}
/**
* Return a pointer to the singleton DriverStation.
*/
DriverStation* DriverStation::GetInstance()
{
if (m_instance == NULL)
{
m_instance = new DriverStation();
}
return m_instance;
}
/**
* Copy data from the DS task for the user.
* If no new data exists, it will just be returned, otherwise
* the data will be copied from the DS polling loop.
*/
void DriverStation::GetData()
{
static bool lastEnabled = false;
HALGetCommonControlData(m_controlData, HAL_WAIT_FOREVER);
if (!lastEnabled && IsEnabled())
{
// If starting teleop, assume that autonomous just took up 15 seconds
if (IsAutonomous())
m_approxMatchTimeOffset = Timer::GetFPGATimestamp();
else
m_approxMatchTimeOffset = Timer::GetFPGATimestamp() - 15.0;
}
else if (lastEnabled && !IsEnabled())
{
m_approxMatchTimeOffset = -1.0;
}
lastEnabled = IsEnabled();
giveSemaphore(m_newControlData);
}
/**
* Copy status data from the DS task for the user.
*/
void DriverStation::SetData()
{
char *userStatusDataHigh;
int32_t userStatusDataHighSize;
char *userStatusDataLow;
int32_t userStatusDataLowSize;
Synchronized sync(m_statusDataSemaphore);
m_dashboardInUseHigh->GetStatusBuffer(&userStatusDataHigh, &userStatusDataHighSize);
m_dashboardInUseLow->GetStatusBuffer(&userStatusDataLow, &userStatusDataLowSize);
HALSetStatusData(GetBatteryVoltage(), m_digitalOut, m_updateNumber,
userStatusDataHigh, userStatusDataHighSize, userStatusDataLow, userStatusDataLowSize, HAL_WAIT_FOREVER);
m_dashboardInUseHigh->Flush();
m_dashboardInUseLow->Flush();
}
/**
* Read the battery voltage from the specified AnalogChannel.
*
* This accessor assumes that the battery voltage is being measured
* through the voltage divider on an analog breakout.
*
* @return The battery voltage.
*/
float DriverStation::GetBatteryVoltage()
{
if (m_batteryChannel == NULL)
wpi_setWPIError(NullParameter);
// The Analog bumper has a voltage divider on the battery source.
// Vbatt *--/\/\/\--* Vsample *--/\/\/\--* Gnd
// 680 Ohms 1000 Ohms
return m_batteryChannel->GetAverageVoltage() * (1680.0 / 1000.0);
}
/**
* 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 (axis < 1 || axis > kJoystickAxes)
{
wpi_setWPIError(BadJoystickAxis);
return 0.0;
}
int8_t value;
switch (stick)
{
case 1:
value = m_controlData->stick0Axes[axis-1];
break;
case 2:
value = m_controlData->stick1Axes[axis-1];
break;
case 3:
value = m_controlData->stick2Axes[axis-1];
break;
case 4:
value = m_controlData->stick3Axes[axis-1];
break;
default:
wpi_setWPIError(BadJoystickIndex);
return 0.0;
}
float result;
if (value < 0)
result = ((float) value) / 128.0;
else
result = ((float) value) / 127.0;
wpi_assert(result <= 1.0 && result >= -1.0);
if (result > 1.0)
result = 1.0;
else if (result < -1.0)
result = -1.0;
return result;
}
/**
* The state of the buttons on the joystick.
* 12 buttons (4 msb are unused) from the joystick.
*
* @param stick The joystick to read.
* @return The state of the buttons on the joystick.
*/
short DriverStation::GetStickButtons(uint32_t stick)
{
if (stick < 1 || stick > 4)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 1 and 4");
switch (stick)
{
case 1:
return m_controlData->stick0Buttons;
case 2:
return m_controlData->stick1Buttons;
case 3:
return m_controlData->stick2Buttons;
case 4:
return m_controlData->stick3Buttons;
}
return 0;
}
// 5V divided by 10 bits
#define kDSAnalogInScaling ((float)(5.0 / 1023.0))
/**
* Get an analog voltage from the Driver Station.
* The analog values are returned as voltage values for the Driver Station analog inputs.
* These inputs are typically used for advanced operator interfaces consisting of potentiometers
* or resistor networks representing values on a rotary switch.
*
* @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4.
* @return The analog voltage on the input.
*/
float DriverStation::GetAnalogIn(uint32_t channel)
{
if (channel < 1 || channel > 4)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 4");
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationCIO, channel, HALUsageReporting::kDriverStationCIO_Analog);
reported_mask |= (1 >> channel);
}
switch (channel)
{
case 1:
return kDSAnalogInScaling * m_controlData->analog1;
case 2:
return kDSAnalogInScaling * m_controlData->analog2;
case 3:
return kDSAnalogInScaling * m_controlData->analog3;
case 4:
return kDSAnalogInScaling * m_controlData->analog4;
}
return 0.0;
}
/**
* Get values from the digital inputs on the Driver Station.
* Return digital values from the Drivers Station. These values are typically used for buttons
* and switches on advanced operator interfaces.
* @param channel The digital input to get. Valid range is 1 - 8.
*/
bool DriverStation::GetDigitalIn(uint32_t channel)
{
if (channel < 1 || channel > 8)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationCIO, channel, HALUsageReporting::kDriverStationCIO_DigitalIn);
reported_mask |= (1 >> channel);
}
return ((m_controlData->dsDigitalIn >> (channel-1)) & 0x1) ? true : false;
}
/**
* Set a value for the digital outputs on the Driver Station.
*
* Control digital outputs on the Drivers Station. These values are typically used for
* giving feedback on a custom operator station such as LEDs.
*
* @param channel The digital output to set. Valid range is 1 - 8.
* @param value The state to set the digital output.
*/
void DriverStation::SetDigitalOut(uint32_t channel, bool value)
{
if (channel < 1 || channel > 8)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationCIO, channel,HALUsageReporting::kDriverStationCIO_DigitalOut);
reported_mask |= (1 >> channel);
}
m_digitalOut &= ~(0x1 << (channel-1));
m_digitalOut |= ((uint8_t)value << (channel-1));
}
/**
* Get a value that was set for the digital outputs on the Driver Station.
* @param channel The digital ouput to monitor. Valid range is 1 through 8.
* @return A digital value being output on the Drivers Station.
*/
bool DriverStation::GetDigitalOut(uint32_t channel)
{
if (channel < 1 || channel > 8)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
return ((m_digitalOut >> (channel-1)) & 0x1) ? true : false;
}
bool DriverStation::IsEnabled()
{
return m_controlData->enabled;
}
bool DriverStation::IsDisabled()
{
return !m_controlData->enabled;
}
bool DriverStation::IsAutonomous()
{
return m_controlData->autonomous;
}
bool DriverStation::IsOperatorControl()
{
return !(m_controlData->autonomous || m_controlData->test);
}
bool DriverStation::IsTest()
{
return m_controlData->test;
}
/**
* Has a new control packet from the driver station arrived since the last time this function was called?
* Warning: If you call this function from more than one place at the same time,
* you will not get the get the intended behavior
* @return True if the control data has been updated since the last call.
*/
bool DriverStation::IsNewControlData()
{
return tryTakeSemaphore(m_newControlData) == 0;
}
/**
* Is the driver station attached to a Field Management System?
* Note: This does not work with the Blue DS.
* @return True if the robot is competing on a field being controlled by a Field Management System
*/
bool DriverStation::IsFMSAttached()
{
return m_controlData->fmsAttached;
}
/**
* Return the DS packet number.
* The packet number is the index of this set of data returned by the driver station.
* Each time new data is received, the packet number (included with the sent data) is returned.
* @return The driver station packet number
*/
uint32_t DriverStation::GetPacketNumber()
{
return m_controlData->packetIndex;
}
/**
* Return the alliance that the driver station says it is on.
* This could return kRed or kBlue
* @return The Alliance enum
*/
DriverStation::Alliance DriverStation::GetAlliance()
{
if (m_controlData->dsID_Alliance == 'R') return kRed;
if (m_controlData->dsID_Alliance == 'B') return kBlue;
wpi_assert(false);
return kInvalid;
}
/**
* Return the driver station location on the field
* This could return 1, 2, or 3
* @return The location of the driver station
*/
uint32_t DriverStation::GetLocation()
{
wpi_assert ((m_controlData->dsID_Position >= '1') && (m_controlData->dsID_Position <= '3'));
return m_controlData->dsID_Position - '0';
}
/**
* Wait until a new packet comes from the driver station
* This blocks on a semaphore, so the waiting is efficient.
* This is a good way to delay processing until there is new driver station data to act on
*/
void DriverStation::WaitForData()
{
takeMultiWait(m_waitForDataSem, SEMAPHORE_WAIT_FOREVER);
}
/**
* Return the approximate match time
* The FMS does not currently send the official match time to the robots
* This returns the time since the enable signal sent from the Driver Station
* At the beginning of autonomous, the time is reset to 0.0 seconds
* At the beginning of teleop, the time is reset to +15.0 seconds
* If the robot is disabled, this returns 0.0 seconds
* Warning: This is not an official time (so it cannot be used to argue with referees)
* @return Match time in seconds since the beginning of autonomous
*/
double DriverStation::GetMatchTime()
{
if (m_approxMatchTimeOffset < 0.0)
return 0.0;
return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
}
/**
* Return the team number that the Driver Station is configured for
* @return The team number
*/
uint16_t DriverStation::GetTeamNumber()
{
return m_controlData->teamID;
}

996
wpilibc/src/main/native/DriverStationEnhancedIO.cpp
View File

@@ -1,996 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DriverStationEnhancedIO.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <string.h>
/**
* DriverStationEnhancedIO contructor.
*
* This is only called once when the DriverStation constructor is called.
*/
DriverStationEnhancedIO::DriverStationEnhancedIO()
: m_inputValid (false)
, m_outputValid (false)
, m_configChanged (false)
, m_requestEnhancedEnable (false)
{
bzero((char*)&m_inputData, sizeof(m_inputData));
bzero((char*)&m_outputData, sizeof(m_outputData));
m_outputData.size = sizeof(m_outputData) - 1;
m_outputData.id = kOutputBlockID;
// Expected to be active low, so initialize inactive.
m_outputData.data.fixed_digital_out = 0x3;
m_inputDataSemaphore = initializeMutexNormal();
m_outputDataSemaphore = initializeMutexNormal();
m_encoderOffsets[0] = 0;
m_encoderOffsets[1] = 0;
}
/**
* DriverStationEnhancedIO destructor.
*
* Called only when the DriverStation class is destroyed.
*/
DriverStationEnhancedIO::~DriverStationEnhancedIO()
{
deleteMutex(m_outputDataSemaphore);
deleteMutex(m_inputDataSemaphore);
}
/**
* Called by the DriverStation class when data is available.
* This function will set any modified configuration / output,
* then read the input and configuration from the IO.
*/
void DriverStationEnhancedIO::UpdateData()
{
int32_t retVal;
{
status_block_t tempOutputData;
Synchronized sync(m_outputDataSemaphore);
if (m_outputValid || m_configChanged || m_requestEnhancedEnable)
{
m_outputData.flags = kStatusValid;
if (m_requestEnhancedEnable)
{
// Someone called one of the get config APIs, but we are not in enhanced mode.
m_outputData.flags |= kForceEnhancedMode;
}
if (m_configChanged)
{
if (!m_outputValid)
{
// Someone called one of the set config APIs, but we are not in enhanced mode.
m_outputData.flags |= kForceEnhancedMode;
}
m_outputData.flags |= kStatusConfigChanged;
}
HALOverrideIOConfig((char*)&m_outputData, 5);
}
retVal = HALGetDynamicControlData(kOutputBlockID, (char*)&tempOutputData, sizeof(status_block_t), 5);
if (retVal == 0)
{
if (m_outputValid)
{
if (m_configChanged)
{
// If our config change made the round trip then clear the flag.
if (IsConfigEqual(tempOutputData, m_outputData))
{
m_configChanged = false;
}
}
else
{
// TODO: This won't work until artf1128 is fixed
//if (tempOutputData.flags & kStatusConfigChanged)
{
// Configuration was updated on the DS, so update our local cache.
MergeConfigIntoOutput(tempOutputData, m_outputData);
}
}
}
else
{
// Initialize the local cache.
MergeConfigIntoOutput(tempOutputData, m_outputData);
}
m_requestEnhancedEnable = false;
m_outputValid = true;
}
else
{
m_outputValid = false;
m_inputValid = false;
}
}
{
Synchronized sync(m_inputDataSemaphore);
control_block_t tempInputData;
retVal = HALGetDynamicControlData(kInputBlockID, (char*)&tempInputData, sizeof(control_block_t), 5);
if (retVal == 0 && tempInputData.data.api_version == kSupportedAPIVersion)
{
m_inputData = tempInputData;
m_inputValid = true;
}
else
{
m_outputValid = false;
m_inputValid = false;
}
}
}
/**
* Merge the config portion of the DS output block into the local cache.
*/
void DriverStationEnhancedIO::MergeConfigIntoOutput(const status_block_t &dsOutputBlock, status_block_t &localCache)
{
localCache.data.digital = (localCache.data.digital & dsOutputBlock.data.digital_oe) |
(dsOutputBlock.data.digital & ~dsOutputBlock.data.digital_oe);
localCache.data.digital_oe = dsOutputBlock.data.digital_oe;
localCache.data.digital_pe = dsOutputBlock.data.digital_pe;
localCache.data.pwm_period[0] = dsOutputBlock.data.pwm_period[0];
localCache.data.pwm_period[1] = dsOutputBlock.data.pwm_period[1];
localCache.data.enables = dsOutputBlock.data.enables;
}
/**
* Compare the config portion of the output blocks.
*/
bool DriverStationEnhancedIO::IsConfigEqual(const status_block_t &dsOutputBlock, const status_block_t &localCache)
{
if (localCache.data.digital_oe != dsOutputBlock.data.digital_oe) return false;
if ((localCache.data.digital & ~dsOutputBlock.data.digital) !=
(dsOutputBlock.data.digital & ~dsOutputBlock.data.digital)) return false;
if (localCache.data.digital_pe != dsOutputBlock.data.digital_pe) return false;
if (localCache.data.pwm_period[0] != dsOutputBlock.data.pwm_period[0]) return false;
if (localCache.data.pwm_period[1] != dsOutputBlock.data.pwm_period[1]) return false;
if (localCache.data.enables != dsOutputBlock.data.enables) return false;
return true;
}
/**
* Query an accelerometer channel on the DS IO.
*
* @param channel The channel number to read.
* @return The current acceleration on the channel in Gs.
*/
double DriverStationEnhancedIO::GetAcceleration(tAccelChannel channel)
{
if (channel < 1 || channel > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
return 0.0;
}
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0.0;
}
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_Acceleration);
reported_mask |= (1 >> channel);
}
Synchronized sync(m_inputDataSemaphore);
return (m_inputData.data.accel[channel] - kAccelOffset) / kAccelScale;
}
/**
* Query an analog input channel on the DS IO.
*
* @param channel The channel number to read. [1,8]
* @return The analog input voltage for the channel.
*/
double DriverStationEnhancedIO::GetAnalogIn(uint32_t channel)
{
// 3.3V is the analog reference voltage
return GetAnalogInRatio(channel) * kAnalogInputReference;
}
/**
* Query an analog input channel on the DS IO in ratiometric form.
*
* @param channel The channel number to read. [1,8]
* @return The analog input percentage for the channel.
*/
double DriverStationEnhancedIO::GetAnalogInRatio(uint32_t channel)
{
if (channel < 1 || channel > 8)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
return 0.0;
}
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0.0;
}
static uint16_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_AnalogIn);
reported_mask |= (1 >> channel);
}
Synchronized sync(m_inputDataSemaphore);
return m_inputData.data.analog[channel-1] / kAnalogInputResolution;
}
/**
* Query the voltage currently being output.
*
* AO1 is pin 11 on the top connector (P2).
* AO2 is pin 12 on the top connector (P2).
*
* @param channel The analog output channel on the DS IO. [1,2]
* @return The voltage being output on the channel.
*/
double DriverStationEnhancedIO::GetAnalogOut(uint32_t channel)
{
if (channel < 1 || channel > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
return 0.0;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0.0;
}
Synchronized sync(m_outputDataSemaphore);
return m_outputData.data.dac[channel-1] * kAnalogOutputReference / kAnalogOutputResolution;
}
/**
* Set the analog output voltage.
*
* AO1 is pin 11 on the top connector (P2).
* AO2 is pin 12 on the top connector (P2).
* AO1 is the reference voltage for the 2 analog comparators on DIO15 and DIO16.
*
* The output range is 0V to 4V, however due to the supply voltage don't expect more than about 3V.
* Current supply capability is only 100uA.
*
* @param channel The analog output channel on the DS IO. [1,2]
* @param value The voltage to output on the channel.
*/
void DriverStationEnhancedIO::SetAnalogOut(uint32_t channel, double value)
{
if (channel < 1 || channel > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
return;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
if (value < 0.0) value = 0.0;
if (value > kAnalogOutputReference) value = kAnalogOutputReference;
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_AnalogOut);
reported_mask |= (1 >> channel);
}
Synchronized sync(m_outputDataSemaphore);
m_outputData.data.dac[channel-1] = (uint8_t)(value / kAnalogOutputReference * kAnalogOutputResolution);
}
/**
* Get the state of a button on the IO board.
*
* Button1 is the physical button "S1".
* Button2 is pin 4 on the top connector (P2).
* Button3 is pin 6 on the top connector (P2).
* Button4 is pin 8 on the top connector (P2).
* Button5 is pin 10 on the top connector (P2).
* Button6 is pin 7 on the top connector (P2).
*
* Button2 through Button6 are Capacitive Sense buttons.
*
* @param channel The button channel to read. [1,6]
* @return The state of the selected button.
*/
bool DriverStationEnhancedIO::GetButton(uint32_t channel)
{
if (channel < 1 || channel > 6)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 6");
return false;
}
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_Button);
reported_mask |= (1 >> channel);
}
return ((GetButtons() >> (channel-1)) & 1) != 0;
}
/**
* Get the state of all the button channels.
*
* @return The state of the 6 button channels in the 6 lsb of the returned byte.
*/
uint8_t DriverStationEnhancedIO::GetButtons()
{
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0;
}
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, 0, HALUsageReporting::kDriverStationEIO_Button);
Synchronized sync(m_inputDataSemaphore);
return m_inputData.data.buttons;
}
/**
* Set the state of an LED on the IO board.
*
* @param channel The LED channel to set. [1,8]
* @param value True to turn the LED on.
*/
void DriverStationEnhancedIO::SetLED(uint32_t channel, bool value)
{
if (channel < 1 || channel > 8)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
return;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
static uint16_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_LED);
reported_mask |= (1 >> channel);
}
uint8_t leds;
Synchronized sync(m_outputDataSemaphore);
leds = m_outputData.data.leds;
leds &= ~(1 << (channel-1));
if (value) leds |= 1 << (channel-1);
m_outputData.data.leds = leds;
}
/**
* Set the state of all 8 LEDs on the IO board.
*
* @param value The state of each LED. LED1 is lsb and LED8 is msb.
*/
void DriverStationEnhancedIO::SetLEDs(uint8_t value)
{
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, 0, HALUsageReporting::kDriverStationEIO_LED);
Synchronized sync(m_outputDataSemaphore);
m_outputData.data.leds = value;
}
/**
* Get the current state of a DIO channel regardless of mode.
*
* @param channel The DIO channel to read. [1,16]
* @return The state of the selected digital line.
*/
bool DriverStationEnhancedIO::GetDigital(uint32_t channel)
{
if (channel < 1 || channel > 16)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
return false;
}
static uint32_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_DigitalIn);
reported_mask |= (1 >> channel);
}
return ((GetDigitals() >> (channel-1)) & 1) != 0;
}
/**
* Get the state of all 16 DIO lines regardless of mode.
*
* @return The state of all DIO lines. DIO1 is lsb and DIO16 is msb.
*/
uint16_t DriverStationEnhancedIO::GetDigitals()
{
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0;
}
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, 0, HALUsageReporting::kDriverStationEIO_DigitalIn);
Synchronized sync(m_inputDataSemaphore);
return m_inputData.data.digital;
}
/**
* Set the state of a DIO line that is configured for digital output.
*
* @param channel The DIO channel to set. [1,16]
* @param value The state to set the selected channel to.
*/
void DriverStationEnhancedIO::SetDigitalOutput(uint32_t channel, bool value)
{
if (channel < 1 || channel > 16)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
return;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
static uint32_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_DigitalOut);
reported_mask |= (1 >> channel);
}
uint16_t digital;
Synchronized sync(m_outputDataSemaphore);
if (m_outputData.data.digital_oe & (1 << (channel-1)))
{
digital = m_outputData.data.digital;
digital &= ~(1 << (channel-1));
if (value) digital |= 1 << (channel-1);
m_outputData.data.digital = digital;
}
else
{
wpi_setWPIError(LineNotOutput);
}
}
/**
* Get the current configuration for a DIO line.
*
* This has the side effect of forcing the Driver Station to switch to Enhanced mode if it's not when called.
* If Enhanced mode is not enabled when this is called, it will return kUnknown.
*
* @param channel The DIO channel config to get. [1,16]
* @return The configured mode for the DIO line.
*/
DriverStationEnhancedIO::tDigitalConfig DriverStationEnhancedIO::GetDigitalConfig(uint32_t channel)
{
if (channel < 1 || channel > 16)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
return kUnknown;
}
if (!m_outputValid)
{
m_requestEnhancedEnable = true;
wpi_setWPIError(EnhancedIOMissing);
return kUnknown;
}
Synchronized sync(m_outputDataSemaphore);
if ((channel >= 1) && (channel <= 4))
{
if (m_outputData.data.pwm_enable & (1 << (channel - 1)))
{
return kPWM;
}
}
if ((channel >= 15) && (channel <= 16))
{
if (m_outputData.data.comparator_enable & (1 << (channel - 15)))
{
return kAnalogComparator;
}
}
if (m_outputData.data.digital_oe & (1 << (channel - 1)))
{
return kOutput;
}
if (!(m_outputData.data.digital_pe & (1 << (channel - 1))))
{
return kInputFloating;
}
if (m_outputData.data.digital & (1 << (channel - 1)))
{
return kInputPullUp;
}
else
{
return kInputPullDown;
}
}
/**
* Override the DS's configuration of a DIO line.
*
* If configured to kInputFloating, the selected DIO line will be tri-stated with no internal pull resistor.
*
* If configured to kInputPullUp, the selected DIO line will be tri-stated with a 5k-Ohm internal pull-up resistor enabled.
*
* If configured to kInputPullDown, the selected DIO line will be tri-stated with a 5k-Ohm internal pull-down resistor enabled.
*
* If configured to kOutput, the selected DIO line will actively drive to 0V or Vddio (specified by J1 and J4).
* DIO1 through DIO12, DIO15, and DIO16 can source 4mA and can sink 8mA.
* DIO12 and DIO13 can source 4mA and can sink 25mA.
*
* In addition to the common configurations, DIO1 through DIO4 can be configured to kPWM to enable PWM output.
*
* In addition to the common configurations, DIO15 and DIO16 can be configured to kAnalogComparator to enable
* analog comparators on those 2 DIO lines. When enabled, the lines are tri-stated and will accept analog voltages
* between 0V and 3.3V. If the input voltage is greater than the voltage output by AO1, the DIO will read as true,
* if less then false.
*
* @param channel The DIO line to configure. [1,16]
* @param config The mode to put the DIO line in.
*/
void DriverStationEnhancedIO::SetDigitalConfig(uint32_t channel, tDigitalConfig config)
{
if (channel < 1 || channel > 16)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
return;
}
if (config == kPWM && (channel < 1 || channel > 4))
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel in PWM mode must be between 1 and 4");
return;
}
if (config == kAnalogComparator && (channel < 15 || channel > 16))
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel in Analog Comparator mode must be between 15 and 16");
return;
}
Synchronized sync(m_outputDataSemaphore);
m_configChanged = true;
if ((channel >= 1) && (channel <= 4))
{
if (config == kPWM)
{
m_outputData.data.pwm_enable |= 1 << (channel - 1);
m_outputData.data.digital &= ~(1 << (channel - 1));
m_outputData.data.digital_oe |= 1 << (channel - 1);
m_outputData.data.digital_pe &= ~(1 << (channel - 1));
return;
}
else
{
m_outputData.data.pwm_enable &= ~(1 << (channel - 1));
}
}
else if ((channel >= 15) && (channel <= 16))
{
if (config == kAnalogComparator)
{
m_outputData.data.comparator_enable |= 1 << (channel - 15);
m_outputData.data.digital &= ~(1 << (channel - 1));
m_outputData.data.digital_oe &= ~(1 << (channel - 1));
m_outputData.data.digital_pe &= ~(1 << (channel - 1));
return;
}
else
{
m_outputData.data.comparator_enable &= ~(1 << (channel - 15));
}
}
if (config == kInputFloating)
{
m_outputData.data.digital &= ~(1 << (channel - 1));
m_outputData.data.digital_oe &= ~(1 << (channel - 1));
m_outputData.data.digital_pe &= ~(1 << (channel - 1));
}
else if (config == kInputPullUp)
{
m_outputData.data.digital |= 1 << (channel - 1);
m_outputData.data.digital_oe &= ~(1 << (channel - 1));
m_outputData.data.digital_pe |= 1 << (channel - 1);
}
else if (config == kInputPullDown)
{
m_outputData.data.digital &= ~(1 << (channel - 1));
m_outputData.data.digital_oe &= ~(1 << (channel - 1));
m_outputData.data.digital_pe |= 1 << (channel - 1);
}
else if (config == kOutput)
{
m_outputData.data.digital_oe |= 1 << (channel - 1);
m_outputData.data.digital_pe &= ~(1 << (channel - 1));
}
else
{
// Something went wrong.
}
}
/**
* Get the period of a PWM generator.
*
* This has the side effect of forcing the Driver Station to switch to Enhanced mode if it's not when called.
* If Enhanced mode is not enabled when this is called, it will return 0.
*
* @param channels Select the generator by specifying the two channels to which it is connected.
* @return The period of the PWM generator in seconds.
*/
double DriverStationEnhancedIO::GetPWMPeriod(tPWMPeriodChannels channels)
{
if (channels < kPWMChannels1and2 || channels > kPWMChannels3and4)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channels must be kPWMChannels1and2 or kPWMChannels3and4");
return 0.0;
}
if (!m_outputValid)
{
m_requestEnhancedEnable = true;
wpi_setWPIError(EnhancedIOMissing);
return 0.0;
}
Synchronized sync(m_outputDataSemaphore);
return m_outputData.data.pwm_period[channels] / 24000000.0;
}
/**
* Set the period of a PWM generator.
*
* There are 2 PWM generators on the IO board. One can generate PWM signals on DIO1 and DIO2,
* the other on DIO3 and DIO4. Each generator has one counter and two compare registers. As such,
* each pair of PWM outputs share the output period but have independent duty cycles.
*
* @param channels Select the generator by specifying the two channels to which it is connected.
* @param period The period of the PWM generator in seconds. [0.0,0.002731]
*/
void DriverStationEnhancedIO::SetPWMPeriod(tPWMPeriodChannels channels, double period)
{
if (channels < kPWMChannels1and2 || channels > kPWMChannels3and4)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channels must be kPWMChannels1and2 or kPWMChannels3and4");
return;
}
// Convert to ticks based on the IO board's 24MHz clock
double ticks = period * 24000000.0;
// Limit the range of the ticks... warn if too big.
if (ticks > 65534.0)
{
wpi_setWPIError(EnhancedIOPWMPeriodOutOfRange);
ticks = 65534.0;
}
else if (ticks < 0.0) ticks = 0.0;
// Preserve the duty cycles.
double dutyCycles[2];
dutyCycles[0] = GetPWMOutput((channels << 1) + 1);
dutyCycles[1] = GetPWMOutput((channels << 1) + 2);
{
Synchronized sync(m_outputDataSemaphore);
// Update the period
m_outputData.data.pwm_period[channels] = (uint16_t)ticks;
m_configChanged = true;
}
// Restore the duty cycles
SetPWMOutput((channels << 1) + 1, dutyCycles[0]);
SetPWMOutput((channels << 1) + 2, dutyCycles[1]);
}
/**
* Get the state being output on a fixed digital output.
*
* @param channel The FixedDO line to get. [1,2]
* @return The state of the FixedDO line.
*/
bool DriverStationEnhancedIO::GetFixedDigitalOutput(uint32_t channel)
{
if (channel < 1 || channel > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
return 0;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0;
}
Synchronized sync(m_outputDataSemaphore);
return ((m_outputData.data.fixed_digital_out >> (channel-1)) & 1) != 0;
}
/**
* Set the state to output on a Fixed High Current Digital Output line.
*
* FixedDO1 is pin 5 on the top connector (P2).
* FixedDO2 is pin 3 on the top connector (P2).
*
* The FixedDO lines always output 0V and 3.3V regardless of J1 and J4.
* They can source 4mA and can sink 25mA. Because of this, they are expected to be used
* in an active low configuration, such as connecting to the cathode of a bright LED.
* Because they are expected to be active low, they default to true.
*
* @param channel The FixedDO channel to set.
* @param value The state to set the FixedDO.
*/
void DriverStationEnhancedIO::SetFixedDigitalOutput(uint32_t channel, bool value)
{
if (channel < 1 || channel > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
return;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_FixedDigitalOut);
reported_mask |= (1 >> channel);
}
uint8_t digital;
Synchronized sync(m_outputDataSemaphore);
digital = m_outputData.data.fixed_digital_out;
digital &= ~(1 << (channel-1));
if (value) digital |= 1 << (channel-1);
m_outputData.data.fixed_digital_out = digital;
}
/**
* Get the position of a quadrature encoder.
*
* There are two signed 16-bit 4X quadrature decoders on the IO board. These decoders are always monitoring
* the state of the lines assigned to them, but these lines do not have to be used for encoders.
*
* Encoder1 uses DIO4 for "A", DIO6 for "B", and DIO8 for "Index".
* Encoder2 uses DIO5 for "A", DIO7 for "B", and DIO9 for "Index".
*
* The index functionality can be enabled or disabled using SetEncoderIndexEnable().
*
* @param encoderNumber The quadrature encoder to access. [1,2]
* @return The current position of the quadrature encoder.
*/
int16_t DriverStationEnhancedIO::GetEncoder(uint32_t encoderNumber)
{
if (encoderNumber < 1 || encoderNumber > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
return 0;
}
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0;
}
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> encoderNumber)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, encoderNumber, HALUsageReporting::kDriverStationEIO_Encoder);
reported_mask |= (1 >> encoderNumber);
}
Synchronized sync(m_inputDataSemaphore);
return m_inputData.data.quad[encoderNumber - 1] - m_encoderOffsets[encoderNumber - 1];
}
/**
* Reset the position of an encoder to 0.
*
* This simply stores an offset locally. It does not reset the hardware counter on the IO board.
* If you use this method with Index enabled, you may get unexpected results.
*
* @param encoderNumber The quadrature encoder to reset. [1,2]
*/
void DriverStationEnhancedIO::ResetEncoder(uint32_t encoderNumber)
{
if (encoderNumber < 1 || encoderNumber > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
return;
}
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
Synchronized sync(m_inputDataSemaphore);
m_encoderOffsets[encoderNumber - 1] = m_inputData.data.quad[encoderNumber - 1];
}
/**
* Get the current configuration of a quadrature encoder index channel.
*
* This has the side effect of forcing the Driver Station to switch to Enhanced mode if it's not when called.
* If Enhanced mode is not enabled when this is called, it will return false.
*
* @param encoderNumber The quadrature encoder. [1,2]
* @return Is the index channel of the encoder enabled.
*/
bool DriverStationEnhancedIO::GetEncoderIndexEnable(uint32_t encoderNumber)
{
if (encoderNumber < 1 || encoderNumber > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
return false;
}
if (!m_outputValid)
{
m_requestEnhancedEnable = true;
wpi_setWPIError(EnhancedIOMissing);
return false;
}
Synchronized sync(m_outputDataSemaphore);
return ((m_outputData.data.quad_index_enable >> (encoderNumber - 1)) & 1) != 0;
}
/**
* Enable or disable the index channel of a quadrature encoder.
*
* The quadrature decoders on the IO board support an active-low index input.
*
* Encoder1 uses DIO8 for "Index".
* Encoder2 uses DIO9 for "Index".
*
* When enabled, the decoder's counter will be reset to 0 when A, B, and Index are all low.
*
* @param encoderNumber The quadrature encoder. [1,2]
* @param enable If true, reset the encoder in an index condition.
*/
void DriverStationEnhancedIO::SetEncoderIndexEnable(uint32_t encoderNumber, bool enable)
{
if (encoderNumber < 1 || encoderNumber > 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
return;
}
Synchronized sync(m_outputDataSemaphore);
m_outputData.data.quad_index_enable &= ~(1 << (encoderNumber - 1));
if (enable) m_outputData.data.quad_index_enable |= 1 << (encoderNumber - 1);
m_configChanged = true;
}
/**
* Get the value of the Capacitive Sense touch slider.
*
* @return Value between 0.0 (toward center of board) and 1.0 (toward edge of board). -1.0 means no touch detected.
*/
double DriverStationEnhancedIO::GetTouchSlider()
{
if (!m_inputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0.0;
}
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, 1, HALUsageReporting::kDriverStationEIO_TouchSlider);
Synchronized sync(m_inputDataSemaphore);
uint8_t value = m_inputData.data.capsense_slider;
return value == 255 ? -1.0 : value / 254.0;
}
/**
* Get the percent duty-cycle that the PWM generator channel is configured to output.
*
* @param channel The DIO line's PWM generator to get the duty-cycle from. [1,4]
* @return The percent duty-cycle being output (if the DIO line is configured for PWM). [0.0,1.0]
*/
double DriverStationEnhancedIO::GetPWMOutput(uint32_t channel)
{
if (channel < 1 || channel > 4)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 4");
return 0.0;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return 0.0;
}
Synchronized sync(m_outputDataSemaphore);
return (double)m_outputData.data.pwm_compare[channel - 1] / (double)m_outputData.data.pwm_period[(channel - 1) >> 1];
}
/**
* Set the percent duty-cycle to output on a PWM enabled DIO line.
*
* DIO1 through DIO4 have the ability to output a PWM signal. The period of the
* signal can be configured in pairs using SetPWMPeriod().
*
* @param channel The DIO line's PWM generator to set. [1,4]
* @param value The percent duty-cycle to output from the PWM generator. [0.0,1.0]
*/
void DriverStationEnhancedIO::SetPWMOutput(uint32_t channel, double value)
{
if (channel < 1 || channel > 4)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 4");
return;
}
if (!m_outputValid)
{
wpi_setWPIError(EnhancedIOMissing);
return;
}
static uint8_t reported_mask = 0;
if (!(reported_mask & (1 >> channel)))
{
HALReport(HALUsageReporting::kResourceType_DriverStationEIO, channel, HALUsageReporting::kDriverStationEIO_PWM);
reported_mask |= (1 >> channel);
}
if (value > 1.0) value = 1.0;
else if (value < 0.0) value = 0.0;
Synchronized sync(m_outputDataSemaphore);
m_outputData.data.pwm_compare[channel - 1] = (uint16_t)(value * (double)m_outputData.data.pwm_period[(channel - 1) >> 1]);
}
/**
* Get the firmware version running on the IO board.
*
* This also has the side effect of forcing the driver station to switch to Enhanced mode if it is not.
* If you plan to switch between Driver Stations with unknown IO configurations, you can call this
* until it returns a non-0 version to ensure that this API is accessible before proceeding.
*
* @return The version of the firmware running on the IO board. 0 if the board is not attached or not in Enhanced mode.
*/
uint8_t DriverStationEnhancedIO::GetFirmwareVersion()
{
if (!m_inputValid)
{
m_requestEnhancedEnable = true;
wpi_setWPIError(EnhancedIOMissing);
return 0;
}
Synchronized sync(m_inputDataSemaphore);
return m_inputData.data.fw_version;
}

168
wpilibc/src/main/native/DriverStationLCD.cpp
View File

@@ -1,168 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "DriverStationLCD.h"
//#include <algorithm>
#include <string.h>
//#include "NetworkCommunication/FRCComm.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#undef min
const uint32_t DriverStationLCD::kSyncTimeout_ms;
const uint16_t DriverStationLCD::kFullDisplayTextCommand;
const int32_t DriverStationLCD::kLineLength;
const int32_t DriverStationLCD::kNumLines;
DriverStationLCD* DriverStationLCD::m_instance = NULL;
/**
* DriverStationLCD contructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStationLCD::DriverStationLCD()
: m_textBuffer (NULL)
, m_textBufferSemaphore (NULL)
{
m_textBuffer = new char[HAL_USER_DS_LCD_DATA_SIZE];
memset(m_textBuffer, ' ', HAL_USER_DS_LCD_DATA_SIZE);
*((uint16_t *)m_textBuffer) = kFullDisplayTextCommand;
m_textBufferSemaphore = initializeMutexNormal();
HALReport(HALUsageReporting::kResourceType_DriverStationLCD, 0);
AddToSingletonList();
}
DriverStationLCD::~DriverStationLCD()
{
deleteMutex(m_textBufferSemaphore);
delete [] m_textBuffer;
m_instance = NULL;
}
/**
* Return a pointer to the singleton DriverStationLCD.
*/
DriverStationLCD* DriverStationLCD::GetInstance()
{
if (m_instance == NULL)
{
m_instance = new DriverStationLCD();
}
return m_instance;
}
/**
* Send the text data to the Driver Station.
*/
void DriverStationLCD::UpdateLCD()
{
Synchronized sync(m_textBufferSemaphore);
HALSetUserDsLcdData(m_textBuffer, HAL_USER_DS_LCD_DATA_SIZE, kSyncTimeout_ms);
}
/**
* Print formatted text to the Driver Station LCD text bufer.
*
* Use UpdateLCD() periodically to actually send the text 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_t startingColumn, const char *writeFmt, ...)
{
va_list args;
va_start (args, writeFmt);
VPrintf(line, startingColumn, writeFmt, args);
va_end (args);
}
void DriverStationLCD::VPrintf(Line line, int32_t startingColumn, const char *writeFmt, va_list args)
{
uint32_t start = startingColumn - 1;
int32_t 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;
}
{
Synchronized sync(m_textBufferSemaphore);
// snprintf appends NULL to its output. Therefore we can't write directly to the buffer.
int32_t length = vsnprintf(lineBuffer, kLineLength + 1, writeFmt, args);
if (length < 0) length = kLineLength;
memcpy(m_textBuffer + start + line * kLineLength + sizeof(uint16_t), lineBuffer, std::min(maxLength,length));
}
}
/**
* 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 text 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;
va_start (args, writeFmt);
VPrintfLine(line, writeFmt, args);
va_end (args);
}
void DriverStationLCD::VPrintfLine(Line line, const char *writeFmt, va_list args)
{
char lineBuffer[kLineLength + 1];
if (line < kMain_Line6 || line > kUser_Line6)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "line");
return;
}
{
Synchronized sync(m_textBufferSemaphore);
// snprintf appends NULL to its output. Therefore we can't write directly to the buffer.
int32_t length = std::min(vsnprintf(lineBuffer, kLineLength + 1, writeFmt, args), (int)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_t), lineBuffer, kLineLength);
}
}
/**
* Clear all lines on the LCD.
*/
void DriverStationLCD::Clear()
{
Synchronized sync(m_textBufferSemaphore);
memset(m_textBuffer + sizeof(uint16_t), ' ', kLineLength*kNumLines);
}

579
wpilibc/src/main/native/Encoder.cpp
View File

@@ -1,579 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Encoder.h"
#include "DigitalInput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
/**
* Common initialization code for Encoders.
* This code allocates resources for Encoders and is common to all constructors.
* @param reverseDirection If true, counts down instead of up (this is all relative)
* @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.
*/
void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType)
{
m_table = NULL;
m_encodingType = encodingType;
int32_t index = 0;
switch (encodingType)
{
case k4X:
{
if (m_aSource->StatusIsFatal())
{
CloneError(m_aSource);
return;
}
if (m_bSource->StatusIsFatal())
{
CloneError(m_bSource);
return;
}
int32_t status = 0;
int32_t index = 0;
m_encoder = initializeEncoder(m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(),
m_aSource->GetAnalogTriggerForRouting(),
m_bSource->GetModuleForRouting(), m_bSource->GetChannelForRouting(),
m_bSource->GetAnalogTriggerForRouting(),
reverseDirection, &index, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_counter = NULL;
break;
}
case k1X:
case k2X:
{
m_counter = new Counter(m_encodingType, m_aSource, m_bSource, reverseDirection);
index = m_counter->GetIndex();
break;
}
}
m_distancePerPulse = 1.0;
m_pidSource = kDistance;
HALReport(HALUsageReporting::kResourceType_Encoder, index, encodingType);
LiveWindow::GetInstance()->AddSensor("Encoder", m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(), this);
}
/**
* Encoder constructor.
* Construct a Encoder given a and b modules and channels fully specified.
* @param aModuleNumber The a channel digital input module.
* @param aChannel The a channel digital input channel.
* @param bModuleNumber The b channel digital input module.
* @param bChannel The b channel digital input 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(uint8_t aModuleNumber, uint32_t aChannel,
uint8_t bModuleNumber, uint32_t bChannel,
bool reverseDirection, EncodingType encodingType) :
m_encoder(NULL),
m_counter(NULL)
{
m_aSource = new DigitalInput(aModuleNumber, aChannel);
m_bSource = new DigitalInput(bModuleNumber, bChannel);
InitEncoder(reverseDirection, encodingType);
m_allocatedASource = true;
m_allocatedBSource = true;
}
/**
* Encoder constructor.
* Construct a Encoder given a and b channels assuming the default module.
* @param aChannel The a channel digital input channel.
* @param bChannel The b channel digital input 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(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, EncodingType encodingType) :
m_encoder(NULL),
m_counter(NULL)
{
m_aSource = new DigitalInput(aChannel);
m_bSource = new DigitalInput(bChannel);
InitEncoder(reverseDirection, encodingType);
m_allocatedASource = true;
m_allocatedBSource = true;
}
/**
* 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.
* @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_encoder(NULL),
m_counter(NULL)
{
m_aSource = aSource;
m_bSource = bSource;
m_allocatedASource = false;
m_allocatedBSource = false;
if (m_aSource == NULL || m_bSource == NULL)
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.
* @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_encoder(NULL),
m_counter(NULL)
{
m_aSource = &aSource;
m_bSource = &bSource;
m_allocatedASource = false;
m_allocatedBSource = false;
InitEncoder(reverseDirection, encodingType);
}
/**
* Free the resources for an Encoder.
* Frees the FPGA resources associated with an Encoder.
*/
Encoder::~Encoder()
{
if (m_allocatedASource) delete m_aSource;
if (m_allocatedBSource) delete m_bSource;
if (m_counter)
{
delete m_counter;
}
else
{
int32_t status = 0;
freeEncoder(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Start the Encoder.
* Starts counting pulses on the Encoder device.
*/
void Encoder::Start()
{
if (StatusIsFatal()) return;
if (m_counter)
m_counter->Start();
else
{
int32_t status = 0;
startEncoder(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Stops counting pulses on the Encoder device. The value is not changed.
*/
void Encoder::Stop()
{
if (StatusIsFatal()) return;
if (m_counter)
m_counter->Stop();
else
{
int32_t status = 0;
stopEncoder(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Gets the raw value from the encoder.
* The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
* factor.
* @return Current raw count from the encoder
*/
int32_t Encoder::GetRaw()
{
if (StatusIsFatal()) return 0;
int32_t value;
if (m_counter)
value = m_counter->Get();
else
{
int32_t status = 0;
value = getEncoder(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
return value;
}
/**
* Gets the current count.
* Returns the current count on the Encoder.
* This method compensates for the decoding type.
*
* @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
*/
int32_t Encoder::Get()
{
if (StatusIsFatal()) return 0;
return (int32_t) (GetRaw() * DecodingScaleFactor());
}
/**
* Reset the Encoder distance to zero.
* Resets the current count to zero on the encoder.
*/
void Encoder::Reset()
{
if (StatusIsFatal()) return;
if (m_counter)
m_counter->Reset();
else
{
int32_t status = 0;
resetEncoder(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Returns the period of the most recent pulse.
* Returns the period of the most recent Encoder pulse in seconds.
* This method compenstates for the decoding type.
*
* @deprecated Use GetRate() in favor of this method. This returns unscaled periods and GetRate() scales using value from SetDistancePerPulse().
*
* @return Period in seconds of the most recent pulse.
*/
double Encoder::GetPeriod()
{
if (StatusIsFatal()) return 0.0;
if (m_counter)
{
return m_counter->GetPeriod() / DecodingScaleFactor();
}
else
{
int32_t status = 0;
double period = getEncoderPeriod(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return period;
}
}
/**
* Sets the maximum period for stopped detection.
* Sets the value that represents the maximum period of the Encoder before it will assume
* that the attached device is stopped. This timeout allows users to determine if the wheels or
* other shaft has stopped rotating.
* This method compensates for the decoding type.
*
* @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse().
*
* @param maxPeriod The maximum time between rising and falling edges before the FPGA will
* report the device stopped. This is expressed in seconds.
*/
void Encoder::SetMaxPeriod(double maxPeriod)
{
if (StatusIsFatal()) return;
if (m_counter)
{
m_counter->SetMaxPeriod(maxPeriod * DecodingScaleFactor());
}
else
{
int32_t status = 0;
setEncoderMaxPeriod(m_encoder, maxPeriod, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Determine if the encoder is stopped.
* Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered
* stopped and false if it is still moving. A stopped encoder is one where the most recent pulse
* width exceeds the MaxPeriod.
* @return True if the encoder is considered stopped.
*/
bool Encoder::GetStopped()
{
if (StatusIsFatal()) return true;
if (m_counter)
{
return m_counter->GetStopped();
}
else
{
int32_t status = 0;
bool value = getEncoderStopped(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
}
/**
* The last direction the encoder value changed.
* @return The last direction the encoder value changed.
*/
bool Encoder::GetDirection()
{
if (StatusIsFatal()) return false;
if (m_counter)
{
return m_counter->GetDirection();
}
else
{
int32_t status = 0;
bool value = getEncoderDirection(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
}
/**
* The scale needed to convert a raw counter value into a number of encoder pulses.
*/
double Encoder::DecodingScaleFactor()
{
if (StatusIsFatal()) return 0.0;
switch (m_encodingType)
{
case k1X:
return 1.0;
case k2X:
return 0.5;
case k4X:
return 0.25;
default:
return 0.0;
}
}
/**
* Get the distance the robot has driven since the last reset.
*
* @return The distance driven since the last reset as scaled by the value from SetDistancePerPulse().
*/
double Encoder::GetDistance()
{
if (StatusIsFatal()) return 0.0;
return GetRaw() * DecodingScaleFactor() * m_distancePerPulse;
}
/**
* Get the current rate of the encoder.
* Units are distance per second as scaled by the value from SetDistancePerPulse().
*
* @return The current rate of the encoder.
*/
double Encoder::GetRate()
{
if (StatusIsFatal()) return 0.0;
return (m_distancePerPulse / GetPeriod());
}
/**
* Set the minimum rate of the device before the hardware reports it stopped.
*
* @param minRate The minimum rate. The units are in distance per second as scaled by the value from SetDistancePerPulse().
*/
void Encoder::SetMinRate(double minRate)
{
if (StatusIsFatal()) return;
SetMaxPeriod(m_distancePerPulse / minRate);
}
/**
* Set the distance per pulse for this encoder.
* This sets the multiplier used to determine the distance driven based on the count value
* from the encoder.
* Do not include the decoding type in this scale. The library already compensates for the decoding type.
* Set this value based on the encoder's rated Pulses per Revolution and
* factor in gearing reductions following the encoder shaft.
* This distance can be in any units you like, linear or angular.
*
* @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
*/
void Encoder::SetDistancePerPulse(double distancePerPulse)
{
if (StatusIsFatal()) return;
m_distancePerPulse = distancePerPulse;
}
/**
* Set the direction sensing for this encoder.
* This sets the direction sensing on the encoder so that it could count in the correct
* software direction regardless of the mounting.
* @param reverseDirection true if the encoder direction should be reversed
*/
void Encoder::SetReverseDirection(bool reverseDirection)
{
if (StatusIsFatal()) return;
if (m_counter)
{
m_counter->SetReverseDirection(reverseDirection);
}
else
{
int32_t status = 0;
setEncoderReverseDirection(m_encoder, reverseDirection, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* 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 Encoder::SetSamplesToAverage(int samplesToAverage)
{
if (samplesToAverage < 1 || samplesToAverage > 127)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Average counter values must be between 1 and 127");
}
int32_t status = 0;
switch (m_encodingType) {
case k4X:
setEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
break;
case k1X:
case k2X:
m_counter->SetSamplesToAverage(samplesToAverage);
break;
}
}
/**
* Get 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.
* @return SamplesToAverage The number of samples being averaged (from 1 to 127)
*/
int Encoder::GetSamplesToAverage()
{
int result = 1;
int32_t status = 0;
switch (m_encodingType) {
case k4X:
result = getEncoderSamplesToAverage(m_encoder, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
break;
case k1X:
case k2X:
result = m_counter->GetSamplesToAverage();
break;
}
return result;
}
/**
* Set which parameter of the encoder you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource)
{
if (StatusIsFatal()) return;
m_pidSource = pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
double Encoder::PIDGet()
{
if (StatusIsFatal()) return 0.0;
switch (m_pidSource)
{
case kDistance:
return GetDistance();
case kRate:
return GetRate();
default:
return 0.0;
}
}
void Encoder::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Speed", GetRate());
m_table->PutNumber("Distance", GetDistance());
m_table->PutNumber("Distance per Tick", m_distancePerPulse);
}
}
void Encoder::StartLiveWindowMode() {
}
void Encoder::StopLiveWindowMode() {
}
std::string Encoder::GetSmartDashboardType() {
if (m_encodingType == k4X)
return "Quadrature Encoder";
else
return "Encoder";
}
void Encoder::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Encoder::GetTable() {
return m_table;
}

123
wpilibc/src/main/native/Error.cpp
View File

@@ -1,123 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Error.h"
#include "HAL/cpp/StackTrace.h"
#include <cstdio>
#include <cstring>
//#include "NetworkCommunication/FRCComm.h"
#include "Timer.h"
#include "Utility.h"
bool Error::m_stackTraceEnabled = false;
bool Error::m_suspendOnErrorEnabled = false;
Error::Error()
: m_code(0)
, m_lineNumber(0)
, m_originatingObject(NULL)
, m_timestamp (0.0)
{}
Error::~Error()
{}
void Error::Clone(Error &error)
{
m_code = error.m_code;
m_message = error.m_message;
m_filename = error.m_filename;
m_function = error.m_function;
m_lineNumber = error.m_lineNumber;
m_originatingObject = error.m_originatingObject;
m_timestamp = error.m_timestamp;
}
Error::Code Error::GetCode() const
{ return m_code; }
const char * Error::GetMessage() const
{ return m_message.c_str(); }
const char * Error::GetFilename() const
{ return m_filename.c_str(); }
const char * Error::GetFunction() const
{ return m_function.c_str(); }
uint32_t Error::GetLineNumber() const
{ return m_lineNumber; }
const ErrorBase* Error::GetOriginatingObject() const
{ return m_originatingObject; }
double Error::GetTime() const
{ return m_timestamp; }
void Error::Set(Code code, const char* contextMessage, const char* filename, const char* function, uint32_t lineNumber, const ErrorBase* originatingObject)
{
m_code = code;
m_message = contextMessage;
m_filename = filename;
m_function = function;
m_lineNumber = lineNumber;
m_originatingObject = originatingObject;
m_timestamp = GetTime();
Report();
if (m_suspendOnErrorEnabled) suspendTask(0);
}
void Error::Report()
{
// Error string buffers
char *error = new char[256];
char *error_with_code = new char[256];
// Build error strings
if (m_code != -1)
{
snprintf(error, 256, "%s: status = %d (0x%08X) %s ...in %s() in %s at line %d\n",
m_code < 0 ? "ERROR" : "WARNING", (int32_t)m_code, (uint32_t)m_code, m_message.c_str(),
m_function.c_str(), m_filename.c_str(), m_lineNumber);
sprintf(error_with_code,"<Code>%ld %s", (int32_t)m_code, error);
} else {
snprintf(error, 256, "ERROR: %s ...in %s() in %s at line %d\n", m_message.c_str(),
m_function.c_str(), m_filename.c_str(), m_lineNumber);
strcpy(error_with_code, error);
}
// TODO: Add logging to disk
// Send to the DriverStation
HALSetErrorData(error_with_code, strlen(error_with_code), 100);
delete [] error_with_code;
// Print to console
printf("\n\n>>>>%s", error);
delete [] error;
if (m_stackTraceEnabled)
{
printf("-----------<Stack Trace>----------------\n");
printCurrentStackTrace();
}
}
void Error::Clear()
{
m_code = 0;
m_message = "";
m_filename = "";
m_function = "";
m_lineNumber = 0;
m_originatingObject = NULL;
m_timestamp = 0.0;
}

212
wpilibc/src/main/native/ErrorBase.cpp
View File

@@ -1,212 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "ErrorBase.h"
#include "HAL/cpp/Synchronized.h"
#include "HAL/cpp/StackTrace.h"
#include "nivision.h"
#define WPI_ERRORS_DEFINE_STRINGS
#include "WPIErrors.h"
#include <errno.h>
#include <cstdio>
MUTEX_ID ErrorBase::_globalErrorMutex = initializeMutexNormal();
Error ErrorBase::_globalError;
/**
* @brief Initialize the instance status to 0 for now.
*/
ErrorBase::ErrorBase()
{}
ErrorBase::~ErrorBase()
{}
/**
* @brief Retrieve the current error.
* Get the current error information associated with this sensor.
*/
Error& ErrorBase::GetError()
{
return m_error;
}
const Error& ErrorBase::GetError() const
{
return m_error;
}
/**
* @brief Clear the current error information associated with this sensor.
*/
void ErrorBase::ClearError() const
{
m_error.Clear();
}
/**
* @brief Set error information associated with a C library call that set an error to the "errno" global variable.
*
* @param contextMessage A custom message from the code that set the error.
* @param filename Filename of the error source
* @param function Function of the error source
* @param lineNumber Line number of the error source
*/
void ErrorBase::SetErrnoError(const char *contextMessage,
const char* filename, const char* function, uint32_t lineNumber) const
{
char err[256];
int errNo = errno;
if (errNo == 0)
{
sprintf(err, "OK: %s", contextMessage);
}
else
{
char *statName = new char[256 + 1]; // TODO: MAX_SYS_SYM_LEN or linux equivalent.
if (getErrnoToName(errNo, statName))
snprintf(err, 256, "%s (0x%08X): %s", statName, errNo, contextMessage);
else
snprintf(err, 256, "Unknown errno 0x%08X: %s", errNo, contextMessage);
delete [] statName;
}
// Set the current error information for this object.
m_error.Set(-1, err, filename, function, lineNumber, this);
// Update the global error if there is not one already set.
Synchronized mutex(_globalErrorMutex);
if (_globalError.GetCode() == 0) {
_globalError.Clone(m_error);
}
}
/**
* @brief Set the current error information associated from the nivision Imaq API.
*
* @param success The return from the function
* @param contextMessage A custom message from the code that set the error.
* @param filename Filename of the error source
* @param function Function of the error source
* @param lineNumber Line number of the error source
*/
void ErrorBase::SetImaqError(int success, const char *contextMessage, const char* filename, const char* function, uint32_t lineNumber) const
{
// If there was an error
if (success <= 0) {
char err[256];
// XXX: sprintf(err, "%s: %s", contextMessage, imaqGetErrorText(imaqGetLastError()));
// Set the current error information for this object.
// XXX: m_error.Set(imaqGetLastError(), err, filename, function, lineNumber, this);
// Update the global error if there is not one already set.
Synchronized mutex(_globalErrorMutex);
if (_globalError.GetCode() == 0) {
_globalError.Clone(m_error);
}
}
}
/**
* @brief Set the current error information associated with this sensor.
*
* @param code The error code
* @param contextMessage A custom message from the code that set the error.
* @param filename Filename of the error source
* @param function Function of the error source
* @param lineNumber Line number of the error source
*/
void ErrorBase::SetError(Error::Code code, const char *contextMessage,
const char* filename, const char* function, uint32_t lineNumber) const
{
// If there was an error
if (code != 0) {
// Set the current error information for this object.
m_error.Set(code, contextMessage, filename, function, lineNumber, this);
// Update the global error if there is not one already set.
Synchronized mutex(_globalErrorMutex);
if (_globalError.GetCode() == 0) {
_globalError.Clone(m_error);
}
}
}
/**
* @brief Set the current error information associated with this sensor.
*
* @param errorMessage The error message from WPIErrors.h
* @param contextMessage A custom message from the code that set the error.
* @param filename Filename of the error source
* @param function Function of the error source
* @param lineNumber Line number of the error source
*/
void ErrorBase::SetWPIError(const char *errorMessage, const char *contextMessage,
const char* filename, const char* function, uint32_t lineNumber) const
{
char err[256];
sprintf(err, "%s: %s", errorMessage, contextMessage);
// Set the current error information for this object.
m_error.Set(-1, err, filename, function, lineNumber, this);
// Update the global error if there is not one already set.
Synchronized mutex(_globalErrorMutex);
if (_globalError.GetCode() == 0) {
_globalError.Clone(m_error);
}
}
void ErrorBase::CloneError(ErrorBase *rhs) const
{
m_error.Clone(rhs->GetError());
}
/**
@brief Check if the current error code represents a fatal error.
@return true if the current error is fatal.
*/
bool ErrorBase::StatusIsFatal() const
{
return m_error.GetCode() < 0;
}
void ErrorBase::SetGlobalError(Error::Code code, const char *contextMessage,
const char* filename, const char* function, uint32_t lineNumber)
{
// If there was an error
if (code != 0) {
Synchronized mutex(_globalErrorMutex);
// Set the current error information for this object.
_globalError.Set(code, contextMessage, filename, function, lineNumber, NULL);
}
}
void ErrorBase::SetGlobalWPIError(const char *errorMessage, const char *contextMessage,
const char* filename, const char* function, uint32_t lineNumber)
{
char err[256];
sprintf(err, "%s: %s", errorMessage, contextMessage);
Synchronized mutex(_globalErrorMutex);
if (_globalError.GetCode() != 0) {
_globalError.Clear();
}
_globalError.Set(-1, err, filename, function, lineNumber, NULL);
}
/**
* Retrieve the current global error.
*/
Error& ErrorBase::GetGlobalError()
{
Synchronized mutex(_globalErrorMutex);
return _globalError;
}

80
wpilibc/src/main/native/GearTooth.cpp
View File

@@ -1,80 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "GearTooth.h"
#include "LiveWindow/LiveWindow.h"
constexpr double GearTooth::kGearToothThreshold;
/**
* Common code called by the constructors.
*/
void GearTooth::EnableDirectionSensing(bool directionSensitive)
{
if (directionSensitive)
{
SetPulseLengthMode(kGearToothThreshold);
}
}
/**
* Construct a GearTooth sensor given a channel.
*
* The default module is assumed.
*
* @param channel The GPIO channel on the digital module that the sensor is connected to.
* @param directionSensitive Enable the pulse length decoding in hardware to specify count direction.
*/
GearTooth::GearTooth(uint32_t channel, bool directionSensitive)
: Counter(channel)
{
EnableDirectionSensing(directionSensitive);
}
/**
* Construct a GearTooth sensor given a channel and module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The GPIO channel on the digital module that the sensor is connected to.
* @param directionSensitive Enable the pulse length decoding in hardware to specify count direction.
*/
GearTooth::GearTooth(uint8_t moduleNumber, uint32_t channel, bool directionSensitive)
: Counter(moduleNumber, channel)
{
EnableDirectionSensing(directionSensitive);
LiveWindow::GetInstance()->AddSensor("GearTooth", moduleNumber, channel, this);
}
/**
* Construct a GearTooth sensor given a digital input.
* This should be used when sharing digial inputs.
*
* @param source An object that fully descibes the input that the sensor is connected to.
* @param directionSensitive Enable the pulse length decoding in hardware to specify count direction.
*/
GearTooth::GearTooth(DigitalSource *source, bool directionSensitive)
: Counter(source)
{
EnableDirectionSensing(directionSensitive);
}
GearTooth::GearTooth(DigitalSource &source, bool directionSensitive): Counter(source)
{
EnableDirectionSensing(directionSensitive);
}
/**
* Free the resources associated with a gear tooth sensor.
*/
GearTooth::~GearTooth()
{
}
std::string GearTooth::GetSmartDashboardType() {
return "GearTooth";
}

248
wpilibc/src/main/native/Gyro.cpp
View File

@@ -1,248 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Gyro.h"
#include "AnalogChannel.h"
#include "AnalogModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Timer.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
const uint32_t Gyro::kOversampleBits;
const uint32_t Gyro::kAverageBits;
constexpr float Gyro::kSamplesPerSecond;
constexpr float Gyro::kCalibrationSampleTime;
constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;
/**
* Initialize the gyro.
* Calibrate the gyro by running for a number of samples and computing the center value for this
* part. Then use the center value as the Accumulator center value for subsequent measurements.
* It's important to make sure that the robot is not moving while the centering calculations are
* in progress, this is typically done when the robot is first turned on while it's sitting at
* rest before the competition starts.
*/
void Gyro::InitGyro()
{
m_table = NULL;
if (!m_analog->IsAccumulatorChannel())
{
wpi_setWPIErrorWithContext(ParameterOutOfRange,
"moduleNumber and/or channel (must be accumulator channel)");
if (m_channelAllocated)
{
delete m_analog;
m_analog = NULL;
}
return;
}
m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
m_analog->SetAverageBits(kAverageBits);
m_analog->SetOversampleBits(kOversampleBits);
float sampleRate = kSamplesPerSecond *
(1 << (kAverageBits + kOversampleBits));
m_analog->GetModule()->SetSampleRate(sampleRate);
Wait(1.0);
m_analog->InitAccumulator();
Wait(kCalibrationSampleTime);
int64_t value;
uint32_t count;
m_analog->GetAccumulatorOutput(&value, &count);
m_center = (uint32_t)((float)value / (float)count + .5);
m_offset = ((float)value / (float)count) - (float)m_center;
m_analog->SetAccumulatorCenter(m_center);
m_analog->SetAccumulatorDeadband(0); ///< TODO: compute / parameterize this
m_analog->ResetAccumulator();
SetPIDSourceParameter(kAngle);
HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel(), m_analog->GetModuleNumber() - 1);
LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetModuleNumber(), m_analog->GetChannel(), this);
}
/**
* Gyro constructor given a slot and a channel.
*
* @param moduleNumber The analog module the gyro is connected to (1).
* @param channel The analog channel the gyro is connected to (1 or 2).
*/
Gyro::Gyro(uint8_t moduleNumber, uint32_t channel)
{
m_analog = new AnalogChannel(moduleNumber, channel);
m_channelAllocated = true;
InitGyro();
}
/**
* Gyro constructor with only a channel.
*
* Use the default analog module slot.
*
* @param channel The analog channel the gyro is connected to.
*/
Gyro::Gyro(uint32_t channel)
{
m_analog = new AnalogChannel(channel);
m_channelAllocated = true;
InitGyro();
}
/**
* Gyro constructor with a precreated analog channel object.
* Use this constructor when the analog channel needs to be shared. There
* is no reference counting when an AnalogChannel is passed to the gyro.
* @param channel The AnalogChannel object that the gyro is connected to.
*/
Gyro::Gyro(AnalogChannel *channel)
{
m_analog = channel;
m_channelAllocated = false;
if (channel == NULL)
{
wpi_setWPIError(NullParameter);
}
else
{
InitGyro();
}
}
Gyro::Gyro(AnalogChannel &channel)
{
m_analog = &channel;
m_channelAllocated = false;
InitGyro();
}
/**
* Reset the gyro.
* Resets the gyro to a heading of zero. This can be used if there is significant
* drift in the gyro and it needs to be recalibrated after it has been running.
*/
void Gyro::Reset()
{
m_analog->ResetAccumulator();
}
/**
* Delete (free) the accumulator and the analog components used for the gyro.
*/
Gyro::~Gyro()
{
if (m_channelAllocated)
delete m_analog;
}
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the oversampling rate, the
* gyro type and the A/D calibration values.
* The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
*
* @return the current heading of the robot in degrees. This heading is based on integration
* of the returned rate from the gyro.
*/
float Gyro::GetAngle( void )
{
int64_t rawValue;
uint32_t count;
m_analog->GetAccumulatorOutput(&rawValue, &count);
int64_t value = rawValue - (int64_t)((float)count * m_offset);
double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() * (double)(1 << m_analog->GetAverageBits()) /
(m_analog->GetModule()->GetSampleRate() * m_voltsPerDegreePerSecond);
return (float)scaledValue;
}
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro analog value
*
* @return the current rate in degrees per second
*/
double Gyro::GetRate( void )
{
return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 * m_analog->GetLSBWeight()
/ ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
}
/**
* Set the gyro type based on the sensitivity.
* This takes the number of volts/degree/second sensitivity of the gyro and uses it in subsequent
* calculations to allow the code to work with multiple gyros.
*
* @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second.
*/
void Gyro::SetSensitivity( float voltsPerDegreePerSecond )
{
m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
}
void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
{
if(pidSource == 0 || pidSource > 2)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
m_pidSource = pidSource;
}
/**
* Get the angle in degrees for the PIDSource base object.
*
* @return The angle in degrees.
*/
double Gyro::PIDGet()
{
switch(m_pidSource){
case kRate:
return GetRate();
case kAngle:
return GetAngle();
default:
return 0;
}
}
void Gyro::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetAngle());
}
}
void Gyro::StartLiveWindowMode() {
}
void Gyro::StopLiveWindowMode() {
}
std::string Gyro::GetSmartDashboardType() {
return "Gyro";
}
void Gyro::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Gyro::GetTable() {
return m_table;
}

388
wpilibc/src/main/native/HiTechnicColorSensor.cpp
View File

@@ -1,388 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "HiTechnicColorSensor.h"
#include "DigitalModule.h"
#include "I2C.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "networktables2/type/NumberArray.h"
#include "WPIErrors.h"
const uint8_t HiTechnicColorSensor::kAddress;
const uint8_t HiTechnicColorSensor::kManufacturerBaseRegister;
const uint8_t HiTechnicColorSensor::kManufacturerSize;
const uint8_t HiTechnicColorSensor::kSensorTypeBaseRegister;
const uint8_t HiTechnicColorSensor::kSensorTypeSize;
const uint8_t HiTechnicColorSensor::kModeRegister;
const uint8_t HiTechnicColorSensor::kColorRegister;
const uint8_t HiTechnicColorSensor::kRedRegister;
const uint8_t HiTechnicColorSensor::kGreenRegister;
const uint8_t HiTechnicColorSensor::kBlueRegister;
const uint8_t HiTechnicColorSensor::kRawRedRegister;
const uint8_t HiTechnicColorSensor::kRawGreenRegister;
const uint8_t HiTechnicColorSensor::kRawBlueRegister;
/**
* Constructor.
*
* @param moduleNumber The digital module that the sensor is plugged into (1 or 2).
*/
HiTechnicColorSensor::HiTechnicColorSensor(uint8_t 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_t kExpectedManufacturer[] = "HiTechnc";
const uint8_t kExpectedSensorType[] = "ColorPD ";
if ( ! m_i2c->VerifySensor(kManufacturerBaseRegister, kManufacturerSize, kExpectedManufacturer) )
{
wpi_setWPIError(CompassManufacturerError);
return;
}
if ( ! m_i2c->VerifySensor(kSensorTypeBaseRegister, kSensorTypeSize, kExpectedSensorType) )
{
wpi_setWPIError(CompassTypeError);
}
HALReport(HALUsageReporting::kResourceType_HiTechnicColorSensor, moduleNumber - 1);
}
}
/**
* Destructor.
*/
HiTechnicColorSensor::~HiTechnicColorSensor()
{
delete m_i2c;
m_i2c = NULL;
}
/**
* Get the estimated color.
*
* Gets a color estimate from the sensor corresponding to the
* table found with the sensor or at the following site:
* http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NCO1038
*
* @return The estimated color.
*/
uint8_t HiTechnicColorSensor::GetColor()
{
uint8_t color = 0;
if(m_mode != kActive)
{
SetMode(kActive);
}
if (m_i2c)
{
m_i2c->Read(kColorRegister, sizeof(color), &color);
}
return color;
}
/**
* Get the Red value.
*
* Gets the (0-255) red value from the sensor.
*
* The sensor must be in active mode to access the regular RGB data
* if the sensor is not in active mode, it will be placed into active
* mode by this method.
*
* @return The Red sensor value.
*/
uint8_t HiTechnicColorSensor::GetRed()
{
uint8_t red = 0;
if(m_mode != kActive)
{
SetMode(kActive);
}
if (m_i2c)
{
m_i2c->Read(kRedRegister, sizeof(red), &red);
}
return red;
}
/**
* Get the Green value.
*
* Gets the(0-255) green value from the sensor.
*
* The sensor must be in active mode to access the regular RGB data
* if the sensor is not in active mode, it will be placed into active
* mode by this method.
*
* @return The Green sensor value.
*/
uint8_t HiTechnicColorSensor::GetGreen()
{
uint8_t green = 0;
if(m_mode != kActive)
{
SetMode(kActive);
}
if (m_i2c)
{
m_i2c->Read(kGreenRegister, sizeof(green), &green);
}
return green;
}
/**
* Get the Blue value.
*
* Gets the raw (0-255) blue value from the sensor.
*
* The sensor must be in active mode to access the regular RGB data
* if the sensor is not in active mode, it will be placed into active
* mode by this method.
*
* @return The Blue sensor value.
*/
uint8_t HiTechnicColorSensor::GetBlue()
{
uint8_t blue = 0;
if(m_mode != kActive)
{
SetMode(kActive);
}
if (m_i2c)
{
m_i2c->Read(kBlueRegister, sizeof(blue), &blue);
}
return blue;
}
/**
* Get the value of all three colors from a single sensor reading.
* Using this method ensures that all three values come from the
* same sensor reading, using the individual color methods provides
* no such guarantee.
*
* The sensor must be in active mode to access the regular RGB data.
* If the sensor is not in active mode, it will be placed into active
* mode by this method.
*
* @return RGB object with the three color values
*/
HiTechnicColorSensor::RGB HiTechnicColorSensor::GetRGB()
{
uint8_t colors[3] = {0,0,0};
RGB result;
if(m_mode != kActive)
{
SetMode(kActive);
}
if(m_i2c)
{
m_i2c->Read(kRawRedRegister, sizeof(colors), (uint8_t*)&colors);
}
result.red = colors[0];
result.green = colors[1];
result.blue = colors[2];
return result;
}
/**
* Get the Raw Red value.
*
* Gets the (0-65536) raw red value from the sensor.
*
* The sensor must be in raw or passive mode to access the regular RGB data
* if the sensor is not in raw or passive mode, it will be placed into raw
* mode by this method.
*
* @return The Raw Red sensor value.
*/
uint16_t HiTechnicColorSensor::GetRawRed()
{
uint16_t rawRed = 0;
if(m_mode == kActive)
{
SetMode(kRaw);
}
if (m_i2c)
{
m_i2c->Read(kRawRedRegister, sizeof(rawRed), (uint8_t *)&rawRed);
}
return rawRed;
}
/**
* Get the Raw Green value.
*
* Gets the (0-65536) raw green value from the sensor.
*
* The sensor must be in raw or passive mode to access the regular RGB data
* if the sensor is not in raw or passive mode, it will be placed into raw
* mode by this method.
*
* @return The Raw Green sensor value.
*/
uint16_t HiTechnicColorSensor::GetRawGreen()
{
uint16_t rawGreen = 0;
if(m_mode == kActive)
{
SetMode(kRaw);
}
if (m_i2c)
{
m_i2c->Read(kRawGreenRegister, sizeof(rawGreen), (uint8_t *)&rawGreen);
}
return rawGreen;
}
/**
* Get the Raw Blue value.
*
* Gets the (0-65536) raw blue value from the sensor.
*
* The sensor must be in raw or passive mode to access the regular RGB data
* if the sensor is not in raw or passive mode, it will be placed into raw
* mode by this method.
*
* @return The Raw Blue sensor value.
*/
uint16_t HiTechnicColorSensor::GetRawBlue()
{
uint16_t rawBlue = 0;
if(m_mode == kActive)
{
SetMode(kRaw);
}
if (m_i2c)
{
m_i2c->Read(kRawBlueRegister, sizeof(rawBlue), (uint8_t *)&rawBlue);
}
return rawBlue;
}
/**
* Get the raw value of all three colors from a single sensor reading.
* Using this method ensures that all three values come from the
* same sensor reading, using the individual color methods provides
* no such guarantee.
*
* Gets the (0-65536) raw color values from the sensor.
*
* The sensor must be in raw or passive mode to access the regular RGB data
* if the sensor is not in raw or passive mode, it will be placed into raw
* mode by this method.
*
* @return An RGB object with the raw sensor values.
*/
HiTechnicColorSensor::RGB HiTechnicColorSensor::GetRawRGB()
{
uint8_t colors[6] = {0,0,0,0,0,0};
RGB result;
if(m_mode != kActive)
{
SetMode(kActive);
}
if(m_i2c)
{
m_i2c->Read(kRedRegister, sizeof(colors), (uint8_t*)&colors);
}
result.red = (colors[0]<<8) + colors[1];
result.green = (colors[2]<<8) + colors[3];
result.blue = (colors[4]<<8) + colors[5];
return result;
}
/**
* Set the Mode of the color sensor
* This method is used to set the color sensor to one of the three modes,
* active, passive or raw. The sensor defaults to active mode which uses the
* internal LED and returns an interpreted color value and 3 8-bit RGB channel
* values. Raw mode uses the internal LED and returns 3 16-bit RGB channel values.
* Passive mode disables the internal LED and returns 3 16-bit RGB channel values.
* @param mode The mode to set
*/
void HiTechnicColorSensor::SetMode(tColorMode mode)
{
if(m_i2c)
{
m_i2c->Write(kModeRegister, (uint8_t)mode);
}
}
/*
* Live Window code, only does anything if live window is activated.
*/
std::string HiTechnicColorSensor::GetType()
{
return "Compass";
}
/**
* {@inheritDoc}
*/
void HiTechnicColorSensor::InitTable(ITable *subtable) {
m_table = subtable;
UpdateTable();
}
/**
* {@inheritDoc}
*/
void HiTechnicColorSensor::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetColor());
NumberArray* rgb = new NumberArray();
rgb->add(GetRed());
rgb->add(GetGreen());
rgb->add(GetBlue());
m_table->PutValue("RGB", *rgb);
delete rgb;
}
}
/**
* {@inheritDoc}
*/
ITable* HiTechnicColorSensor::GetTable()
{
return m_table;
}
/**
* {@inheritDoc}
*/
void HiTechnicColorSensor::StartLiveWindowMode()
{
}
/**
* {@inheritDoc}
*/
void HiTechnicColorSensor::StopLiveWindowMode()
{
}

108
wpilibc/src/main/native/HiTechnicCompass.cpp
View File

@@ -1,108 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "HiTechnicCompass.h"
#include "DigitalModule.h"
#include "I2C.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
const uint8_t HiTechnicCompass::kAddress;
const uint8_t HiTechnicCompass::kManufacturerBaseRegister;
const uint8_t HiTechnicCompass::kManufacturerSize;
const uint8_t HiTechnicCompass::kSensorTypeBaseRegister;
const uint8_t HiTechnicCompass::kSensorTypeSize;
const uint8_t HiTechnicCompass::kHeadingRegister;
/**
* Constructor.
*
* @param moduleNumber The digital module that the sensor is plugged into (1 or 2).
*/
HiTechnicCompass::HiTechnicCompass(uint8_t moduleNumber)
: m_i2c (NULL)
{
m_table = NULL;
DigitalModule *module = DigitalModule::GetInstance(moduleNumber);
if (module)
{
m_i2c = module->GetI2C(kAddress);
// Verify Sensor
const uint8_t kExpectedManufacturer[] = "HiTechnc";
const uint8_t kExpectedSensorType[] = "Compass ";
if ( ! m_i2c->VerifySensor(kManufacturerBaseRegister, kManufacturerSize, kExpectedManufacturer) )
{
wpi_setWPIError(CompassManufacturerError);
return;
}
if ( ! m_i2c->VerifySensor(kSensorTypeBaseRegister, kSensorTypeSize, kExpectedSensorType) )
{
wpi_setWPIError(CompassTypeError);
}
HALReport(HALUsageReporting::kResourceType_HiTechnicCompass, moduleNumber - 1);
LiveWindow::GetInstance()->AddSensor("HiTechnicCompass", moduleNumber, 0, this);
}
}
/**
* Destructor.
*/
HiTechnicCompass::~HiTechnicCompass()
{
delete m_i2c;
m_i2c = NULL;
}
/**
* Get the compass angle in degrees.
*
* The resolution of this reading is 1 degree.
*
* @return Angle of the compass in degrees.
*/
float HiTechnicCompass::GetAngle()
{
uint16_t heading = 0;
if (m_i2c)
{
m_i2c->Read(kHeadingRegister, sizeof(heading), (uint8_t *)&heading);
// Sensor is little endian... swap bytes
heading = (heading >> 8) | (heading << 8);
}
return (float)heading;
}
void HiTechnicCompass::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetAngle());
}
}
void HiTechnicCompass::StartLiveWindowMode() {
}
void HiTechnicCompass::StopLiveWindowMode() {
}
std::string HiTechnicCompass::GetSmartDashboardType() {
return "HiTechnicCompass";
}
void HiTechnicCompass::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * HiTechnicCompass::GetTable() {
return m_table;
}

199
wpilibc/src/main/native/I2C.cpp
View File

@@ -1,199 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "I2C.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
MUTEX_ID I2C::m_semaphore = NULL;
uint32_t I2C::m_objCount = 0;
/**
* Constructor.
*
* @param module The Digital Module to which the device is conneted.
* @param deviceAddress The address of the device on the I2C bus.
*/
I2C::I2C(DigitalModule *module, uint8_t deviceAddress)
: m_module (module)
, m_deviceAddress (deviceAddress)
, m_compatibilityMode (true)
{
if (m_semaphore == NULL)
{
m_semaphore = initializeMutexNormal();
}
m_objCount++;
HALReport(HALUsageReporting::kResourceType_I2C, deviceAddress, module->GetNumber() - 1);
}
/**
* Destructor.
*/
I2C::~I2C()
{
m_objCount--;
if (m_objCount <= 0)
{
deleteMutex(m_semaphore);
m_semaphore = NULL;
}
}
/**
* Generic transaction.
*
* This is a lower-level interface to the I2C hardware giving you more control over each transaction.
*
* @param dataToSend Buffer of data to send as part of the transaction.
* @param sendSize Number of bytes to send as part of the transaction. [0..6]
* @param dataReceived Buffer to read data into.
* @param receiveSize Number of byted to read from the device. [0..7]
* @return Transfer Aborted... false for success, true for aborted.
*/
bool I2C::Transaction(uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize)
{
if (sendSize > 6) // Optional, provides better error message.
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "sendSize");
return true;
}
if (receiveSize > 7) // Optional, provides better error message.
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "receiveSize");
return true;
}
int32_t status = 0;
bool value = doI2CTransactionWithModule(m_module->m_module, m_deviceAddress, m_compatibilityMode,
dataToSend, sendSize, dataReceived, receiveSize, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Attempt to address a device on the I2C bus.
*
* This allows you to figure out if there is a device on the I2C bus that
* responds to the address specified in the constructor.
*
* @return Transfer Aborted... false for success, true for aborted.
*/
bool I2C::AddressOnly()
{
return Transaction(NULL, 0, NULL, 0);
}
/**
* Execute a write transaction with the device.
*
* Write a single byte to a register on a device and wait until the
* transaction is complete.
*
* @param registerAddress The address of the register on the device to be written.
* @param data The byte to write to the register on the device.
* @return Transfer Aborted... false for success, true for aborted.
*/
bool I2C::Write(uint8_t registerAddress, uint8_t data)
{
uint8_t buffer[2];
buffer[0] = registerAddress;
buffer[1] = data;
return Transaction(buffer, sizeof(buffer), NULL, 0);
}
/**
* Execute a read transaction with the device.
*
* Read 1 to 7 bytes from a device.
* Most I2C devices will auto-increment the register pointer internally
* allowing you to read up to 7 consecutive registers on a device in a
* single transaction.
*
* @param registerAddress The register to read first in the transaction.
* @param count The number of bytes to read in the transaction. [1..7]
* @param buffer A pointer to the array of bytes to store the data read from the device.
* @return Transfer Aborted... false for success, true for aborted.
*/
bool I2C::Read(uint8_t registerAddress, uint8_t count, uint8_t *buffer)
{
if (count < 1 || count > 7)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
return true;
}
if (buffer == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "buffer");
return true;
}
return Transaction(&registerAddress, sizeof(registerAddress), buffer, count);
}
/**
* Send a broadcast write to all devices on the I2C bus.
*
* This is not currently implemented!
*
* @param registerAddress The register to write on all devices on the bus.
* @param data The value to write to the devices.
*/
void I2C::Broadcast(uint8_t registerAddress, uint8_t data)
{
}
/**
* SetCompatibilityMode
*
* Enables bitwise clock skewing detection. This will reduce the I2C interface speed,
* but will allow you to communicate with devices that skew the clock at abnormal times.
* Compatability mode is enabled by default.
* @param enable Enable compatibility mode for this sensor or not.
*/
void I2C::SetCompatibilityMode(bool enable)
{
m_compatibilityMode = enable;
const char *cm = NULL;
if (m_compatibilityMode) cm = "C";
HALReport(HALUsageReporting::kResourceType_I2C, m_deviceAddress, m_module->GetNumber() - 1, cm);
}
/**
* Verify that a device's registers contain expected values.
*
* Most devices will have a set of registers that contain a known value that
* can be used to identify them. This allows an I2C device driver to easily
* verify that the device contains the expected value.
*
* @pre The device must support and be configured to use register auto-increment.
*
* @param registerAddress The base register to start reading from the device.
* @param count The size of the field to be verified.
* @param expected A buffer containing the values expected from the device.
*/
bool I2C::VerifySensor(uint8_t registerAddress, uint8_t count, const uint8_t *expected)
{
// TODO: Make use of all 7 read bytes
uint8_t deviceData[4];
for (uint8_t i=0, curRegisterAddress = registerAddress; i < count; i+=4, curRegisterAddress+=4)
{
uint8_t toRead = count - i < 4 ? count - i : 4;
// Read the chunk of data. Return false if the sensor does not respond.
if (Read(curRegisterAddress, toRead, deviceData)) return false;
for (uint8_t j=0; j<toRead; j++)
{
if(deviceData[j] != expected[i + j]) return false;
}
}
return true;
}

90
wpilibc/src/main/native/InterruptableSensorBase.cpp
View File

@@ -1,90 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "InterruptableSensorBase.h"
#include "Utility.h"
InterruptableSensorBase::InterruptableSensorBase()
{
m_interrupt = NULL;
}
InterruptableSensorBase::~InterruptableSensorBase()
{
}
void InterruptableSensorBase::AllocateInterrupts(bool watcher)
{
wpi_assert(m_interrupt == NULL);
// Expects the calling leaf class to allocate an interrupt index.
int32_t status = 0;
m_interrupt = initializeInterrupts(m_interruptIndex, watcher, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Cancel interrupts on this device.
* This deallocates all the chipobject structures and disables any interrupts.
*/
void InterruptableSensorBase::CancelInterrupts()
{
wpi_assert(m_interrupt != NULL);
int32_t status = 0;
cleanInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_interrupt = NULL;
}
/**
* In synchronous mode, wait for the defined interrupt to occur.
* @param timeout Timeout in seconds
*/
void InterruptableSensorBase::WaitForInterrupt(float timeout)
{
wpi_assert(m_interrupt != NULL);
int32_t status = 0;
waitForInterrupt(m_interrupt, timeout, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Enable interrupts to occur on this input.
* Interrupts are disabled when the RequestInterrupt call is made. This gives time to do the
* setup of the other options before starting to field interrupts.
*/
void InterruptableSensorBase::EnableInterrupts()
{
wpi_assert(m_interrupt != NULL);
int32_t status = 0;
enableInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Disable Interrupts without without deallocating structures.
*/
void InterruptableSensorBase::DisableInterrupts()
{
wpi_assert(m_interrupt != NULL);
int32_t status = 0;
disableInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Return the timestamp for the interrupt that occurred most recently.
* This is in the same time domain as GetClock().
* @return Timestamp in seconds since boot.
*/
double InterruptableSensorBase::ReadInterruptTimestamp()
{
wpi_assert(m_interrupt != NULL);
int32_t status = 0;
double timestamp = readInterruptTimestamp(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return timestamp;
}

337
wpilibc/src/main/native/IterativeRobot.cpp
View File

@@ -1,337 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "IterativeRobot.h"
#include "DriverStation.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/HAL.h"
#include "SmartDashboard/SmartDashboard.h"
#include "LiveWindow/LiveWindow.h"
#include "networktables/NetworkTable.h"
constexpr double IterativeRobot::kDefaultPeriod;
/**
* Constructor for RobotIterativeBase
*
* The constructor initializes the instance variables for the robot to indicate
* the status of initialization for disabled, autonomous, teleop, and test code.
*/
IterativeRobot::IterativeRobot()
: m_disabledInitialized (false)
, m_autonomousInitialized (false)
, m_teleopInitialized (false)
, m_testInitialized (false)
, m_period (kDefaultPeriod)
{
}
/**
* Free the resources for a RobotIterativeBase class.
*/
IterativeRobot::~IterativeRobot()
{
}
/**
* Set the period for the periodic functions.
*
* @param period The period of the periodic function calls. 0.0 means sync to driver station control data.
*/
void IterativeRobot::SetPeriod(double period)
{
if (period > 0.0)
{
// Not syncing with the DS, so start the timer for the main loop
m_mainLoopTimer.Reset();
m_mainLoopTimer.Start();
}
else
{
// Syncing with the DS, don't need the timer
m_mainLoopTimer.Stop();
}
m_period = period;
}
/**
* Get the period for the periodic functions.
* Returns 0.0 if configured to syncronize with DS control data packets.
* @return Period of the periodic function calls
*/
double IterativeRobot::GetPeriod()
{
return m_period;
}
/**
* Get the number of loops per second for the IterativeRobot
* @return Frequency of the periodic function calls
*/
double IterativeRobot::GetLoopsPerSec()
{
// If syncing to the driver station, we don't know the rate,
// so guess something close.
if (m_period <= 0.0)
return 50.0;
return 1.0 / m_period;
}
/**
* Provide an alternate "main loop" via StartCompetition().
*
* This specific StartCompetition() implements "main loop" behavior like that of the FRC
* control system in 2008 and earlier, with a primary (slow) loop that is
* called periodically, and a "fast loop" (a.k.a. "spin loop") that is
* called as fast as possible with no delay between calls.
*/
void IterativeRobot::StartCompetition()
{
HALReport(HALUsageReporting::kResourceType_Framework, HALUsageReporting::kFramework_Iterative);
LiveWindow *lw = LiveWindow::GetInstance();
// first and one-time initialization
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
RobotInit();
// loop forever, calling the appropriate mode-dependent function
lw->SetEnabled(false);
while (true)
{
// Call the appropriate function depending upon the current robot mode
if (IsDisabled())
{
// call DisabledInit() if we are now just entering disabled mode from
// either a different mode or from power-on
if(!m_disabledInitialized)
{
lw->SetEnabled(false);
DisabledInit();
m_disabledInitialized = true;
// reset the initialization flags for the other modes
m_autonomousInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
if (NextPeriodReady())
{
HALNetworkCommunicationObserveUserProgramDisabled();
DisabledPeriodic();
}
}
else if (IsAutonomous())
{
// call AutonomousInit() if we are now just entering autonomous mode from
// either a different mode or from power-on
if(!m_autonomousInitialized)
{
lw->SetEnabled(false);
AutonomousInit();
m_autonomousInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
if (NextPeriodReady())
{
HALNetworkCommunicationObserveUserProgramAutonomous();
AutonomousPeriodic();
}
}
else if (IsTest())
{
// call TestInit() if we are now just entering test mode from
// either a different mode or from power-on
if(!m_testInitialized)
{
lw->SetEnabled(true);
TestInit();
m_testInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady())
{
HALNetworkCommunicationObserveUserProgramTest();
TestPeriodic();
}
}
else
{
// call TeleopInit() if we are now just entering teleop mode from
// either a different mode or from power-on
if(!m_teleopInitialized)
{
lw->SetEnabled(false);
TeleopInit();
m_teleopInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_testInitialized = false;
Scheduler::GetInstance()->SetEnabled(true);
}
if (NextPeriodReady())
{
HALNetworkCommunicationObserveUserProgramTeleop();
TeleopPeriodic();
}
}
// wait for driver station data so the loop doesn't hog the CPU
m_ds->WaitForData();
}
}
/**
* Determine if the periodic functions should be called.
*
* If m_period > 0.0, call the periodic function every m_period as compared
* to Timer.Get(). If m_period == 0.0, call the periodic functions whenever
* a packet is received from the Driver Station, or about every 20ms.
*
* @todo Decide what this should do if it slips more than one cycle.
*/
bool IterativeRobot::NextPeriodReady()
{
if (m_period > 0.0)
{
return m_mainLoopTimer.HasPeriodPassed(m_period);
}
else
{
return m_ds->IsNewControlData();
}
}
/**
* Robot-wide initialization code should go here.
*
* Users should override this method for default Robot-wide initialization which will
* be called when the robot is first powered on. It will be called exactly 1 time.
*/
void IterativeRobot::RobotInit()
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for disabled mode should go here.
*
* Users should override this method for initialization code which will be called each time
* the robot enters disabled mode.
*/
void IterativeRobot::DisabledInit()
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for autonomous mode should go here.
*
* Users should override this method for initialization code which will be called each time
* the robot enters autonomous mode.
*/
void IterativeRobot::AutonomousInit()
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for teleop mode should go here.
*
* Users should override this method for initialization code which will be called each time
* the robot enters teleop mode.
*/
void IterativeRobot::TeleopInit()
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for test mode should go here.
*
* Users should override this method for initialization code which will be called each time
* the robot enters test mode.
*/
void IterativeRobot::TestInit()
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Periodic code for disabled mode should go here.
*
* Users should override this method for code which will be called periodically at a regular
* rate while the robot is in disabled mode.
*/
void IterativeRobot::DisabledPeriodic()
{
static bool firstRun = true;
if (firstRun)
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
delayTicks(1);
}
/**
* Periodic code for autonomous mode should go here.
*
* Users should override this method for code which will be called periodically at a regular
* rate while the robot is in autonomous mode.
*/
void IterativeRobot::AutonomousPeriodic()
{
static bool firstRun = true;
if (firstRun)
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
delayTicks(1);
}
/**
* Periodic code for teleop mode should go here.
*
* Users should override this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void IterativeRobot::TeleopPeriodic()
{
static bool firstRun = true;
if (firstRun)
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
delayTicks(1);
}
/**
* Periodic code for test mode should go here.
*
* Users should override this method for code which will be called periodically at a regular
* rate while the robot is in test mode.
*/
void IterativeRobot::TestPeriodic()
{
static bool firstRun = true;
if (firstRun)
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
delayTicks(1);
}

101
wpilibc/src/main/native/Jaguar.cpp
View File

@@ -1,101 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Jaguar.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "DigitalModule.h"
#include "LiveWindow/LiveWindow.h"
/**
* Common initialization code called by all constructors.
*/
void Jaguar::InitJaguar()
{
/*
* Input profile defined by Luminary Micro.
*
* Full reverse ranges from 0.671325ms to 0.6972211ms
* Proportional reverse ranges from 0.6972211ms to 1.4482078ms
* Neutral ranges from 1.4482078ms to 1.5517922ms
* Proportional forward ranges from 1.5517922ms to 2.3027789ms
* Full forward ranges from 2.3027789ms to 2.328675ms
*/
SetBounds(2.31, 1.55, 1.507, 1.454, .697);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel(), GetModuleNumber() - 1);
LiveWindow::GetInstance()->AddActuator("Jaguar", GetModuleNumber(), GetChannel(), this);
}
/**
* Constructor that assumes the default digital module.
*
* @param channel The PWM channel on the digital module that the Jaguar is attached to.
*/
Jaguar::Jaguar(uint32_t channel) : SafePWM(channel)
{
InitJaguar();
}
/**
* Constructor that specifies the digital module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel on the digital module that the Jaguar is attached to.
*/
Jaguar::Jaguar(uint8_t moduleNumber, uint32_t channel) : SafePWM(moduleNumber, channel)
{
InitJaguar();
}
Jaguar::~Jaguar()
{
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
* @param syncGroup Unused interface.
*/
void Jaguar::Set(float speed, uint8_t syncGroup)
{
SetSpeed(speed);
}
/**
* Get the recently set value of the PWM.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
float Jaguar::Get()
{
return GetSpeed();
}
/**
* Common interface for disabling a motor.
*/
void Jaguar::Disable()
{
SetRaw(kPwmDisabled);
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
void Jaguar::PIDWrite(float output)
{
Set(output);
}

302
wpilibc/src/main/native/Joystick.cpp
View File

@@ -1,302 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Joystick.h"
#include "DriverStation.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include <math.h>
const uint32_t Joystick::kDefaultXAxis;
const uint32_t Joystick::kDefaultYAxis;
const uint32_t Joystick::kDefaultZAxis;
const uint32_t Joystick::kDefaultTwistAxis;
const uint32_t Joystick::kDefaultThrottleAxis;
const uint32_t Joystick::kDefaultTriggerButton;
const uint32_t Joystick::kDefaultTopButton;
static Joystick *joysticks[DriverStation::kJoystickPorts];
static bool joySticksInitialized = false;
/**
* Construct an instance of a joystick.
* The joystick index is the usb port on the drivers station.
*
* @param port The port on the driver station that the joystick is plugged into.
*/
Joystick::Joystick(uint32_t port)
: m_ds (NULL)
, m_port (port)
, m_axes (NULL)
, m_buttons (NULL)
{
InitJoystick(kNumAxisTypes, kNumButtonTypes);
m_axes[kXAxis] = kDefaultXAxis;
m_axes[kYAxis] = kDefaultYAxis;
m_axes[kZAxis] = kDefaultZAxis;
m_axes[kTwistAxis] = kDefaultTwistAxis;
m_axes[kThrottleAxis] = kDefaultThrottleAxis;
m_buttons[kTriggerButton] = kDefaultTriggerButton;
m_buttons[kTopButton] = kDefaultTopButton;
HALReport(HALUsageReporting::kResourceType_Joystick, port);
}
/**
* Version of the constructor to be called by sub-classes.
*
* This constructor allows the subclass to configure the number of constants
* for axes and buttons.
*
* @param port The port on the driver station that the joystick is plugged into.
* @param numAxisTypes The number of axis types in the enum.
* @param numButtonTypes The number of button types in the enum.
*/
Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes)
: m_ds (NULL)
, m_port (port)
, m_axes (NULL)
, m_buttons (NULL)
{
InitJoystick(numAxisTypes, numButtonTypes);
}
void Joystick::InitJoystick(uint32_t numAxisTypes, uint32_t numButtonTypes)
{
if ( !joySticksInitialized )
{
for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++)
joysticks[i] = NULL;
joySticksInitialized = true;
}
joysticks[m_port - 1] = this;
m_ds = DriverStation::GetInstance();
m_axes = new uint32_t[numAxisTypes];
m_buttons = new uint32_t[numButtonTypes];
}
Joystick * Joystick::GetStickForPort(uint32_t port)
{
Joystick *stick = joysticks[port - 1];
if (stick == NULL)
{
stick = new Joystick(port);
joysticks[port - 1] = stick;
}
return stick;
}
Joystick::~Joystick()
{
delete [] m_buttons;
delete [] m_axes;
}
/**
* Get the X value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetX(JoystickHand hand)
{
return GetRawAxis(m_axes[kXAxis]);
}
/**
* Get the Y value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetY(JoystickHand hand)
{
return GetRawAxis(m_axes[kYAxis]);
}
/**
* Get the Z value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetZ()
{
return GetRawAxis(m_axes[kZAxis]);
}
/**
* Get the twist value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetTwist()
{
return GetRawAxis(m_axes[kTwistAxis]);
}
/**
* Get the throttle value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetThrottle()
{
return GetRawAxis(m_axes[kThrottleAxis]);
}
/**
* Get the value of the axis.
*
* @param axis The axis to read [1-6].
* @return The value of the axis.
*/
float Joystick::GetRawAxis(uint32_t axis)
{
return m_ds->GetStickAxis(m_port, axis);
}
/**
* For the current joystick, return the axis determined by the argument.
*
* This is for cases where the joystick axis is returned programatically, otherwise one of the
* previous functions would be preferable (for example GetX()).
*
* @param axis The axis to read.
* @return The value of the axis.
*/
float Joystick::GetAxis(AxisType axis)
{
switch(axis)
{
case kXAxis: return this->GetX();
case kYAxis: return this->GetY();
case kZAxis: return this->GetZ();
case kTwistAxis: return this->GetTwist();
case kThrottleAxis: return this->GetThrottle();
default:
wpi_setWPIError(BadJoystickAxis);
return 0.0;
}
}
/**
* Read the state of the trigger on the joystick.
*
* Look up which button has been assigned to the trigger and read its state.
*
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the trigger.
*/
bool Joystick::GetTrigger(JoystickHand hand)
{
return GetRawButton(m_buttons[kTriggerButton]);
}
/**
* Read the state of the top button on the joystick.
*
* Look up which button has been assigned to the top and read its state.
*
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the top button.
*/
bool Joystick::GetTop(JoystickHand hand)
{
return GetRawButton(m_buttons[kTopButton]);
}
/**
* This is not supported for the Joystick.
* This method is only here to complete the GenericHID interface.
*/
bool Joystick::GetBumper(JoystickHand hand)
{
// Joysticks don't have bumpers.
return false;
}
/**
* Get the button value for buttons 1 through 12.
*
* The buttons are returned in a single 16 bit value with one bit representing the state
* of each button. The appropriate button is returned as a boolean value.
*
* @param button The button number to be read.
* @return The state of the button.
**/
bool Joystick::GetRawButton(uint32_t button)
{
return ((0x1 << (button-1)) & m_ds->GetStickButtons(m_port)) != 0;
}
/**
* Get buttons based on an enumerated type.
*
* The button type will be looked up in the list of buttons and then read.
*
* @param button The type of button to read.
* @return The state of the button.
*/
bool Joystick::GetButton(ButtonType button)
{
switch (button)
{
case kTriggerButton: return GetTrigger();
case kTopButton: return GetTop();
default:
return false;
}
}
/**
* Get the channel currently associated with the specified axis.
*
* @param axis The axis to look up the channel for.
* @return The channel fr the axis.
*/
uint32_t Joystick::GetAxisChannel(AxisType axis)
{
return m_axes[axis];
}
/**
* Set the channel associated with a specified axis.
*
* @param axis The axis to set the channel for.
* @param channel The channel to set the axis to.
*/
void Joystick::SetAxisChannel(AxisType axis, uint32_t channel)
{
m_axes[axis] = channel;
}
/**
* Get the magnitude of the direction vector formed by the joystick's
* current position relative to its origin
*
* @return The magnitude of the direction vector
*/
float Joystick::GetMagnitude(){
return sqrt(pow(GetX(),2) + pow(GetY(),2) );
}
/**
* Get the direction of the vector formed by the joystick and its origin
* in radians
*
* @return The direction of the vector in radians
*/
float Joystick::GetDirectionRadians(){
return atan2(GetX(), -GetY());
}
/**
* Get the direction of the vector formed by the joystick and its origin
* in degrees
*
* uses acos(-1) to represent Pi due to absence of readily accessable Pi
* constant in C++
*
* @return The direction of the vector in degrees
*/
float Joystick::GetDirectionDegrees(){
return (180/acos(-1))*GetDirectionRadians();
}

196
wpilibc/src/main/native/Kinect.cpp
View File

@@ -1,196 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Kinect.h"
#include "DriverStation.h"
//#include "NetworkCommunication/FRCComm.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Skeleton.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <cstring>
#define kHeaderBundleID HALFRC_NetworkCommunication_DynamicType_Kinect_Header
#define kSkeletonExtraBundleID HALFRC_NetworkCommunication_DynamicType_Kinect_Extra1
#define kSkeletonBundleID HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices1
Kinect *Kinect::_instance = NULL;
Kinect::Kinect() :
m_recentPacketNumber(0),
m_numberOfPlayers(0)
{
AddToSingletonList();
m_dataLock = initializeMutexNormal();
HALReport(HALUsageReporting::kResourceType_Kinect, 0);
}
Kinect::~Kinect()
{
takeMutex(m_dataLock);
deleteMutex(m_dataLock);
}
/**
* Get the one and only Kinect object
* @returns pointer to a Kinect
*/
Kinect *Kinect::GetInstance()
{
if (_instance == NULL)
_instance = new Kinect();
return _instance;
}
/**
* Get the number of tracked players on the Kinect
* @return the number of players being actively tracked
*/
int Kinect::GetNumberOfPlayers()
{
UpdateData();
return m_numberOfPlayers;
}
/**
* Get the floor clip plane as defined in the Kinect SDK
* @return The floor clip plane
*/
Kinect::Point4 Kinect::GetFloorClipPlane()
{
UpdateData();
return m_floorClipPlane;
}
/**
* Get the gravity normal from the kinect as defined in the Kinect SDK
* @return The gravity normal (w is ignored)
*/
Kinect::Point4 Kinect::GetGravityNormal()
{
UpdateData();
return m_gravityNormal;
}
/**
* Get the skeleton data
* Returns the detected skeleton data from the kinect as defined in the Kinect SDK
* @param skeletonIndex Which of (potentially 2) skeletons to return. This is ignored in this implementation and
* only a single skeleton is supported for the FRC release default gesture interpretation.
* @return The current version of the skeleton object.
*/
Skeleton Kinect::GetSkeleton(int skeletonIndex)
{
if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
return Skeleton();
}
UpdateData();
return m_skeletons[skeletonIndex-1];
}
/**
* Get the current position of the skeleton
* @param skeletonIndex the skeleton to read from
* @return the current position as defined in the Kinect SDK (w is ignored)
*/
Kinect::Point4 Kinect::GetPosition(int skeletonIndex)
{
if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
return Point4();
}
UpdateData();
return m_position[skeletonIndex-1];
}
/**
* Get the quality of the skeleton.
* Quality masks are defined in the SkeletonQuality enum
* @param skeletonIndex the skeleton to read from
* @return the quality value as defined in the Kinect SDK
*/
uint32_t Kinect::GetQuality(int skeletonIndex)
{
if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
return kClippedRight | kClippedLeft | kClippedTop | kClippedBottom;
}
UpdateData();
return m_quality[skeletonIndex-1];
}
/**
* Get the TrackingState of the skeleton.
* Tracking states are defined in the SkeletonTrackingState enum
* @param skeletonIndex the skeleton to read from
* @return the tracking state value as defined in the Kinect SDK
*/
Kinect::SkeletonTrackingState Kinect::GetTrackingState(int skeletonIndex)
{
if (skeletonIndex <= 0 || skeletonIndex > kNumSkeletons)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Skeleton index must be 1");
return kNotTracked;
}
UpdateData();
return m_trackingState[skeletonIndex-1];
}
/**
* Check for an update of new data from the Driver Station
* This will read the new values and update the data structures in this class.
*/
void Kinect::UpdateData()
{
Synchronized sync(m_dataLock);
uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
if (m_recentPacketNumber != packetNumber)
{
m_recentPacketNumber = packetNumber;
int retVal = HALGetDynamicControlData(kHeaderBundleID, m_rawHeader, sizeof(m_rawHeader), 5);
if(retVal == 0)
{
m_numberOfPlayers = (int)m_rawHeader[13];
memcpy(&m_floorClipPlane.x, &m_rawHeader[18], 4);
memcpy(&m_floorClipPlane.y, &m_rawHeader[22], 4);
memcpy(&m_floorClipPlane.z, &m_rawHeader[26], 4);
memcpy(&m_floorClipPlane.w, &m_rawHeader[30], 4);
memcpy(&m_gravityNormal.x, &m_rawHeader[34], 4);
memcpy(&m_gravityNormal.y, &m_rawHeader[38], 4);
memcpy(&m_gravityNormal.z, &m_rawHeader[42], 4);
}
retVal = HALGetDynamicControlData(kSkeletonExtraBundleID, m_rawSkeletonExtra, sizeof(m_rawSkeletonExtra), 5);
if(retVal == 0)
{
memcpy(&m_position[0].x, &m_rawSkeletonExtra[22], 4);
memcpy(&m_position[0].y, &m_rawSkeletonExtra[26], 4);
memcpy(&m_position[0].z, &m_rawSkeletonExtra[30], 4);
memcpy(&m_quality[0], &m_rawSkeletonExtra[34], 4);
memcpy(&m_trackingState[0], &m_rawSkeletonExtra[38], 4);
}
retVal = HALGetDynamicControlData(kSkeletonBundleID, m_rawSkeleton, sizeof(m_rawSkeleton), 5);
if(retVal == 0)
{
for(int i=0; i < Skeleton::JointCount; i++)
{
memcpy(&m_skeletons[0].m_joints[i].x, &m_rawSkeleton[i*12+2], 4);
memcpy(&m_skeletons[0].m_joints[i].y, &m_rawSkeleton[i*12+6], 4);
memcpy(&m_skeletons[0].m_joints[i].z, &m_rawSkeleton[i*12+10], 4);
m_skeletons[0].m_joints[i].trackingState = (Skeleton::JointTrackingState)m_rawSkeletonExtra[i+2];
}
}
// TODO: Read skeleton #2
}
}

201
wpilibc/src/main/native/KinectStick.cpp
View File

@@ -1,201 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "KinectStick.h"
#include "DriverStation.h"
#include "Joystick.h"
//#include "NetworkCommunication/FRCComm.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include "WPIErrors.h"
uint32_t KinectStick::_recentPacketNumber = 0;
KinectStick::KinectStickData KinectStick::_sticks;
#define kJoystickBundleID HALFRC_NetworkCommunication_DynamicType_Kinect_Joystick
#define kTriggerMask 1
#define kTopMask 2
/**
* Kinect joystick constructor
* @param id value is either 1 or 2 for the left or right joystick decoded from
* gestures interpreted by the Kinect server on the Driver Station computer.
*/
KinectStick::KinectStick(int id)
{
if (id != 1 && id != 2)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "KinectStick ID must be 1 or 2");
return;
}
m_id = id;
HALReport(HALUsageReporting::kResourceType_KinectStick, id);
}
/**
* Get the X value of the KinectStick. This axis
* is unimplemented in the default gestures but can
* be populated by teams editing the Kinect Server.
* @param hand Unused
* @return The X value of the KinectStick
*/
float KinectStick::GetX(JoystickHand hand)
{
return GetRawAxis(Joystick::kDefaultXAxis);
}
/**
* Get the Y value of the KinectStick. This axis
* represents arm angle in the default gestures
* @param hand Unused
* @return The Y value of the KinectStick
*/
float KinectStick::GetY(JoystickHand hand)
{
return GetRawAxis(Joystick::kDefaultYAxis);
}
/**
* Get the Z value of the KinectStick. This axis
* is unimplemented in the default gestures but can
* be populated by teams editing the Kinect Server.
* @param hand Unused
* @return The Z value of the KinectStick
*/
float KinectStick::GetZ()
{
return GetRawAxis(Joystick::kDefaultZAxis);
}
/**
* Get the Twist value of the KinectStick. This axis
* is unimplemented in the default gestures but can
* be populated by teams editing the Kinect Server.
* @return The Twist value of the KinectStick
*/
float KinectStick::GetTwist()
{
return GetRawAxis(Joystick::kDefaultTwistAxis);
}
/**
* Get the Throttle value of the KinectStick. This axis
* is unimplemented in the default gestures but can
* be populated by teams editing the Kinect Server.
* @return The Throttle value of the KinectStick
*/
float KinectStick::GetThrottle()
{
return GetRawAxis(Joystick::kDefaultThrottleAxis);
}
/**
* Get the value of the KinectStick axis.
*
* @param axis The axis to read [1-6].
* @return The value of the axis
*/
float KinectStick::GetRawAxis(uint32_t axis)
{
if (StatusIsFatal()) return 0.0;
GetData();
float value = ConvertRawToFloat(_sticks.formatted.rawSticks[m_id - 1].axis[axis-1]);
return value;
}
/**
* Get the button value for the button set as the default trigger
*
* @param hand Unused
* @return The state of the button.
*/
bool KinectStick::GetTrigger(JoystickHand hand)
{
return GetRawButton(kTriggerMask);
}
/**
* Get the button value for the button set as the default top
*
* @param hand Unused
* @return The state of the button.
*/
bool KinectStick::GetTop(JoystickHand hand)
{
return GetRawButton(kTopMask);
}
/**
* Get the button value for the button set as the default bumper (button 4)
*
* @param hand Unused
* @return The state of the button.
*/
bool KinectStick::GetBumper(JoystickHand hand)
{
// TODO: Should this even be in GenericHID? Is 4 an appropriate mask value (button 3)?
return GetRawButton(4);
}
/**
* Get the button value for buttons 1 through 12. The default gestures
* implement only 9 buttons.
*
* The appropriate button is returned as a boolean value.
*
* @param button The button number to be read.
* @return The state of the button.
*/
bool KinectStick::GetRawButton(uint32_t button)
{
if (StatusIsFatal()) return false;
GetData();
return (_sticks.formatted.rawSticks[m_id - 1].buttons & (1 << button)) != 0;
}
/**
* Get dynamic data from the driver station buffer
*/
void KinectStick::GetData()
{
uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
if (_recentPacketNumber != packetNumber)
{
_recentPacketNumber = packetNumber;
int retVal = HALGetDynamicControlData(kJoystickBundleID, _sticks.data, sizeof(_sticks.data), 5);
if (retVal == 0)
{
wpi_assert(_sticks.formatted.size == sizeof(_sticks.data) - 1);
}
}
}
/**
* Convert an 8 bit joystick value to a floating point (-1,1) value
* @param value The 8 bit raw joystick value returned from the driver station
*/
float KinectStick::ConvertRawToFloat(int8_t value)
{
float result;
if (value < 0)
result = ((float) value) / 128.0;
else
result = ((float) value) / 127.0;
wpi_assert(result <= 1.0 && result >= -1.0);
if (result > 1.0)
result = 1.0;
else if (result < -1.0)
result = -1.0;
return result;
}

187
wpilibc/src/main/native/LiveWindow/LiveWindow.cpp
View File

@@ -1,187 +0,0 @@
#include "LiveWindow/LiveWindow.h"
#include "networktables/NetworkTable.h"
#include <algorithm>
#include <sstream>
LiveWindow* LiveWindow::m_instance = NULL;
/**
* Get an instance of the LiveWindow main class
* This is a singleton to guarantee that there is only a single instance regardless of
* how many times GetInstance is called.
*/
LiveWindow * LiveWindow::GetInstance()
{
if (m_instance == NULL)
{
m_instance = new LiveWindow();
}
return m_instance;
}
/**
* LiveWindow constructor.
* Allocate the necessary tables.
*/
LiveWindow::LiveWindow()
{
m_enabled = false;
m_liveWindowTable = NetworkTable::GetTable("LiveWindow");
m_statusTable = m_liveWindowTable->GetSubTable("~STATUS~");
m_scheduler = Scheduler::GetInstance();
}
/**
* Change the enabled status of LiveWindow
* If it changes to enabled, start livewindow running otherwise stop it
*/
void LiveWindow::SetEnabled(bool enabled)
{
if (m_enabled == enabled)
return;
if (enabled)
{
if (m_firstTime)
{
InitializeLiveWindowComponents();
m_firstTime = false;
}
m_scheduler->SetEnabled(false);
m_scheduler->RemoveAll();
for (std::map<LiveWindowSendable *, LiveWindowComponent>::iterator it =
m_components.begin(); it != m_components.end(); ++it)
{
it->first->StartLiveWindowMode();
}
}
else
{
for (std::map<LiveWindowSendable *, LiveWindowComponent>::iterator it =
m_components.begin(); it != m_components.end(); ++it)
{
it->first->StopLiveWindowMode();
}
m_scheduler->SetEnabled(true);
}
m_enabled = enabled;
m_statusTable->PutBoolean("LW Enabled", m_enabled);
}
LiveWindow::~LiveWindow()
{
}
/**
* Add a Sensor associated with the subsystem and with call it by the given name.
* @param subsystem The subsystem this component is part of.
* @param name The name of this component.
* @param component A LiveWindowSendable component that represents a sensor.
*/
void LiveWindow::AddSensor(const char *subsystem, const char *name,
LiveWindowSendable *component)
{
m_components[component].subsystem = subsystem;
m_components[component].name = name;
m_components[component].isSensor = true;
}
/**
* Add an Actuator associated with the subsystem and with call it by the given name.
* @param subsystem The subsystem this component is part of.
* @param name The name of this component.
* @param component A LiveWindowSendable component that represents a actuator.
*/
void LiveWindow::AddActuator(const char *subsystem, const char *name,
LiveWindowSendable *component)
{
m_components[component].subsystem = subsystem;
m_components[component].name = name;
m_components[component].isSensor = false;
}
/**
* INTERNAL
*/
void LiveWindow::AddSensor(std::string type, int module, int channel, LiveWindowSendable *component)
{
std::ostringstream oss;
oss << type << "[" << module << "," << channel << "]";
std::string types(oss.str());
char* cc = new char[types.size() + 1];
types.copy(cc, types.size());
cc[types.size()]='\0';
AddSensor("Ungrouped", cc, component);
if (std::find(m_sensors.begin(), m_sensors.end(), component) == m_sensors.end())
m_sensors.push_back(component);
}
/**
* INTERNAL
*/
void LiveWindow::AddActuator(std::string type, int module, int channel, LiveWindowSendable *component)
{
std::ostringstream oss;
oss << type << "[" << module << "," << channel << "]";
std::string types(oss.str());
char* cc = new char[types.size() + 1];
types.copy(cc, types.size());
cc[types.size()]='\0';
AddActuator("Ungrouped", cc, component);
}
/**
* Tell all the sensors to update (send) their values
* Actuators are handled through callbacks on their value changing from the
* SmartDashboard widgets.
*/
void LiveWindow::UpdateValues()
{
for (unsigned int i = 0; i < m_sensors.size(); i++)
{
m_sensors[i]->UpdateTable();
}
}
/**
* This method is called periodically to cause the sensors to send new values
* to the SmartDashboard.
*/
void LiveWindow::Run()
{
if (m_enabled)
{
UpdateValues();
}
}
/**
* Initialize all the LiveWindow elements the first time we enter LiveWindow mode.
* By holding off creating the NetworkTable entries, it allows them to be redefined
* before the first time in LiveWindow mode. This allows default sensor and actuator
* values to be created that are replaced with the custom names from users calling
* addActuator and addSensor.
*/
void LiveWindow::InitializeLiveWindowComponents()
{
for (std::map<LiveWindowSendable *, LiveWindowComponent>::iterator it =
m_components.begin(); it != m_components.end(); ++it)
{
LiveWindowSendable *component = it->first;
LiveWindowComponent c = it->second;
std::string subsystem = c.subsystem;
std::string name = c.name;
m_liveWindowTable->GetSubTable(subsystem)->PutString("~TYPE~",
"LW Subsystem");
ITable *table = m_liveWindowTable->GetSubTable(subsystem)->GetSubTable(
name);
table->PutString("~TYPE~", component->GetSmartDashboardType());
table->PutString("Name", name);
table->PutString("Subsystem", subsystem);
component->InitTable(table);
if (c.isSensor)
{
m_sensors.push_back(component);
}
}
}

7
wpilibc/src/main/native/LiveWindow/LiveWindowStatusListener.cpp
View File

@@ -1,7 +0,0 @@
#include "LiveWindow/LiveWindowStatusListener.h"
#include "Commands/Scheduler.h"
void LiveWindowStatusListener::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
}

76
wpilibc/src/main/native/Module.cpp
View File

@@ -1,76 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Module.h"
#include "AnalogModule.h"
#include "DigitalModule.h"
//#include "SolenoidModule.h"
Module* Module::m_modules[kMaxModules] = {NULL};
/**
* Constructor.
*
* @param type The type of module represented.
* @param number The module index within the module type.
*/
Module::Module(nLoadOut::tModuleType type, uint8_t number)
: m_moduleType (type)
, m_moduleNumber (number)
{
m_modules[ToIndex(type, number)] = this;
}
/**
* Destructor.
*/
Module::~Module()
{
}
/**
* Static module singleton factory.
*
* @param type The type of module represented.
* @param number The module index within the module type.
*/
Module* Module::GetModule(nLoadOut::tModuleType type, uint8_t number)
{
if (m_modules[ToIndex(type, number)] == NULL)
{
switch(type)
{
case nLoadOut::kModuleType_Analog:
new AnalogModule(number);
break;
case nLoadOut::kModuleType_Digital:
new DigitalModule(number);
break;
/*
case nLoadOut::kModuleType_Solenoid:
new SolenoidModule(number);
break;
*/
default:
return NULL;
}
}
return m_modules[ToIndex(type, number)];
}
/**
* Create an index into the m_modules array based on type and number
*
* @param type The type of module represented.
* @param number The module index within the module type.
* @return The index into m_modules.
*/
uint8_t Module::ToIndex(nLoadOut::tModuleType type, uint8_t number)
{
if (number == 0 || number > kMaxModuleNumber) return 0;
if (type < nLoadOut::kModuleType_Analog || type > nLoadOut::kModuleType_Solenoid) return 0;
return (type * kMaxModuleNumber) + (number - 1);
}

156
wpilibc/src/main/native/MotorSafetyHelper.cpp
View File

@@ -1,156 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "MotorSafetyHelper.h"
#include "DriverStation.h"
#include "MotorSafety.h"
#include "Timer.h"
#include "WPIErrors.h"
#include <stdio.h>
MotorSafetyHelper *MotorSafetyHelper::m_headHelper = NULL;
ReentrantSemaphore MotorSafetyHelper::m_listMutex;
/**
* The constructor for a MotorSafetyHelper object.
* The helper object is constructed for every object that wants to implement the Motor
* Safety protocol. The helper object has the code to actually do the timing and call the
* motors Stop() method when the timeout expires. The motor object is expected to call the
* Feed() method whenever the motors value is updated.
* @param safeObject a pointer to the motor object implementing MotorSafety. This is used
* to call the Stop() method on the motor.
*/
MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
{
m_safeObject = safeObject;
m_enabled = false;
m_expiration = DEFAULT_SAFETY_EXPIRATION;
m_stopTime = Timer::GetFPGATimestamp();
Synchronized sync(m_listMutex);
m_nextHelper = m_headHelper;
m_headHelper = this;
}
MotorSafetyHelper::~MotorSafetyHelper()
{
Synchronized sync(m_listMutex);
if (m_headHelper == this)
{
m_headHelper = m_nextHelper;
}
else
{
MotorSafetyHelper *prev = NULL;
MotorSafetyHelper *cur = m_headHelper;
while (cur != this && cur != NULL)
prev = cur, cur = cur->m_nextHelper;
if (cur == this)
prev->m_nextHelper = cur->m_nextHelper;
}
}
/*
* Feed the motor safety object.
* Resets the timer on this object that is used to do the timeouts.
*/
void MotorSafetyHelper::Feed()
{
Synchronized sync(m_syncMutex);
m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
}
/*
* Set the expiration time for the corresponding motor safety object.
* @param expirationTime The timeout value in seconds.
*/
void MotorSafetyHelper::SetExpiration(float expirationTime)
{
Synchronized sync(m_syncMutex);
m_expiration = expirationTime;
}
/**
* Retrieve the timeout value for the corresponding motor safety object.
* @returns the timeout value in seconds.
*/
float MotorSafetyHelper::GetExpiration()
{
Synchronized sync(m_syncMutex);
return m_expiration;
}
/**
* Determine if the motor is still operating or has timed out.
* @returns a true value if the motor is still operating normally and hasn't timed out.
*/
bool MotorSafetyHelper::IsAlive()
{
Synchronized sync(m_syncMutex);
return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
}
/**
* Check if this motor has exceeded its timeout.
* This method is called periodically to determine if this motor has exceeded its timeout
* value. If it has, the stop method is called, and the motor is shut down until its value is
* updated again.
*/
void MotorSafetyHelper::Check()
{
DriverStation *ds = DriverStation::GetInstance();
if (!m_enabled || ds->IsDisabled() || ds->IsTest()) return;
Synchronized sync(m_syncMutex);
if (m_stopTime < Timer::GetFPGATimestamp())
{
char buf[128];
char desc[64];
m_safeObject->GetDescription(desc);
snprintf(buf, 128, "%s... Output not updated often enough.", desc);
wpi_setWPIErrorWithContext(Timeout, buf);
m_safeObject->StopMotor();
}
}
/**
* Enable/disable motor safety for this device
* Turn on and off the motor safety option for this PWM object.
* @param enabled True if motor safety is enforced for this object
*/
void MotorSafetyHelper::SetSafetyEnabled(bool enabled)
{
Synchronized sync(m_syncMutex);
m_enabled = enabled;
}
/**
* Return the state of the motor safety enabled flag
* Return if the motor safety is currently enabled for this devicce.
* @returns True if motor safety is enforced for this device
*/
bool MotorSafetyHelper::IsSafetyEnabled()
{
Synchronized sync(m_syncMutex);
return m_enabled;
}
/**
* Check the motors to see if any have timed out.
* This static method is called periodically to poll all the motors and stop any that have
* timed out.
*/
void MotorSafetyHelper::CheckMotors()
{
Synchronized sync(m_listMutex);
for (MotorSafetyHelper *msh = m_headHelper; msh != NULL; msh = msh->m_nextHelper)
{
msh->Check();
}
}

260
wpilibc/src/main/native/Notifier.cpp
View File

@@ -1,260 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Notifier.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"
Notifier *Notifier::timerQueueHead = NULL;
ReentrantSemaphore Notifier::queueSemaphore;
void* Notifier::m_notifier = NULL;
int Notifier::refcount = 0;
/**
* 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 = initializeSemaphore(SEMAPHORE_FULL);
{
Synchronized sync(queueSemaphore);
// do the first time intialization of static variables
if (refcount == 0)
{
int32_t status = 0;
m_notifier = initializeNotifier(ProcessQueue, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
refcount++;
}
}
/**
* Free the resources for a timer event.
* All resources will be freed and the timer event will be removed from the
* queue if necessary.
*/
Notifier::~Notifier()
{
{
Synchronized sync(queueSemaphore);
DeleteFromQueue();
// Delete the static variables when the last one is going away
if (!(--refcount))
{
int32_t status = 0;
cleanNotifier(m_notifier, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
// Acquire the semaphore; this makes certain that the handler is
// not being executed by the interrupt manager.
takeSemaphore(m_handlerSemaphore);
// Delete while holding the semaphore so there can be no race.
deleteSemaphore(m_handlerSemaphore);
}
/**
* Update the alarm hardware to reflect the current first element in the queue.
* Compute the time the next alarm should occur based on the current time and the
* period for the first element in the timer queue.
* WARNING: this method does not do synchronization! It must be called from somewhere
* that is taking care of synchronizing access to the queue.
*/
void Notifier::UpdateAlarm()
{
if (timerQueueHead != NULL)
{
int32_t status = 0;
updateNotifierAlarm(m_notifier, (uint32_t)(timerQueueHead->m_expirationTime * 1e6), &status);
wpi_setStaticErrorWithContext(timerQueueHead, status, getHALErrorMessage(status));
}
}
/**
* ProcessQueue is called whenever there is a timer interrupt.
* We need to wake up and process the current top item in the timer queue as long
* as its scheduled time is after the current time. Then the item is removed or
* rescheduled (repetitive events) in the queue.
*/
void Notifier::ProcessQueue(uint32_t mask, void *params)
{
Notifier *current;
while (true) // keep processing past events until no more
{
{
Synchronized sync(queueSemaphore);
double currentTime = GetClock();
current = timerQueueHead;
if (current == NULL || current->m_expirationTime > currentTime)
{
break; // no more timer events to process
}
// need to process this entry
timerQueueHead = current->m_nextEvent;
if (current->m_periodic)
{
// if periodic, requeue the event
// compute when to put into queue
current->InsertInQueue(true);
}
else
{
// not periodic; removed from queue
current->m_queued = false;
}
// Take handler semaphore while holding queue semaphore to make sure
// the handler will execute to completion in case we are being deleted.
takeSemaphore(current->m_handlerSemaphore);
}
current->m_handler(current->m_param); // call the event handler
giveSemaphore(current->m_handlerSemaphore);
}
// reschedule the first item in the queue
Synchronized sync(queueSemaphore);
UpdateAlarm();
}
/**
* Insert this Notifier into the timer queue in right place.
* WARNING: this method does not do synchronization! It must be called from somewhere
* that is taking care of synchronizing access to the queue.
* @param reschedule If false, the scheduled alarm is based on the curent time and UpdateAlarm
* method is called which will enable the alarm if necessary.
* If true, update the time by adding the period (no drift) when rescheduled periodic from ProcessQueue.
* This ensures that the public methods only update the queue after finishing inserting.
*/
void Notifier::InsertInQueue(bool reschedule)
{
if (reschedule)
{
m_expirationTime += m_period;
}
else
{
m_expirationTime = GetClock() + m_period;
}
if (timerQueueHead == NULL || timerQueueHead->m_expirationTime >= this->m_expirationTime)
{
// the queue is empty or greater than the new entry
// the new entry becomes the first element
this->m_nextEvent = timerQueueHead;
timerQueueHead = this;
if (!reschedule)
{
// since the first element changed, update alarm, unless we already plan to
UpdateAlarm();
}
}
else
{
for (Notifier **npp = &(timerQueueHead->m_nextEvent); ; npp = &(*npp)->m_nextEvent)
{
Notifier *n = *npp;
if (n == NULL || n->m_expirationTime > this->m_expirationTime)
{
*npp = this;
this->m_nextEvent = n;
break;
}
}
}
m_queued = true;
}
/**
* Delete this Notifier from the timer queue.
* WARNING: this method does not do synchronization! It must be called from somewhere
* that is taking care of synchronizing access to the queue.
* Remove this Notifier from the timer queue and adjust the next interrupt time to reflect
* the current top of the queue.
*/
void Notifier::DeleteFromQueue()
{
if (m_queued)
{
m_queued = false;
wpi_assert(timerQueueHead != NULL);
if (timerQueueHead == this)
{
// remove the first item in the list - update the alarm
timerQueueHead = this->m_nextEvent;
UpdateAlarm();
}
else
{
for (Notifier *n = timerQueueHead; n != NULL; n = n->m_nextEvent)
{
if (n->m_nextEvent == this)
{
// this element is the next element from *n from the queue
n->m_nextEvent = this->m_nextEvent; // point around this one
}
}
}
}
}
/**
* Register for single event notification.
* A timer event is queued for a single event after the specified delay.
* @param delay Seconds to wait before the handler is called.
*/
void Notifier::StartSingle(double delay)
{
Synchronized sync(queueSemaphore);
m_periodic = false;
m_period = delay;
DeleteFromQueue();
InsertInQueue(false);
}
/**
* Register for periodic event notification.
* A timer event is queued for periodic event notification. Each time the interrupt
* occurs, the event will be immediately requeued for the same time interval.
* @param period Period in seconds to call the handler starting one period after the call to this method.
*/
void Notifier::StartPeriodic(double period)
{
Synchronized sync(queueSemaphore);
m_periodic = true;
m_period = period;
DeleteFromQueue();
InsertInQueue(false);
}
/**
* Stop timer events from occuring.
* Stop any repeating timer events from occuring. This will also remove any single
* notification events from the queue.
* If a timer-based call to the registered handler is in progress, this function will
* block until the handler call is complete.
*/
void Notifier::Stop()
{
{
Synchronized sync(queueSemaphore);
DeleteFromQueue();
}
// Wait for a currently executing handler to complete before returning from Stop()
Synchronized sync(m_handlerSemaphore);
}

614
wpilibc/src/main/native/PIDController.cpp
View File

@@ -1,614 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "PIDController.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Notifier.h"
#include "PIDSource.h"
#include "PIDOutput.h"
#include <math.h>
#include "HAL/cpp/Synchronized.h"
static const char *kP = "p";
static const char *kI = "i";
static const char *kD = "d";
static const char *kF = "f";
static const char *kSetpoint = "setpoint";
static const char *kEnabled = "enabled";
/**
* Allocate a PID object with the given constants for P, I, D
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations. This particularly effects calculations of the
* integral and differental terms. The default is 50ms.
*/
PIDController::PIDController(float Kp, float Ki, float Kd,
PIDSource *source, PIDOutput *output,
float period) :
m_semaphore (0)
{
Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
}
/**
* Allocate a PID object with the given constants for P, I, D
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations. This particularly effects calculations of the
* integral and differental terms. The default is 50ms.
*/
PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
PIDSource *source, PIDOutput *output,
float period) :
m_semaphore (0)
{
Initialize(Kp, Ki, Kd, Kf, source, output, period);
}
void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
PIDSource *source, PIDOutput *output,
float period)
{
m_table = NULL;
m_semaphore = initializeMutexNormal();
m_controlLoop = new Notifier(PIDController::CallCalculate, this);
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_maximumOutput = 1.0;
m_minimumOutput = -1.0;
m_maximumInput = 0;
m_minimumInput = 0;
m_continuous = false;
m_enabled = false;
m_setpoint = 0;
m_prevError = 0;
m_totalError = 0;
m_tolerance = .05;
m_result = 0;
m_pidInput = source;
m_pidOutput = output;
m_period = period;
m_controlLoop->StartPeriodic(m_period);
static int32_t instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_PIDController, instances);
m_toleranceType = kNoTolerance;
}
/**
* Free the PID object
*/
PIDController::~PIDController()
{
takeMutex(m_semaphore);
deleteMutex(m_semaphore);
delete m_controlLoop;
}
/**
* Call the Calculate method as a non-static method. This avoids having to prepend
* all local variables in that method with the class pointer. This way the "this"
* pointer will be set up and class variables can be called more easily.
* This method is static and called by the Notifier class.
* @param controller the address of the PID controller object to use in the background loop
*/
void PIDController::CallCalculate(void *controller)
{
PIDController *control = (PIDController*) controller;
control->Calculate();
}
/**
* Read the input, calculate the output accordingly, and write to the output.
* This should only be called by the Notifier indirectly through CallCalculate
* and is created during initialization.
*/
void PIDController::Calculate()
{
bool enabled;
PIDSource *pidInput;
CRITICAL_REGION(m_semaphore)
{
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
pidInput = m_pidInput;
}
END_REGION;
if (enabled)
{
float input = pidInput->PIDGet();
float result;
PIDOutput *pidOutput;
{
Synchronized sync(m_semaphore);
m_error = m_setpoint - input;
if (m_continuous)
{
if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2)
{
if (m_error > 0)
{
m_error = m_error - m_maximumInput + m_minimumInput;
}
else
{
m_error = m_error + m_maximumInput - m_minimumInput;
}
}
}
if(m_I != 0)
{
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput)
{
if (potentialIGain > m_minimumOutput)
m_totalError += m_error;
else
m_totalError = m_minimumOutput / m_I;
}
else
{
m_totalError = m_maximumOutput / m_I;
}
}
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F;
m_prevError = m_error;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;
else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
pidOutput = m_pidOutput;
result = m_result;
}
pidOutput->PIDWrite(result);
}
}
/**
* Set the PID Controller gain parameters.
* Set the proportional, integral, and differential coefficients.
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
void PIDController::SetPID(float p, float i, float d)
{
CRITICAL_REGION(m_semaphore)
{
m_P = p;
m_I = i;
m_D = d;
}
END_REGION;
if (m_table != NULL) {
m_table->PutNumber("p", m_P);
m_table->PutNumber("i", m_I);
m_table->PutNumber("d", m_D);
}
}
/**
* Set the PID Controller gain parameters.
* Set the proportional, integral, and differential coefficients.
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
void PIDController::SetPID(float p, float i, float d, float f)
{
CRITICAL_REGION(m_semaphore)
{
m_P = p;
m_I = i;
m_D = d;
m_F = f;
}
END_REGION;
if (m_table != NULL) {
m_table->PutNumber("p", m_P);
m_table->PutNumber("i", m_I);
m_table->PutNumber("d", m_D);
m_table->PutNumber("f", m_F);
}
}
/**
* Get the Proportional coefficient
* @return proportional coefficient
*/
float PIDController::GetP()
{
CRITICAL_REGION(m_semaphore)
{
return m_P;
}
END_REGION;
}
/**
* Get the Integral coefficient
* @return integral coefficient
*/
float PIDController::GetI()
{
CRITICAL_REGION(m_semaphore)
{
return m_I;
}
END_REGION;
}
/**
* Get the Differential coefficient
* @return differential coefficient
*/
float PIDController::GetD()
{
CRITICAL_REGION(m_semaphore)
{
return m_D;
}
END_REGION;
}
/**
* Get the Feed forward coefficient
* @return Feed forward coefficient
*/
float PIDController::GetF()
{
CRITICAL_REGION(m_semaphore)
{
return m_F;
}
END_REGION;
}
/**
* Return the current PID result
* This is always centered on zero and constrained the the max and min outs
* @return the latest calculated output
*/
float PIDController::Get()
{
float result;
CRITICAL_REGION(m_semaphore)
{
result = m_result;
}
END_REGION;
return result;
}
/**
* Set the PID controller to consider the input to be continuous,
* Rather then using the max and min in as constraints, it considers them to
* be the same point and automatically calculates the shortest route to
* the setpoint.
* @param continuous Set to true turns on continuous, false turns off continuous
*/
void PIDController::SetContinuous(bool continuous)
{
CRITICAL_REGION(m_semaphore)
{
m_continuous = continuous;
}
END_REGION;
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
void PIDController::SetInputRange(float minimumInput, float maximumInput)
{
CRITICAL_REGION(m_semaphore)
{
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
END_REGION;
SetSetpoint(m_setpoint);
}
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum value to write to the output
* @param maximumOutput the maximum value to write to the output
*/
void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
{
CRITICAL_REGION(m_semaphore)
{
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
END_REGION;
}
/**
* Set the setpoint for the PIDController
* @param setpoint the desired setpoint
*/
void PIDController::SetSetpoint(float setpoint)
{
CRITICAL_REGION(m_semaphore)
{
if (m_maximumInput > m_minimumInput)
{
if (setpoint > m_maximumInput)
m_setpoint = m_maximumInput;
else if (setpoint < m_minimumInput)
m_setpoint = m_minimumInput;
else
m_setpoint = setpoint;
}
else
{
m_setpoint = setpoint;
}
}
END_REGION;
if (m_table != NULL) {
m_table->PutNumber("setpoint", m_setpoint);
}
}
/**
* Returns the current setpoint of the PIDController
* @return the current setpoint
*/
float PIDController::GetSetpoint()
{
float setpoint;
CRITICAL_REGION(m_semaphore)
{
setpoint = m_setpoint;
}
END_REGION;
return setpoint;
}
/**
* Retruns the current difference of the input from the setpoint
* @return the current error
*/
float PIDController::GetError()
{
float error;
CRITICAL_REGION(m_semaphore)
{
error = m_setpoint - m_pidInput->PIDGet();
}
END_REGION;
return error;
}
/*
* Set the percentage error which is considered tolerable for use with
* OnTarget.
* @param percentage error which is tolerable
*/
void PIDController::SetTolerance(float percent)
{
CRITICAL_REGION(m_semaphore)
{
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
END_REGION;
}
/*
* Set the percentage error which is considered tolerable for use with
* OnTarget.
* @param percentage error which is tolerable
*/
void PIDController::SetPercentTolerance(float percent)
{
CRITICAL_REGION(m_semaphore)
{
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
END_REGION;
}
/*
* Set the absolute error which is considered tolerable for use with
* OnTarget.
* @param percentage error which is tolerable
*/
void PIDController::SetAbsoluteTolerance(float absTolerance)
{
CRITICAL_REGION(m_semaphore)
{
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
END_REGION;
}
/*
* Return true if the error is within the percentage of the total input range,
* determined by SetTolerance. This asssumes that the maximum and minimum input
* were set using SetInput.
* Currently this just reports on target as the actual value passes through the setpoint.
* Ideally it should be based on being within the tolerance for some period of time.
*/
bool PIDController::OnTarget()
{
bool temp;
CRITICAL_REGION(m_semaphore)
{
switch (m_toleranceType) {
case kPercentTolerance:
temp = fabs(GetError()) < (m_tolerance / 100 * (m_maximumInput - m_minimumInput));
break;
case kAbsoluteTolerance:
temp = fabs(GetError()) < m_tolerance;
break;
//TODO: this case needs an error
case kNoTolerance:
temp = false;
}
}
END_REGION;
return temp;
}
/**
* Begin running the PIDController
*/
void PIDController::Enable()
{
CRITICAL_REGION(m_semaphore)
{
m_enabled = true;
}
END_REGION;
if (m_table != NULL) {
m_table->PutBoolean("enabled", true);
}
}
/**
* Stop running the PIDController, this sets the output to zero before stopping.
*/
void PIDController::Disable()
{
CRITICAL_REGION(m_semaphore)
{
m_pidOutput->PIDWrite(0);
m_enabled = false;
}
END_REGION;
if (m_table != NULL) {
m_table->PutBoolean("enabled", false);
}
}
/**
* Return true if PIDController is enabled.
*/
bool PIDController::IsEnabled()
{
bool enabled;
CRITICAL_REGION(m_semaphore)
{
enabled = m_enabled;
}
END_REGION;
return enabled;
}
/**
* Reset the previous error,, the integral term, and disable the controller.
*/
void PIDController::Reset()
{
Disable();
CRITICAL_REGION(m_semaphore)
{
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}
END_REGION;
}
std::string PIDController::GetSmartDashboardType(){
return "PIDController";
}
void PIDController::InitTable(ITable* table){
if(m_table!=NULL)
m_table->RemoveTableListener(this);
m_table = table;
if(m_table!=NULL){
m_table->PutNumber(kP, GetP());
m_table->PutNumber(kI, GetI());
m_table->PutNumber(kD, GetD());
m_table->PutNumber(kF, GetF());
m_table->PutNumber(kSetpoint, GetSetpoint());
m_table->PutBoolean(kEnabled, IsEnabled());
m_table->AddTableListener(this, false);
}
}
ITable* PIDController::GetTable(){
return m_table;
}
void PIDController::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew){
if (key==kP || key==kI || key==kD || key==kF) {
if (m_P != m_table->GetNumber(kP) || m_I != m_table->GetNumber(kI) || m_D != m_table->GetNumber(kD) || m_F != m_table->GetNumber(kF) ) {
SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0), m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
}
} else if (key==kSetpoint && m_setpoint != value.f) {
SetSetpoint(value.f);
} else if (key==kEnabled && m_enabled != value.b) {
if (value.b) {
Enable();
} else {
Disable();
}
}
}
void PIDController::UpdateTable() {
}
void PIDController::StartLiveWindowMode() {
Disable();
}
void PIDController::StopLiveWindowMode() {
}

404
wpilibc/src/main/native/PWM.cpp
View File

@@ -1,404 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "PWM.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "Utility.h"
#include "WPIErrors.h"
constexpr float PWM::kDefaultPwmPeriod;
constexpr float PWM::kDefaultPwmCenter;
const int32_t PWM::kDefaultPwmStepsDown;
const int32_t PWM::kPwmDisabled;
static Resource *allocated = NULL;
/**
* 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_t moduleNumber, uint32_t channel)
{
m_table = NULL;
char buf[64];
Resource::CreateResourceObject(&allocated, dio_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;
HALReport(HALUsageReporting::kResourceType_PWM, channel, moduleNumber - 1);
}
/**
* Allocate a PWM given a module and channel.
* Allocate a PWM using a module and channel number.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel on the digital module (1..10).
*/
PWM::PWM(uint8_t moduleNumber, uint32_t channel)
: m_module(NULL)
{
InitPWM(moduleNumber, channel);
}
/**
* Allocate a PWM in the default module given a channel.
*
* Using a default module allocate a PWM given the channel number. The default module is the first
* slot numerically in the cRIO chassis.
*
* @param channel The PWM channel on the digital module.
*/
PWM::PWM(uint32_t channel)
: m_module(NULL)
{
InitPWM(GetDefaultDigitalModule(), channel);
}
/**
* Free the PWM channel.
*
* Free the resource associated with the PWM channel and set the value to 0.
*/
PWM::~PWM()
{
if (m_module)
{
m_module->SetPWM(m_channel, kPwmDisabled);
allocated->Free((m_module->GetNumber() - 1) * kPwmChannels + m_channel - 1);
}
}
/**
* Optionally eliminate the deadband from a speed controller.
* @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate
* the deadband in the middle of the range. Otherwise, keep the full range without
* modifying any values.
*/
void PWM::EnableDeadbandElimination(bool eliminateDeadband)
{
if (StatusIsFatal()) return;
m_eliminateDeadband = eliminateDeadband;
}
/**
* Set the bounds on the PWM values.
* This sets the bounds on the PWM values for a particular each type of controller. The values
* determine the upper and lower speeds as well as the deadband bracket.
* @param max The Minimum pwm value
* @param deadbandMax The high end of the deadband range
* @param center The center speed (off)
* @param deadbandMin The low end of the deadband range
* @param min The minimum pwm value
*/
void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, int32_t min)
{
if (StatusIsFatal()) return;
m_maxPwm = max;
m_deadbandMaxPwm = deadbandMax;
m_centerPwm = center;
m_deadbandMinPwm = deadbandMin;
m_minPwm = min;
}
/**
* Set the bounds on the PWM pulse widths.
* This sets the bounds on the PWM values for a particular type of controller. The values
* determine the upper and lower speeds as well as the deadband bracket.
* @param max The max PWM pulse width in ms
* @param deadbandMax The high end of the deadband range pulse width in ms
* @param center The center (off) pulse width in ms
* @param deadbandMin The low end of the deadband pulse width in ms
* @param min The minimum pulse width in ms
*/
void PWM::SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min)
{
// calculate the loop time in milliseconds
double loopTime = m_module->GetLoopTiming()/(kSystemClockTicksPerMicrosecond*1e3);
if (StatusIsFatal()) return;
m_maxPwm = (int32_t)((max-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
m_deadbandMaxPwm = (int32_t)((deadbandMax-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
m_centerPwm = (int32_t)((center-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
m_deadbandMinPwm = (int32_t)((deadbandMin-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
m_minPwm = (int32_t)((min-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
// printf("Calculated m_minPwm: %d min: %lf loopTime: %lf defaultStepsDown: %d\n", m_minPwm, min, loopTime, kDefaultPwmStepsDown);
}
uint32_t PWM::GetModuleNumber()
{
return m_module->GetNumber();
}
/**
* Set the PWM value based on a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param pos The position to set the servo between 0.0 and 1.0.
*/
void PWM::SetPosition(float pos)
{
if (StatusIsFatal()) return;
if (pos < 0.0)
{
pos = 0.0;
}
else if (pos > 1.0)
{
pos = 1.0;
}
// note, need to perform the multiplication below as floating point before converting to int
unsigned short rawValue = (int32_t)( (pos * (float) GetFullRangeScaleFactor()) + GetMinNegativePwm());
// printf("MinNegPWM: %d FullRangeScaleFactor: %d Raw value: %5d Input value: %4.4f\n", GetMinNegativePwm(), GetFullRangeScaleFactor(), rawValue, pos);
// wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <= GetMaxPositivePwm()));
wpi_assert(rawValue != kPwmDisabled);
// send the computed pwm value to the FPGA
SetRaw((unsigned short)rawValue);
}
/**
* Get the PWM value in terms of a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The position the servo is set to between 0.0 and 1.0.
*/
float PWM::GetPosition()
{
if (StatusIsFatal()) return 0.0;
int32_t value = GetRaw();
if (value < GetMinNegativePwm())
{
return 0.0;
}
else if (value > GetMaxPositivePwm())
{
return 1.0;
}
else
{
return (float)(value - GetMinNegativePwm()) / (float)GetFullRangeScaleFactor();
}
}
/**
* Set the PWM value based on a speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetCenterPwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param speed The speed to set the speed controller between -1.0 and 1.0.
*/
void PWM::SetSpeed(float speed)
{
if (StatusIsFatal()) return;
// clamp speed to be in the range 1.0 >= speed >= -1.0
if (speed < -1.0)
{
speed = -1.0;
}
else if (speed > 1.0)
{
speed = 1.0;
}
// calculate the desired output pwm value by scaling the speed appropriately
int32_t rawValue;
if (speed == 0.0)
{
rawValue = GetCenterPwm();
}
else if (speed > 0.0)
{
rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor()) +
((float) GetMinPositivePwm()) + 0.5);
}
else
{
rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor()) +
((float) GetMaxNegativePwm()) + 0.5);
}
// the above should result in a pwm_value in the valid range
wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <= GetMaxPositivePwm()));
wpi_assert(rawValue != kPwmDisabled);
// send the computed pwm value to the FPGA
SetRaw(rawValue);
}
/**
* Get the PWM value in terms of speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The most recently set speed between -1.0 and 1.0.
*/
float PWM::GetSpeed()
{
if (StatusIsFatal()) return 0.0;
int32_t value = GetRaw();
if (value == PWM::kPwmDisabled)
{
return 0.0;
}
else if (value > GetMaxPositivePwm())
{
return 1.0;
}
else if (value < GetMinNegativePwm())
{
return -1.0;
}
else if (value > GetMinPositivePwm())
{
return (float)(value - GetMinPositivePwm()) / (float)GetPositiveScaleFactor();
}
else if (value < GetMaxNegativePwm())
{
return (float)(value - GetMaxNegativePwm()) / (float)GetNegativeScaleFactor();
}
else
{
return 0.0;
}
}
/**
* Set the PWM value directly to the hardware.
*
* Write a raw value to a PWM channel.
*
* @param value Raw PWM value. Range 0 - 255.
*/
void PWM::SetRaw(unsigned short value)
{
if (StatusIsFatal()) return;
m_module->SetPWM(m_channel, value);
}
/**
* Get the PWM value directly from the hardware.
*
* Read a raw value from a PWM channel.
*
* @return Raw PWM control value. Range: 0 - 255.
*/
unsigned short PWM::GetRaw()
{
if (StatusIsFatal()) return 0;
return m_module->GetPWM(m_channel);
}
/**
* Slow down the PWM signal for old devices.
*
* @param mult The period multiplier to apply to this channel
*/
void PWM::SetPeriodMultiplier(PeriodMultiplier mult)
{
if (StatusIsFatal()) return;
switch(mult)
{
case kPeriodMultiplier_4X:
m_module->SetPWMPeriodScale(m_channel, 3); // Squelch 3 out of 4 outputs
break;
case kPeriodMultiplier_2X:
m_module->SetPWMPeriodScale(m_channel, 1); // Squelch 1 out of 2 outputs
break;
case kPeriodMultiplier_1X:
m_module->SetPWMPeriodScale(m_channel, 0); // Don't squelch any outputs
break;
default:
wpi_assert(false);
}
}
void PWM::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
SetSpeed(value.f);
}
void PWM::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetSpeed());
}
}
void PWM::StartLiveWindowMode() {
SetSpeed(0);
if (m_table != NULL) {
m_table->AddTableListener("Value", this, true);
}
}
void PWM::StopLiveWindowMode() {
SetSpeed(0);
if (m_table != NULL) {
m_table->RemoveTableListener(this);
}
}
std::string PWM::GetSmartDashboardType() {
return "Speed Controller";
}
void PWM::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * PWM::GetTable() {
return m_table;
}

620
wpilibc/src/main/native/Preferences.cpp
View File

@@ -1,620 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Preferences.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <stdio.h>
#include <algorithm>
/** Private NI function needed to write to the VxWorks target */
extern "C" int Priv_SetWriteFileAllowed(uint32_t enable);
/** The Preferences table name */
static const char *kTableName = "Preferences";
/** The value of the save field */
static const char *kSaveField = "~S A V E~";
/** The file to save to */
static const char *kFileName = "/c/wpilib-preferences.ini";
/** The characters to put between a field and value */
static const char *kValuePrefix = "=\"";
/** The characters to put after the value */
static const char *kValueSuffix = "\"\n";
/** The singleton instance */
Preferences *Preferences::_instance = NULL;
Preferences::Preferences() :
m_fileLock(NULL),
m_fileOpStarted(NULL),
m_tableLock(NULL),
m_readTask("PreferencesReadTask", (FUNCPTR)Preferences::InitReadTask),
m_writeTask("PreferencesWriteTask", (FUNCPTR)Preferences::InitWriteTask)
{
m_fileLock = initializeMutexNormal();
m_fileOpStarted = initializeSemaphore (SEMAPHORE_EMPTY);
m_tableLock = initializeMutexNormal();
Synchronized sync(m_fileLock);
m_readTask.Start((uint32_t)this);
takeSemaphore(m_fileOpStarted);
HALReport(HALUsageReporting::kResourceType_Preferences, 0);
}
Preferences::~Preferences()
{
takeMutex(m_tableLock);
deleteMutex(m_tableLock);
takeMutex(m_fileLock);
deleteSemaphore(m_fileOpStarted);
deleteMutex(m_fileLock);
}
/**
* Get the one and only {@link Preferences} object
* @return pointer to the {@link Preferences}
*/
Preferences *Preferences::GetInstance()
{
if (_instance == NULL)
_instance = new Preferences;
return _instance;
}
/**
* Returns a vector of all the keys
* @return a vector of the keys
*/
std::vector<std::string> Preferences::GetKeys()
{
return m_keys;
}
/**
* Returns the string at the given key. If this table does not have a value
* for that position, then the given defaultValue will be returned.
* @param key the key
* @param defaultValue the value to return if none exists in the table
* @return either the value in the table, or the defaultValue
*/
std::string Preferences::GetString(const char *key, const char *defaultValue)
{
std::string value = Get(key);
return value.empty() ? defaultValue : value;
}
/**
* Returns the string at the given key. If this table does not have a value
* for that position, then the given defaultValue will be returned.
* @param key the key
* @param value the buffer to copy the value into
* @param valueSize the size of value
* @param defaultValue the value to return if none exists in the table
* @return The size of the returned string
*/
int Preferences::GetString(const char *key, char *value, int valueSize, const char *defaultValue)
{
std::string stringValue = GetString(key, defaultValue);
stringValue.copy(value, valueSize);
return stringValue.size();
}
/**
* Returns the int at the given key. If this table does not have a value
* for that position, then the given defaultValue value will be returned.
* @param key the key
* @param defaultValue the value to return if none exists in the table
* @return either the value in the table, or the defaultValue
*/
int Preferences::GetInt(const char *key, int defaultValue)
{
std::string value = Get(key);
if (value.empty())
return defaultValue;
return strtol(value.c_str(), NULL, 0);
}
/**
* Returns the double at the given key. If this table does not have a value
* for that position, then the given defaultValue value will be returned.
* @param key the key
* @param defaultValue the value to return if none exists in the table
* @return either the value in the table, or the defaultValue
*/
double Preferences::GetDouble(const char *key, double defaultValue)
{
std::string value = Get(key);
if (value.empty())
return defaultValue;
return strtod(value.c_str(), NULL);
}
/**
* Returns the float at the given key. If this table does not have a value
* for that position, then the given defaultValue value will be returned.
* @param key the key
* @param defaultValue the value to return if none exists in the table
* @return either the value in the table, or the defaultValue
*/
float Preferences::GetFloat(const char *key, float defaultValue)
{
std::string value = Get(key);
if (value.empty())
return defaultValue;
return strtod(value.c_str(), NULL);
}
/**
* Returns the boolean at the given key. If this table does not have a value
* for that position, then the given defaultValue value will be returned.
* @param key the key
* @param defaultValue the value to return if none exists in the table
* @return either the value in the table, or the defaultValue
*/
bool Preferences::GetBoolean(const char *key, bool defaultValue)
{
std::string value = Get(key);
if (value.empty())
return defaultValue;
if (value.compare("true") == 0)
return true;
else if (value.compare("false") == 0)
return false;
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Boolean value does not contain \"true\" or \"false\"");
return false;
}
/**
* Returns the long (int64_t) at the given key. If this table does not have a value
* for that position, then the given defaultValue value will be returned.
* @param key the key
* @param defaultValue the value to return if none exists in the table
* @return either the value in the table, or the defaultValue
*/
int64_t Preferences::GetLong(const char *key, int64_t defaultValue)
{
std::string value = Get(key);
if (value.empty())
return defaultValue;
// Ummm... not available in our VxWorks...
//return strtoll(value.c_str(), NULL, 0);
int64_t intVal;
sscanf(value.c_str(), "%lld", &intVal);
return intVal;
}
/**
* 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 == NULL)
{
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);
}
/**
* Puts the given int into the preferences table.
*
* <p>The key may not 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::PutInt(const char *key, int value)
{
char buf[32];
snprintf(buf, 32, "%d", value);
Put(key, buf);
}
/**
* Puts the given double into the preferences table.
*
* <p>The key may not 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::PutDouble(const char *key, double value)
{
char buf[32];
snprintf(buf, 32, "%f", value);
Put(key, buf);
}
/**
* Puts the given float into the preferences table.
*
* <p>The key may not 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::PutFloat(const char *key, float value)
{
char buf[32];
snprintf(buf, 32, "%f", value);
Put(key, buf);
}
/**
* Puts the given boolean into the preferences table.
*
* <p>The key may not 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::PutBoolean(const char *key, bool value)
{
Put(key, value ? "true" : "false");
}
/**
* Puts the given long (int64_t) into the preferences table.
*
* <p>The key may not 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::PutLong(const char *key, int64_t value)
{
char buf[32];
snprintf(buf, 32, "%lld", value);
Put(key, buf);
}
/**
* Saves the preferences to a file on the cRIO.
*
* <p>This should <b>NOT</b> be called often.
* Too many writes can damage the cRIO's flash memory.
* While it is ok to save once or twice a match, this should never
* be called every run of {@link IterativeRobot#TeleopPeriodic()}, etc.</p>
*
* <p>The actual writing of the file is done in a separate thread.
* However, any call to a get or put method will wait until the table is fully saved before continuing.</p>
*/
void Preferences::Save()
{
Synchronized sync(m_fileLock);
m_writeTask.Start((uint32_t)this);
takeSemaphore(m_fileOpStarted);
}
/**
* Returns whether or not there is a key with the given name.
* @param key the key
* @return if there is a value at the given key
*/
bool Preferences::ContainsKey(const char *key)
{
return !Get(key).empty();
}
/**
* Remove a preference
* @param key the key
*/
void Preferences::Remove(const char *key)
{
m_values.erase(std::string(key));
std::vector<std::string>::iterator it = m_keys.begin();
for (; it != m_keys.end(); it++)
{
if (it->compare(key) == 0)
{
m_keys.erase(it);
break;
}
}
}
/**
* 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)
{
Synchronized sync(m_tableLock);
if (key == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "key");
return std::string("");
}
return m_values[std::string(key)];
}
/**
* Puts the given value into the given key position
* @param key the key
* @param value the value
*/
void Preferences::Put(const char *key, std::string value)
{
Synchronized sync(m_tableLock);
if (key == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "key");
return;
}
if (std::string(key).find_first_of("=\n\r \t\"") != std::string::npos)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "key contains illegal characters");
return;
}
std::pair<StringMap::iterator, bool> ret =
m_values.insert(StringMap::value_type(key, value));
if (ret.second)
m_keys.push_back(key);
else
ret.first->second = value;
NetworkTable::GetTable(kTableName)->PutString(key, value);
}
/**
* The internal method to read from a file.
* This will be called in its own thread when the preferences singleton is
* first created.
*/
void Preferences::ReadTaskRun()
{
Synchronized sync(m_tableLock);
giveSemaphore(m_fileOpStarted);
std::string comment;
FILE *file = NULL;
file = fopen(kFileName, "r");
if (file != NULL)
{
std::string buffer;
while (true)
{
char value;
do
{
value = fgetc(file);
} while (value == ' ' || value == '\t');
if (value == '\n' || value == ';')
{
if (value == '\n')
{
comment += "\n";
}
else
{
buffer.clear();
for (; value != '\n' && !feof(file); value = fgetc(file))
buffer += value;
buffer += '\n';
comment += buffer;
}
}
else if (value == '[')
{
// Find the end of the section and the new line after it and throw it away
for (; value != ']' && !feof(file); value = fgetc(file));
for (; value != '\n' && !feof(file); value = fgetc(file));
}
else
{
buffer.clear();
for (; value != '=' && !feof(file); )
{
buffer += value;
do
{
value = fgetc(file);
} while (value == ' ' || value == '\t');
}
std::string name = buffer;
buffer.clear();
bool shouldBreak = false;
do
{
value = fgetc(file);
} while (value == ' ' || value == '\t');
if (value == '"')
{
for (value = fgetc(file); value != '"' && !feof(file); value = fgetc(file))
buffer += value;
// Clear the line
while (fgetc(file) != '\n' && !feof(file));
}
else
{
for (; value != '\n' && !feof(file);)
{
buffer += value;
do
{
value = fgetc(file);
} while (value == ' ' || value == '\t');
}
if (feof(file))
shouldBreak = true;
}
std::string value = buffer;
if (!name.empty() && !value.empty())
{
m_keys.push_back(name);
m_values.insert(std::pair<std::string, std::string>(name, value));
NetworkTable::GetTable(kTableName)->PutString(name, value);
if (!comment.empty())
{
m_comments.insert(std::pair<std::string, std::string>(name, comment));
comment.clear();
}
}
if (shouldBreak)
break;
}
}
}
else
{
wpi_setWPIErrorWithContext(NoAvailableResources, "Failed to open preferences file.");
}
if (file != NULL)
fclose(file);
if (!comment.empty())
m_endComment = comment;
NetworkTable::GetTable(kTableName)->PutBoolean(kSaveField, false);
NetworkTable::GetTable(kTableName)->AddTableListener(this);
}
/**
* Internal method that actually writes the table to a file.
* This is called in its own thread when {@link Preferences#Save() Save()} is called.
*/
void Preferences::WriteTaskRun()
{
Synchronized sync(m_tableLock);
giveSemaphore(m_fileOpStarted);
FILE *file = NULL;
Priv_SetWriteFileAllowed(1);
file = fopen(kFileName, "w");
fputs("[Preferences]\n", file);
std::vector<std::string>::iterator it = m_keys.begin();
for (; it != m_keys.end(); it++)
{
std::string key = *it;
std::string value = m_values[key];
std::string comment = m_comments[key];
if (!comment.empty())
fputs(comment.c_str(), file);
fputs(key.c_str(), file);
fputs(kValuePrefix, file);
fputs(value.c_str(), file);
fputs(kValueSuffix, file);
}
if (!m_endComment.empty())
fputs(m_endComment.c_str(), file);
if (file != NULL)
fclose(file);
NetworkTable::GetTable(kTableName)->PutBoolean(kSaveField, false);
}
static bool isKeyAcceptable(const std::string& value) {
for (unsigned int i = 0; i < value.length(); i++) {
char letter = value.at(i);
switch (letter) {
case '=':
case '\n':
case '\r':
case ' ':
case '\t':
return false;
}
}
return true;
}
void Preferences::ValueChanged(ITable* table, const std::string& key, EntryValue value, bool isNew)
{
if (key==kSaveField)
{
if (table->GetBoolean(kSaveField, false))
Save();
}
else
{
Synchronized sync(m_tableLock);
if (!isKeyAcceptable(key) || table->GetString(key, "").find('"')!=std::string::npos)
{
if(m_values.find(key) != m_values.end()){
m_values.erase(key);
std::vector<std::string>::iterator it = m_keys.begin();
for (; it != m_keys.end(); it++)
{
if (key==*it)
{
m_keys.erase(it);
break;
}
}
table->PutString(key, "\"");
}
}
else
{
std::pair<StringMap::iterator, bool> ret =
m_values.insert(StringMap::value_type(key, table->GetString(key, "")));
if (ret.second)
m_keys.push_back(key);
else
ret.first->second = table->GetString(key, "");
}
}
}

272
wpilibc/src/main/native/Relay.cpp
View File

@@ -1,272 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Relay.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
// Allocate each direction separately.
static Resource *relayChannels = NULL;
/**
* Common relay intitialization methode.
* This code is common to all Relay constructors and initializes the relay and reserves
* all resources that need to be locked. Initially the relay is set to both lines at 0v.
* @param slot The module slot number this relay is connected to.
*
* @param moduleNumber The digital module this relay is connected to (1 or 2).
*/
void Relay::InitRelay (uint8_t moduleNumber)
{
m_table = NULL;
char buf[64];
Resource::CreateResourceObject(&relayChannels, dio_kNumSystems * kRelayChannels * 2);
if (!SensorBase::CheckRelayModule(moduleNumber))
{
snprintf(buf, 64, "Digital Module %d", moduleNumber);
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
return;
}
if (!SensorBase::CheckRelayChannel(m_channel))
{
snprintf(buf, 64, "Relay Channel %d", m_channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly)
{
snprintf(buf, 64, "Forward Relay %d (Module: %d)", m_channel, moduleNumber);
if (relayChannels->Allocate(((moduleNumber - 1) * kRelayChannels + m_channel - 1) * 2, buf) == ~0ul)
{
CloneError(relayChannels);
return;
}
HALReport(HALUsageReporting::kResourceType_Relay, m_channel, moduleNumber - 1);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly)
{
snprintf(buf, 64, "Reverse Relay %d (Module: %d)", m_channel, moduleNumber);
if (relayChannels->Allocate(((moduleNumber - 1) * kRelayChannels + m_channel - 1) * 2 + 1, buf) == ~0ul)
{
CloneError(relayChannels);
return;
}
HALReport(HALUsageReporting::kResourceType_Relay, m_channel + 128, moduleNumber - 1);
}
m_module = DigitalModule::GetInstance(moduleNumber);
m_module->SetRelayForward(m_channel, false);
m_module->SetRelayReverse(m_channel, false);
LiveWindow::GetInstance()->AddActuator("Relay", moduleNumber, m_channel, this);
}
/**
* Relay constructor given the module and the channel.
*
* @param moduleNumber The digital module this relay is connected to (1 or 2).
* @param channel The channel number within the module for this relay.
* @param direction The direction that the Relay object will control.
*/
Relay::Relay(uint8_t moduleNumber, uint32_t channel, Relay::Direction direction)
: m_channel (channel)
, m_direction (direction)
{
InitRelay(moduleNumber);
}
/**
* Relay constructor given a channel only where the default digital module is used.
* @param channel The channel number within the default module for this relay.
* @param direction The direction that the Relay object will control.
*/
Relay::Relay(uint32_t channel, Relay::Direction direction)
: m_channel (channel)
, m_direction (direction)
{
InitRelay(GetDefaultDigitalModule());
}
/**
* Free the resource associated with a relay.
* The relay channels are set to free and the relay output is turned off.
*/
Relay::~Relay()
{
m_module->SetRelayForward(m_channel, false);
m_module->SetRelayReverse(m_channel, false);
if (m_direction == kBothDirections || m_direction == kForwardOnly)
{
relayChannels->Free(((m_module->GetNumber() - 1) * kRelayChannels + m_channel - 1) * 2);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly)
{
relayChannels->Free(((m_module->GetNumber() - 1) * kRelayChannels + m_channel - 1) * 2 + 1);
}
}
/**
* 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;
switch (value)
{
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly)
{
m_module->SetRelayForward(m_channel, false);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly)
{
m_module->SetRelayReverse(m_channel, false);
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly)
{
m_module->SetRelayForward(m_channel, true);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly)
{
m_module->SetRelayReverse(m_channel, true);
}
break;
case kForward:
if (m_direction == kReverseOnly)
{
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly)
{
m_module->SetRelayForward(m_channel, true);
}
if (m_direction == kBothDirections)
{
m_module->SetRelayReverse(m_channel, false);
}
break;
case kReverse:
if (m_direction == kForwardOnly)
{
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections)
{
m_module->SetRelayForward(m_channel, false);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly)
{
m_module->SetRelayReverse(m_channel, true);
}
break;
}
}
/**
* Get the Relay State
*
* Gets the current state of the relay.
*
* When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
* kForward/kReverse (per the recommendation in Set)
*
* @return The current state of the relay as a Relay::Value
*/
Relay::Value Relay::Get() {
if(m_module->GetRelayForward(m_channel)) {
if(m_module->GetRelayReverse(m_channel)) {
return kOn;
} else {
if(m_direction == kForwardOnly) {
return kOn;
} else {
return kForward;
}
}
} else {
if(m_module->GetRelayReverse(m_channel)) {
if(m_direction == kReverseOnly) {
return kOn;
} else {
return kReverse;
}
} else {
return kOff;
}
}
}
void Relay::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
std::string *val = (std::string *) value.ptr;
if (*val == "Off") Set(kOff);
else if (*val == "Forward") Set(kForward);
else if (*val == "Reverse") Set(kReverse);
}
void Relay::UpdateTable() {
if(m_table != NULL){
if (Get() == kOn) {
m_table->PutString("Value", "On");
}
else if (Get() == kForward) {
m_table->PutString("Value", "Forward");
}
else if (Get() == kReverse) {
m_table->PutString("Value", "Reverse");
}
else {
m_table->PutString("Value", "Off");
}
}
}
void Relay::StartLiveWindowMode() {
if(m_table != NULL){
m_table->AddTableListener("Value", this, true);
}
}
void Relay::StopLiveWindowMode() {
if(m_table != NULL){
m_table->RemoveTableListener(this);
}
}
std::string Relay::GetSmartDashboardType() {
return "Relay";
}
void Relay::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Relay::GetTable() {
return m_table;
}

120
wpilibc/src/main/native/Resource.cpp
View File

@@ -1,120 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Resource.h"
#include "WPIErrors.h"
#include "ErrorBase.h"
ReentrantSemaphore Resource::m_createLock;
/**
* Allocate storage for a new instance of Resource.
* Allocate a bool array of values that will get initialized to indicate that no resources
* have been allocated yet. The indicies of the resources are [0 .. elements - 1].
*/
Resource::Resource(uint32_t elements)
{
Synchronized sync(m_createLock);
m_size = elements;
m_isAllocated = new bool[m_size];
for (uint32_t i=0; i < m_size; i++)
{
m_isAllocated[i] = false;
}
}
/**
* Factory method to create a Resource allocation-tracker *if* needed.
*
* @param r -- address of the caller's Resource pointer. If *r == NULL, this
* will construct a Resource and make *r point to it. If *r != NULL, i.e.
* the caller already has a Resource instance, this won't do anything.
* @param elements -- the number of elements for this Resource allocator to
* track, that is, it will allocate resource numbers in the range
* [0 .. elements - 1].
*/
/*static*/ void Resource::CreateResourceObject(Resource **r, uint32_t elements)
{
Synchronized sync(m_createLock);
if (*r == NULL)
{
*r = new Resource(elements);
}
}
/**
* Delete the allocated array or resources.
* This happens when the module is unloaded (provided it was statically allocated).
*/
Resource::~Resource()
{
delete[] m_isAllocated;
}
/**
* Allocate a resource.
* When a resource is requested, mark it allocated. In this case, a free resource value
* within the range is located and returned after it is marked allocated.
*/
uint32_t Resource::Allocate(const char *resourceDesc)
{
Synchronized sync(m_allocateLock);
for (uint32_t i=0; i < m_size; i++)
{
if (!m_isAllocated[i])
{
m_isAllocated[i] = true;
return i;
}
}
wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc);
return ~0ul;
}
/**
* 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;
}
/**
* 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;
}

202
wpilibc/src/main/native/RobotBase.cpp
View File

@@ -1,202 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "RobotBase.h"
#include "DriverStation.h"
//#include "NetworkCommunication/FRCComm.h"
//#include "NetworkCommunication/symModuleLink.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include <cstring>
#include "HAL/HAL.h"
#ifdef __vxworks
// VXWorks needs som special unloading code
#include <moduleLib.h>
#include <unldLib.h>
#include <taskLib.h>
#endif
RobotBase* RobotBase::m_instance = NULL;
void RobotBase::setInstance(RobotBase* robot)
{
wpi_assert(m_instance == NULL);
m_instance = robot;
}
RobotBase &RobotBase::getInstance()
{
return *m_instance;
}
/**
* Constructor for a generic robot program.
* User code should be placed in the constuctor that runs before the Autonomous or Operator
* Control period starts. The constructor will run to completion before Autonomous is entered.
*
* This must be used to ensure that the communications code starts. In the future it would be
* nice to put this code into it's own task that loads on boot so ensure that it runs.
*/
RobotBase::RobotBase()
: m_task (NULL)
, m_ds (NULL)
{
m_ds = DriverStation::GetInstance();
}
/**
* Free the resources for a RobotBase class.
* This includes deleting all classes that might have been allocated as Singletons to they
* would never be deleted except here.
*/
RobotBase::~RobotBase()
{
SensorBase::DeleteSingletons();
delete m_task;
m_task = NULL;
m_instance = NULL;
}
/**
* Determine if the Robot is currently enabled.
* @return True if the Robot is currently enabled by the field controls.
*/
bool RobotBase::IsEnabled()
{
return m_ds->IsEnabled();
}
/**
* Determine if the Robot is currently disabled.
* @return True if the Robot is currently disabled by the field controls.
*/
bool RobotBase::IsDisabled()
{
return m_ds->IsDisabled();
}
/**
* Determine if the robot is currently in Autnomous mode.
* @return True if the robot is currently operating Autonomously as determined by the field controls.
*/
bool RobotBase::IsAutonomous()
{
return m_ds->IsAutonomous();
}
/**
* Determine if the robot is currently in Operator Control mode.
* @return True if the robot is currently operating in Tele-Op mode as determined by the field controls.
*/
bool RobotBase::IsOperatorControl()
{
return m_ds->IsOperatorControl();
}
/**
* Determine if the robot is currently in Test mode.
* @return True if the robot is currently running tests as determined by the field controls.
*/
bool RobotBase::IsTest()
{
return m_ds->IsTest();
}
/**
* Indicates if new data is available from the driver station.
* @return Has new data arrived over the network since the last time this function was called?
*/
bool RobotBase::IsNewDataAvailable()
{
return m_ds->IsNewControlData();
}
/**
* Static interface that will start the competition in the new task.
*/
void RobotBase::robotTask(FUNCPTR factory, Task *task)
{
RobotBase::setInstance((RobotBase*)factory());
RobotBase::getInstance().m_task = task;
RobotBase::getInstance().StartCompetition();
}
/**
*
* Start the robot code.
* This function starts the robot code running by spawning a task. Currently tasks seemed to be
* started by LVRT without setting the VX_FP_TASK flag so floating point context is not saved on
* interrupts. Therefore the program experiences hard to debug and unpredictable results. So the
* LVRT code starts this function, and it, in turn, starts the actual user program.
*/
void RobotBase::startRobotTask(FUNCPTR factory)
{
#ifdef SVN_REV
if (strlen(SVN_REV))
{
printf("WPILib was compiled from SVN revision %s\n", SVN_REV);
}
else
{
printf("WPILib was compiled from a location that is not source controlled.\n");
}
#else
printf("WPILib was compiled without -D'SVN_REV=nnnn'\n");
#endif
#ifdef __vxworks
// Check for startup code already running
int32_t oldId = taskNameToId(const_cast<char*>("FRC_RobotTask"));
if (oldId != ERROR)
{
// Find the startup code module.
char moduleName[256];
moduleNameFindBySymbolName("FRC_UserProgram_StartupLibraryInit", moduleName);
MODULE_ID startupModId = moduleFindByName(moduleName);
if (startupModId != NULL)
{
// Remove the startup code.
unldByModuleId(startupModId, 0);
printf("!!! Error: Default code was still running... It was unloaded for you... Please try again.\n");
return;
}
// This case should no longer get hit.
printf("!!! Error: Other robot code is still running... Unload it and then try again.\n");
return;
}
#endif
// Let the framework know that we are starting a new user program so the Driver Station can disable.
HALNetworkCommunicationObserveUserProgramStarting();
// Let the Usage Reporting framework know that there is a C++ program running
HALReport(HALUsageReporting::kResourceType_Language, HALUsageReporting::kLanguage_CPlusPlus);
// Start robot task
// This is done to ensure that the C++ robot task is spawned with the floating point
// context save parameter.
Task *task = new Task("RobotTask", (FUNCPTR)RobotBase::robotTask, Task::kDefaultPriority, 64000);
task->Start((int32_t)factory, (int32_t)task);
}
/**
* This class exists for the sole purpose of getting its destructor called when the module unloads.
* Before the module is done unloading, we need to delete the RobotBase derived singleton. This should delete
* the other remaining singletons that were registered. This should also stop all tasks that are using
* the Task class.
*/
class RobotDeleter
{
public:
RobotDeleter() {}
~RobotDeleter()
{
delete &RobotBase::getInstance();
}
};
static RobotDeleter g_robotDeleter;

738
wpilibc/src/main/native/RobotDrive.cpp
View File

@@ -1,738 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "RobotDrive.h"
#include "CANJaguar.h"
#include "GenericHID.h"
#include "Joystick.h"
#include "Jaguar.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <math.h>
#undef max
#include <algorithm>
const int32_t RobotDrive::kMaxNumberOfMotors;
/*
* Driving functions
* These functions provide an interface to multiple motors that is used for C programming
* The Drive(speed, direction) function is the main part of the set that makes it easy
* to set speeds and direction independently in one call.
*/
/**
* Common function to initialize all the robot drive constructors.
* Create a motor safety object (the real reason for the common code) and
* initialize all the motor assignments. The default timeout is set for the robot drive.
*/
void RobotDrive::InitRobotDrive() {
m_frontLeftMotor = NULL;
m_frontRightMotor = NULL;
m_rearRightMotor = NULL;
m_rearLeftMotor = NULL;
m_sensitivity = 0.5;
m_maxOutput = 1.0;
m_safetyHelper = new MotorSafetyHelper(this);
m_safetyHelper->SetSafetyEnabled(true);
}
/** Constructor for RobotDrive with 2 motors specified with channel numbers.
* Set up parameters for a two wheel drive system where the
* left and right motor pwm channels are specified in the call.
* This call assumes Jaguars for controlling the motors.
* @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor.
* @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor.
*/
RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
{
InitRobotDrive();
m_rearLeftMotor = new Jaguar(leftMotorChannel);
m_rearRightMotor = new Jaguar(rightMotorChannel);
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
/**
* Constructor for RobotDrive with 4 motors specified with channel numbers.
* Set up parameters for a four wheel drive system where all four motor
* pwm channels are specified in the call.
* This call assumes Jaguars for controlling the motors.
* @param frontLeftMotor Front left motor channel number on the default digital module
* @param rearLeftMotor Rear Left motor channel number on the default digital module
* @param frontRightMotor Front right motor channel number on the default digital module
* @param rearRightMotor Rear Right motor channel number on the default digital module
*/
RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
uint32_t frontRightMotor, uint32_t rearRightMotor)
{
InitRobotDrive();
m_rearLeftMotor = new Jaguar(rearLeftMotor);
m_rearRightMotor = new Jaguar(rearRightMotor);
m_frontLeftMotor = new Jaguar(frontLeftMotor);
m_frontRightMotor = new Jaguar(frontRightMotor);
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
/**
* Constructor for RobotDrive with 2 motors specified as SpeedController objects.
* The SpeedController version of the constructor enables programs to use the RobotDrive classes with
* subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
* the curve to suit motor bias or deadband elimination.
* @param leftMotor The left SpeedController object used to drive the robot.
* @param rightMotor the right SpeedController object used to drive the robot.
*/
RobotDrive::RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor)
{
InitRobotDrive();
if (leftMotor == NULL || rightMotor == NULL)
{
wpi_setWPIError(NullParameter);
m_rearLeftMotor = m_rearRightMotor = NULL;
return;
}
m_rearLeftMotor = leftMotor;
m_rearRightMotor = rightMotor;
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
m_deleteSpeedControllers = false;
}
RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor)
{
InitRobotDrive();
m_rearLeftMotor = &leftMotor;
m_rearRightMotor = &rightMotor;
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
m_deleteSpeedControllers = false;
}
/**
* Constructor for RobotDrive with 4 motors specified as SpeedController objects.
* Speed controller input version of RobotDrive (see previous comments).
* @param rearLeftMotor The back left SpeedController object used to drive the robot.
* @param frontLeftMotor The front left SpeedController object used to drive the robot
* @param rearRightMotor The back right SpeedController object used to drive the robot.
* @param frontRightMotor The front right SpeedController object used to drive the robot.
*/
RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
SpeedController *frontRightMotor, SpeedController *rearRightMotor)
{
InitRobotDrive();
if (frontLeftMotor == NULL || rearLeftMotor == NULL || frontRightMotor == NULL || rearRightMotor == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
m_frontLeftMotor = frontLeftMotor;
m_rearLeftMotor = rearLeftMotor;
m_frontRightMotor = frontRightMotor;
m_rearRightMotor = rearRightMotor;
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
m_deleteSpeedControllers = false;
}
RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
SpeedController &frontRightMotor, SpeedController &rearRightMotor)
{
InitRobotDrive();
m_frontLeftMotor = &frontLeftMotor;
m_rearLeftMotor = &rearLeftMotor;
m_frontRightMotor = &frontRightMotor;
m_rearRightMotor = &rearRightMotor;
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
m_deleteSpeedControllers = false;
}
/**
* RobotDrive destructor.
* Deletes motor objects that were not passed in and created internally only.
**/
RobotDrive::~RobotDrive()
{
if (m_deleteSpeedControllers)
{
delete m_frontLeftMotor;
delete m_rearLeftMotor;
delete m_frontRightMotor;
delete m_rearRightMotor;
}
delete m_safetyHelper;
}
/**
* Drive the motors at "speed" and "curve".
*
* The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and
* not turning. The algorithm for adding in the direction attempts to provide a constant
* turn radius for differing speeds.
*
* This function will most likely be used in an autonomous routine.
*
* @param outputMagnitude The forward component of the output magnitude to send to the motors.
* @param curve The rate of turn, constant for different forward speeds.
*/
void RobotDrive::Drive(float outputMagnitude, float curve)
{
float leftOutput, rightOutput;
static bool reported = false;
if (!reported)
{
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(), HALUsageReporting::kRobotDrive_ArcadeRatioCurve);
reported = true;
}
if (curve < 0)
{
float value = log(-curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
}
else if (curve > 0)
{
float value = log(curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
}
else
{
leftOutput = outputMagnitude;
rightOutput = outputMagnitude;
}
SetLeftRightMotorOutputs(leftOutput, rightOutput);
}
/**
* 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 == NULL || rightStick == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
}
void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs)
{
TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration.
* This function lets you pick the axis to be used on each Joystick object for the left
* and right sides of the robot.
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
* @param rightStick The Joystick object to use for the right side of the robot.
* @param rightAxis The axis to select on the right side Joystick object.
*/
void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis,
GenericHID *rightStick, uint32_t rightAxis, bool squaredInputs)
{
if (leftStick == NULL || rightStick == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis), squaredInputs);
}
void RobotDrive::TankDrive(GenericHID &leftStick, uint32_t leftAxis,
GenericHID &rightStick, uint32_t rightAxis, bool squaredInputs)
{
TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration.
* This function lets you directly provide joystick values from any source.
* @param leftValue The value of the left stick.
* @param rightValue The value of the right stick.
*/
void RobotDrive::TankDrive(float leftValue, float rightValue, bool squaredInputs)
{
static bool reported = false;
if (!reported)
{
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(), HALUsageReporting::kRobotDrive_Tank);
reported = true;
}
// square the inputs (while preserving the sign) to increase fine control while permitting full power
leftValue = Limit(leftValue);
rightValue = Limit(rightValue);
if(squaredInputs)
{
if (leftValue >= 0.0)
{
leftValue = (leftValue * leftValue);
}
else
{
leftValue = -(leftValue * leftValue);
}
if (rightValue >= 0.0)
{
rightValue = (rightValue * rightValue);
}
else
{
rightValue = -(rightValue * rightValue);
}
}
SetLeftRightMotorOutputs(leftValue, rightValue);
}
/**
* Arcade drive implements single stick driving.
* Given a single Joystick, the class assumes the Y axis for the move value and the X axis
* for the rotate value.
* (Should add more information here regarding the way that arcade drive works.)
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
* for forwards/backwards and the X-axis will be selected for rotation rate.
* @param squaredInputs If true, the sensitivity will be increased for small values
*/
void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs)
{
// simply call the full-featured ArcadeDrive with the appropriate values
ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
}
/**
* Arcade drive implements single stick driving.
* Given a single Joystick, the class assumes the Y axis for the move value and the X axis
* for the rotate value.
* (Should add more information here regarding the way that arcade drive works.)
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
* for forwards/backwards and the X-axis will be selected for rotation rate.
* @param squaredInputs If true, the sensitivity will be increased for small values
*/
void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs)
{
// simply call the full-featured ArcadeDrive with the appropriate values
ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
}
/**
* Arcade drive implements single stick driving.
* Given two joystick instances and two axis, compute the values to send to either two
* or four motors.
* @param moveStick The Joystick object that represents the forward/backward direction
* @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
* @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
*/
void RobotDrive::ArcadeDrive(GenericHID* moveStick, uint32_t moveAxis,
GenericHID* rotateStick, uint32_t rotateAxis,
bool squaredInputs)
{
float moveValue = moveStick->GetRawAxis(moveAxis);
float rotateValue = rotateStick->GetRawAxis(rotateAxis);
ArcadeDrive(moveValue, rotateValue, squaredInputs);
}
/**
* Arcade drive implements single stick driving.
* Given two joystick instances and two axis, compute the values to send to either two
* or four motors.
* @param moveStick The Joystick object that represents the forward/backward direction
* @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
* @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
*/
void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis,
GenericHID &rotateStick, uint32_t rotateAxis,
bool squaredInputs)
{
float moveValue = moveStick.GetRawAxis(moveAxis);
float rotateValue = rotateStick.GetRawAxis(rotateAxis);
ArcadeDrive(moveValue, rotateValue, squaredInputs);
}
/**
* Arcade drive implements single stick driving.
* This function lets you directly provide joystick values from any source.
* @param moveValue The value to use for fowards/backwards
* @param rotateValue The value to use for the rotate right/left
* @param squaredInputs If set, increases the sensitivity at low speeds
*/
void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs)
{
static bool reported = false;
if (!reported)
{
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(), HALUsageReporting::kRobotDrive_ArcadeStandard);
reported = true;
}
// local variables to hold the computed PWM values for the motors
float leftMotorOutput;
float rightMotorOutput;
moveValue = Limit(moveValue);
rotateValue = Limit(rotateValue);
if (squaredInputs)
{
// square the inputs (while preserving the sign) to increase fine control while permitting full power
if (moveValue >= 0.0)
{
moveValue = (moveValue * moveValue);
}
else
{
moveValue = -(moveValue * moveValue);
}
if (rotateValue >= 0.0)
{
rotateValue = (rotateValue * rotateValue);
}
else
{
rotateValue = -(rotateValue * rotateValue);
}
}
if (moveValue > 0.0)
{
if (rotateValue > 0.0)
{
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = std::max(moveValue, rotateValue);
}
else
{
leftMotorOutput = std::max(moveValue, -rotateValue);
rightMotorOutput = moveValue + rotateValue;
}
}
else
{
if (rotateValue > 0.0)
{
leftMotorOutput = - std::max(-moveValue, rotateValue);
rightMotorOutput = moveValue + rotateValue;
}
else
{
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = - std::max(-moveValue, -rotateValue);
}
}
SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45 degrees.
* When looking at the wheels from the top, the roller axles should form an X across the robot.
*
* This is designed to be directly driven by joystick axes.
*
* @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely independent of
* the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls.
*/
void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle)
{
static bool reported = false;
if (!reported)
{
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(), HALUsageReporting::kRobotDrive_MecanumCartesian);
reported = true;
}
double xIn = x;
double yIn = y;
// Negate y for the joystick.
yIn = -yIn;
// Compenstate for gyro angle.
RotateVector(xIn, yIn, gyroAngle);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
CANJaguar::UpdateSyncGroup(syncGroup);
m_safetyHelper->Feed();
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45 degrees.
* When looking at the wheels from the top, the roller axles should form an X across the robot.
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The direction the robot should drive in degrees. The direction and maginitute are
* independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely independent of
* the magnitute or direction. [-1.0..1.0]
*/
void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation)
{
static bool reported = false;
if (!reported)
{
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(), HALUsageReporting::kRobotDrive_MecanumPolar);
reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * sqrt(2.0);
// The rollers are at 45 degree angles.
double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
double cosD = cos(dirInRad);
double sinD = sin(dirInRad);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
CANJaguar::UpdateSyncGroup(syncGroup);
m_safetyHelper->Feed();
}
/**
* Holonomic Drive method for Mecanum wheeled robots.
*
* This is an alias to MecanumDrive_Polar() for backward compatability
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The direction the robot should drive. The direction and maginitute are
* independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely independent of
* the magnitute or direction. [-1.0..1.0]
*/
void RobotDrive::HolonomicDrive(float magnitude, float direction, float rotation)
{
MecanumDrive_Polar(magnitude, direction, rotation);
}
/** Set the speed of the right and left motors.
* This is used once an appropriate drive setup function is called such as
* TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
* and includes flipping the direction of one side for opposing motors.
* @param leftOutput The speed to send to the left side of the robot.
* @param rightOutput The speed to send to the right side of the robot.
*/
void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput)
{
wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL);
uint8_t syncGroup = 0x80;
if (m_frontLeftMotor != NULL)
m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
if (m_frontRightMotor != NULL)
m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
CANJaguar::UpdateSyncGroup(syncGroup);
m_safetyHelper->Feed();
}
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
float RobotDrive::Limit(float num)
{
if (num > 1.0)
{
return 1.0;
}
if (num < -1.0)
{
return -1.0;
}
return num;
}
/**
* Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
*/
void RobotDrive::Normalize(double *wheelSpeeds)
{
double maxMagnitude = fabs(wheelSpeeds[0]);
int32_t i;
for (i=1; i<kMaxNumberOfMotors; i++)
{
double temp = fabs(wheelSpeeds[i]);
if (maxMagnitude < temp) maxMagnitude = temp;
}
if (maxMagnitude > 1.0)
{
for (i=0; i<kMaxNumberOfMotors; i++)
{
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
}
}
}
/**
* Rotate a vector in Cartesian space.
*/
void RobotDrive::RotateVector(double &x, double &y, double angle)
{
double cosA = cos(angle * (3.14159 / 180.0));
double sinA = sin(angle * (3.14159 / 180.0));
double xOut = x * cosA - y * sinA;
double yOut = x * sinA + y * cosA;
x = xOut;
y = yOut;
}
/*
* 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;
}
/**
* Set the turning sensitivity.
*
* This only impacts the Drive() entry-point.
* @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
*/
void RobotDrive::SetSensitivity(float sensitivity)
{
m_sensitivity = sensitivity;
}
/**
* Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
* @param maxOutput Multiplied with the output percentage computed by the drive functions.
*/
void RobotDrive::SetMaxOutput(double maxOutput)
{
m_maxOutput = maxOutput;
}
void RobotDrive::SetExpiration(float timeout)
{
m_safetyHelper->SetExpiration(timeout);
}
float RobotDrive::GetExpiration()
{
return m_safetyHelper->GetExpiration();
}
bool RobotDrive::IsAlive()
{
return m_safetyHelper->IsAlive();
}
bool RobotDrive::IsSafetyEnabled()
{
return m_safetyHelper->IsSafetyEnabled();
}
void RobotDrive::SetSafetyEnabled(bool enabled)
{
m_safetyHelper->SetSafetyEnabled(enabled);
}
void RobotDrive::GetDescription(char *desc)
{
sprintf(desc, "RobotDrive");
}
void RobotDrive::StopMotor()
{
if (m_frontLeftMotor != NULL) m_frontLeftMotor->Disable();
if (m_frontRightMotor != NULL) m_frontRightMotor->Disable();
if (m_rearLeftMotor != NULL) m_rearLeftMotor->Disable();
if (m_rearRightMotor != NULL) m_rearRightMotor->Disable();
}

424
wpilibc/src/main/native/SPI.cpp
View File

@@ -1,424 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SPI.h"
#include "DigitalModule.h"
#include "DigitalInput.h"
#include "DigitalOutput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "WPIErrors.h"
#include <math.h>
/**
* Constructor for input and output.
*
* @param clk The digital output for the clock signal.
* @param mosi The digital output for the written data to the slave
* (master-out slave-in).
* @param miso The digital input for the input data from the slave
* (master-in slave-out).
*/
SPI::SPI(DigitalOutput &clk, DigitalOutput &mosi, DigitalInput &miso)
{
Init(&clk, &mosi, &miso);
}
/**
* Constructor for input and output.
*
* @param clk The digital output for the clock signal.
* @param mosi The digital output for the written data to the slave
* (master-out slave-in).
* @param miso The digital input for the input data from the slave
* (master-in slave-out).
*/
SPI::SPI(DigitalOutput *clk, DigitalOutput *mosi, DigitalInput *miso)
{
Init(clk, mosi, miso);
}
/**
* Constructor for output only.
*
* @param clk The digital output for the clock signal.
* @param mosi The digital output for the written data to the slave
* (master-out slave-in).
*/
SPI::SPI(DigitalOutput &clk, DigitalOutput &mosi)
{
Init(&clk, &mosi, NULL);
}
/**
* Constructor for output only.
*
* @param clk The digital output for the clock signal.
* @param mosi The digital output for the written data to the slave
* (master-out slave-in).
*/
SPI::SPI(DigitalOutput *clk, DigitalOutput *mosi)
{
Init(clk, mosi, NULL);
}
/**
* Constructor for input only.
*
* @param clk The digital output for the clock signal.
* @param miso The digital input for the input data from the slave
* (master-in slave-out).
*/
SPI::SPI(DigitalOutput &clk, DigitalInput &miso)
{
Init(&clk, NULL, &miso);
}
/**
* Constructor for input only.
*
* @param clk The digital output for the clock signal.
* @param miso The digital input for the input data from the slave
* (master-in slave-out).
*/
SPI::SPI(DigitalOutput *clk, DigitalInput *miso)
{
Init(clk, NULL, miso);
}
/**
* Destructor.
*/
SPI::~SPI()
{
int32_t status = 0;
cleanSPI(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Initialize SPI channel configuration.
*
* @param clk The digital output for the clock signal.
* @param mosi The digital output for the written data to the slave
* (master-out slave-in).
* @param miso The digital input for the input data from the slave
* (master-in slave-out).
*/
void SPI::Init(DigitalOutput *clk, DigitalOutput *mosi, DigitalInput *miso)
{
int32_t status = 0;
m_spi = initializeSPI(clk->GetModuleForRouting(), clk->GetChannelForRouting(),
mosi->GetModuleForRouting(), mosi->GetChannelForRouting(),
miso->GetModuleForRouting(), miso->GetChannelForRouting(), &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_ss = NULL;
static int32_t instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_SPI, instances);
}
/**
* Configure the number of bits from each word that the slave transmits
* or receives.
*
* @param bits The number of bits in one frame (1 to 32 bits).
*/
void SPI::SetBitsPerWord(uint32_t bits)
{
int32_t status = 0;
setSPIBitsPerWord(m_spi, bits, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the number of bits from each word that the slave transmits
* or receives.
*
* @return The number of bits in one frame (1 to 32 bits).
*/
uint32_t SPI::GetBitsPerWord()
{
int32_t status = 0;
uint32_t bits = getSPIBitsPerWord(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return bits;
}
/**
* Configure the rate of the generated clock signal.
* The default and maximum value is 76,628.4 Hz.
*
* @param hz The clock rate in Hertz.
*/
void SPI::SetClockRate(double hz)
{
int32_t status = 0;
setSPIClockRate(m_spi, hz, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the order that bits are sent and received on the wire
* to be most significant bit first.
*/
void SPI::SetMSBFirst()
{
int32_t status = 0;
setSPIMSBFirst(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the order that bits are sent and received on the wire
* to be least significant bit first.
*/
void SPI::SetLSBFirst()
{
int32_t status = 0;
setSPILSBFirst(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure that the data is stable on the falling edge and the data
* changes on the rising edge.
*/
void SPI::SetSampleDataOnFalling()
{
int32_t status = 0;
setSPISampleDataOnFalling(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure that the data is stable on the rising edge and the data
* changes on the falling edge.
*/
void SPI::SetSampleDataOnRising()
{
int32_t status = 0;
setSPISampleDataOnRising(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the slave select line behavior.
*
* @param ss slave select digital output.
* @param mode Frame mode:
* kChipSelect: active for the duration of the frame.
* kPreLatchPulse: pulses before the transfer of each frame.
* kPostLatchPulse: pulses after the transfer of each frame.
* kPreAndPostLatchPulse: pulses before and after each frame.
* @param activeLow True if slave select line is active low.
*/
void SPI::SetSlaveSelect(DigitalOutput *ss, tFrameMode mode, bool activeLow)
{
int32_t status = 0;
if (ss)
{
setSPISlaveSelect(m_spi, ss->GetModuleForRouting(), ss->GetChannelForRouting(), &status);
}
else
{
setSPISlaveSelect(m_spi, 0, 0, &status);
}
m_ss = ss;
setSPILatchMode(m_spi, mode, &status);
setSPIFramePolarity(m_spi, activeLow, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the slave select line behavior.
*
* @param ss slave select digital output.
* @param mode Frame mode:
* kChipSelect: active for the duration of the frame.
* kPreLatchPulse: pulses before the transfer of each frame.
* kPostLatchPulse: pulses after the transfer of each frame.
* kPreAndPostLatchPulse: pulses before and after each frame.
* @param activeLow True if slave select line is active low.
*/
void SPI::SetSlaveSelect(DigitalOutput &ss, tFrameMode mode, bool activeLow)
{
SetSlaveSelect(&ss, mode, activeLow);
}
/**
* Get the slave select line behavior.
*
* @param mode Frame mode:
* kChipSelect: active for the duration of the frame.
* kPreLatchPulse: pulses before the transfer of each frame.
* kPostLatchPulse: pulses after the transfer of each frame.
* kPreAndPostLatchPulse: pulses before and after each frame.
* @param activeLow True if slave select line is active low.
* @return The slave select digital output.
*/
DigitalOutput *SPI::GetSlaveSelect(tFrameMode *mode, bool *activeLow)
{
int32_t status = 0;
if (mode != NULL)
{
*mode = getSPILatchMode(m_spi, &status);
}
if (activeLow != NULL)
{
*activeLow = getSPIFramePolarity(m_spi, &status);
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return m_ss;
}
/**
* Configure the clock output line to be active low.
* This is sometimes called clock polarity high.
*/
void SPI::SetClockActiveLow()
{
int32_t status = 0;
setSPIClockActiveLow(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Configure the clock output line to be active high.
* This is sometimes called clock polarity low.
*/
void SPI::SetClockActiveHigh()
{
int32_t status = 0;
setSPIClockActiveHigh(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Apply configuration settings and reset the SPI logic.
*/
void SPI::ApplyConfig()
{
int32_t status = 0;
applySPIConfig(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the number of words that can currently be stored before being
* transmitted to the device.
*
* @return The number of words available to be written.
*/
uint16_t SPI::GetOutputFIFOAvailable()
{
int32_t status = 0;
uint16_t result = getSPIOutputFIFOAvailable(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* Get the number of words received and currently available to be read from
* the receive FIFO.
*
* @return The number of words available to read.
*/
uint16_t SPI::GetNumReceived()
{
int32_t status = 0;
uint16_t result = getSPINumReceived(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* Have all pending transfers completed?
*
* @return True if no transfers are pending.
*/
bool SPI::IsDone()
{
int32_t status = 0;
bool result = isSPIDone(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* Determine if the receive FIFO was full when attempting to add new data at
* end of a transfer.
*
* @return True if the receive FIFO overflowed.
*/
bool SPI::HadReceiveOverflow()
{
int32_t status = 0;
bool result = hadSPIReceiveOverflow(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return result;
}
/**
* Write a word to the slave device. Blocks until there is space in the
* output FIFO.
*
* If not running in output only mode, also saves the data received
* on the MISO input during the transfer into the receive FIFO.
*/
void SPI::Write(uint32_t data)
{
int32_t status = 0;
writeSPI(m_spi, data, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Read a word from the receive FIFO.
*
* Waits for the current transfer to complete if the receive FIFO is empty.
*
* If the receive FIFO is empty, there is no active transfer, and initiate
* is false, errors.
*
* @param initiate If true, this function pushes "0" into the
* transmit buffer and initiates a transfer.
* If false, this function assumes that data is
* already in the receive FIFO from a previous write.
*/
uint32_t SPI::Read(bool initiate)
{
int32_t status = 0;
uint32_t value = readSPI(m_spi, initiate, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return value;
}
/**
* Stop any transfer in progress and empty the transmit FIFO.
*/
void SPI::Reset()
{
int32_t status = 0;
resetSPI(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Empty the receive FIFO.
*/
void SPI::ClearReceivedData()
{
int32_t status = 0;
clearSPIReceivedData(m_spi, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

117
wpilibc/src/main/native/SafePWM.cpp
View File

@@ -1,117 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SafePWM.h"
#include "MotorSafetyHelper.h"
/**
* Initialize a SafePWM object by setting defaults
*/
void SafePWM::InitSafePWM()
{
m_safetyHelper = new MotorSafetyHelper(this);
m_safetyHelper->SetSafetyEnabled(false);
}
/**
* Constructor for a SafePWM object taking a channel number
* @param channel The channel number to be used for the underlying PWM object
*/
SafePWM::SafePWM(uint32_t channel): PWM(channel)
{
InitSafePWM();
}
/**
* Constructor for a SafePWM object taking channel and slot numbers.
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel number on the module (1..10).
*/
SafePWM::SafePWM(uint8_t moduleNumber, uint32_t channel): PWM(moduleNumber, channel)
{
InitSafePWM();
}
SafePWM::~SafePWM()
{
delete m_safetyHelper;
}
/*
* Set the expiration time for the PWM object
* @param timeout The timeout (in seconds) for this motor object
*/
void SafePWM::SetExpiration(float timeout)
{
m_safetyHelper->SetExpiration(timeout);
}
/**
* Return the expiration time for the PWM object.
* @returns The expiration time value.
*/
float SafePWM::GetExpiration()
{
return m_safetyHelper->GetExpiration();
}
/**
* Check if the PWM object is currently alive or stopped due to a timeout.
* @returns a bool value that is true if the motor has NOT timed out and should still
* be running.
*/
bool SafePWM::IsAlive()
{
return m_safetyHelper->IsAlive();
}
/**
* Stop the motor associated with this PWM object.
* This is called by the MotorSafetyHelper object when it has a timeout for this PWM and needs to
* stop it from running.
*/
void SafePWM::StopMotor()
{
SetRaw(kPwmDisabled);
}
/**
* Enable/disable motor safety for this device
* Turn on and off the motor safety option for this PWM object.
* @param enabled True if motor safety is enforced for this object
*/
void SafePWM::SetSafetyEnabled(bool enabled)
{
m_safetyHelper->SetSafetyEnabled(enabled);
}
/**
* Check if motor safety is enabled for this object
* @returns True if motor safety is enforced for this object
*/
bool SafePWM::IsSafetyEnabled()
{
return m_safetyHelper->IsSafetyEnabled();
}
void SafePWM::GetDescription(char *desc)
{
sprintf(desc, "PWM %ld on module %ld", GetChannel(), GetModuleNumber());
}
/**
* Feed the MotorSafety timer when setting the speed.
* This method is called by the subclass motor whenever it updates its speed, thereby reseting
* the timeout value.
* @param speed Value to pass to the PWM class
*/
void SafePWM::SetSpeed(float speed)
{
PWM::SetSpeed(speed);
m_safetyHelper->Feed();
}

191
wpilibc/src/main/native/SensorBase.cpp
View File

@@ -1,191 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SensorBase.h"
#include "NetworkCommunication/LoadOut.h"
#include "WPIErrors.h"
const uint32_t SensorBase::kDigitalChannels;
const uint32_t SensorBase::kAnalogChannels;
const uint32_t SensorBase::kAnalogModules;
const uint32_t SensorBase::kDigitalModules;
const uint32_t SensorBase::kSolenoidChannels;
const uint32_t SensorBase::kSolenoidModules;
const uint32_t SensorBase::kPwmChannels;
const uint32_t SensorBase::kRelayChannels;
const uint32_t SensorBase::kChassisSlots;
SensorBase *SensorBase::m_singletonList = NULL;
/**
* Creates an instance of the sensor base and gets an FPGA handle
*/
SensorBase::SensorBase()
{
}
/**
* Frees the resources for a SensorBase.
*/
SensorBase::~SensorBase()
{
}
/**
* Add sensor to the singleton list.
* Add this sensor to the list of singletons that need to be deleted when
* the robot program exits. Each of the sensors on this list are singletons,
* that is they aren't allocated directly with new, but instead are allocated
* by the static GetInstance method. As a result, they are never deleted when
* the program exits. Consequently these sensors may still be holding onto
* resources and need to have their destructors called at the end of the program.
*/
void SensorBase::AddToSingletonList()
{
m_nextSingleton = m_singletonList;
m_singletonList = this;
}
/**
* Delete all the singleton classes on the list.
* All the classes that were allocated as singletons need to be deleted so
* their resources can be freed.
*/
void SensorBase::DeleteSingletons()
{
for (SensorBase *next = m_singletonList; next != NULL;)
{
SensorBase *tmp = next;
next = next->m_nextSingleton;
delete tmp;
}
m_singletonList = NULL;
}
/**
* Check that the analog module number is valid.
*
* @return Analog module is valid and present
*/
bool SensorBase::CheckAnalogModule(uint8_t moduleNumber)
{
if (nLoadOut::getModulePresence(nLoadOut::kModuleType_Analog, moduleNumber - 1))
return true;
return false;
}
/**
* Check that the digital module number is valid.
*
* @return Digital module is valid and present
*/
bool SensorBase::CheckDigitalModule(uint8_t moduleNumber)
{
if (nLoadOut::getModulePresence(nLoadOut::kModuleType_Digital, moduleNumber - 1))
return true;
return false;
}
/**
* Check that the digital module number is valid.
*
* @return Digital module is valid and present
*/
bool SensorBase::CheckPWMModule(uint8_t moduleNumber)
{
return CheckDigitalModule(moduleNumber);
}
/**
* Check that the digital module number is valid.
*
* @return Digital module is valid and present
*/
bool SensorBase::CheckRelayModule(uint8_t moduleNumber)
{
return CheckDigitalModule(moduleNumber);
}
/**
* Check that the solenoid module number is valid.
*
* @return Solenoid module is valid and present
*/
bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber)
{
if (nLoadOut::getModulePresence(nLoadOut::kModuleType_Solenoid, moduleNumber - 1))
return true;
return false;
}
/**
* Check that the digital channel number is valid.
* Verify that the channel number is one of the legal channel numbers. Channel numbers are
* 1-based.
*
* @return Digital channel is valid
*/
bool SensorBase::CheckDigitalChannel(uint32_t channel)
{
if (channel > 0 && channel <= kDigitalChannels)
return true;
return false;
}
/**
* Check that the digital channel number is valid.
* Verify that the channel number is one of the legal channel numbers. Channel numbers are
* 1-based.
*
* @return Relay channel is valid
*/
bool SensorBase::CheckRelayChannel(uint32_t channel)
{
if (channel > 0 && channel <= kRelayChannels)
return true;
return false;
}
/**
* Check that the digital channel number is valid.
* Verify that the channel number is one of the legal channel numbers. Channel numbers are
* 1-based.
*
* @return PWM channel is valid
*/
bool SensorBase::CheckPWMChannel(uint32_t channel)
{
if (channel > 0 && channel <= kPwmChannels)
return true;
return false;
}
/**
* Check that the analog channel number is value.
* Verify that the analog channel number is one of the legal channel numbers. Channel numbers
* are 1-based.
*
* @return Analog channel is valid
*/
bool SensorBase::CheckAnalogChannel(uint32_t channel)
{
if (channel > 0 && channel <= kAnalogChannels)
return true;
return false;
}
/**
* Verify that the solenoid channel number is within limits.
*
* @return Solenoid channel is valid
*/
bool SensorBase::CheckSolenoidChannel(uint32_t channel)
{
if (channel > 0 && channel <= kSolenoidChannels)
return true;
return false;
}

332
wpilibc/src/main/native/SerialPort.cpp
View File

@@ -1,332 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SerialPort.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "visa/visa.h"
#include <stdarg.h>
//static ViStatus _VI_FUNCH ioCompleteHandler (ViSession vi, ViEventType eventType, ViEvent event, ViAddr userHandle);
/**
* Create an instance of a Serial Port class.
*
* @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud.
* @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
* @param parity Select the type of parity checking to use.
* @param stopBits The number of stop bits to use as defined by the enum StopBits.
*/
SerialPort::SerialPort(uint32_t baudRate, uint8_t dataBits, SerialPort::Parity parity, SerialPort::StopBits stopBits)
: m_resourceManagerHandle (0)
, m_portHandle (0)
, m_consoleModeEnabled (false)
{
ViStatus localStatus = VI_SUCCESS;
localStatus = viOpenDefaultRM((ViSession*)&m_resourceManagerHandle);
wpi_setError(localStatus);
localStatus = viOpen(m_resourceManagerHandle, const_cast<char*>("ASRL1::INSTR"), VI_NULL, VI_NULL, (ViSession*)&m_portHandle);
wpi_setError(localStatus);
if (localStatus != 0)
{
m_consoleModeEnabled = true;
return;
}
localStatus = viSetAttribute(m_portHandle, VI_ATTR_ASRL_BAUD, baudRate);
wpi_setError(localStatus);
localStatus = viSetAttribute(m_portHandle, VI_ATTR_ASRL_DATA_BITS, dataBits);
wpi_setError(localStatus);
localStatus = viSetAttribute(m_portHandle, VI_ATTR_ASRL_PARITY, parity);
wpi_setError(localStatus);
localStatus = viSetAttribute(m_portHandle, VI_ATTR_ASRL_STOP_BITS, stopBits);
wpi_setError(localStatus);
// Set the default timeout to 5 seconds.
SetTimeout(5.0f);
// Don't wait until the buffer is full to transmit.
SetWriteBufferMode(kFlushOnAccess);
EnableTermination();
//viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler, this);
//viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL);
HALReport(HALUsageReporting::kResourceType_SerialPort, 0);
}
/**
* Destructor.
*/
SerialPort::~SerialPort()
{
if (!m_consoleModeEnabled)
{
//viUninstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler, this);
viClose(m_portHandle);
}
viClose(m_resourceManagerHandle);
}
/**
* Set the type of flow control to enable on this port.
*
* By default, flow control is disabled.
*/
void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl)
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viSetAttribute (m_portHandle, VI_ATTR_ASRL_FLOW_CNTRL, flowControl);
wpi_setError(localStatus);
}
}
/**
* Enable termination and specify the termination character.
*
* Termination is currently only implemented for receive.
* When the the terminator is recieved, the Read() or Scanf() will return
* fewer bytes than requested, stopping after the terminator.
*
* @param terminator The character to use for termination.
*/
void SerialPort::EnableTermination(char terminator)
{
if (!m_consoleModeEnabled)
{
viSetAttribute(m_portHandle, VI_ATTR_TERMCHAR_EN, VI_TRUE);
viSetAttribute(m_portHandle, VI_ATTR_TERMCHAR, terminator);
viSetAttribute(m_portHandle, VI_ATTR_ASRL_END_IN, VI_ASRL_END_TERMCHAR);
}
}
/**
* Disable termination behavior.
*/
void SerialPort::DisableTermination()
{
if (!m_consoleModeEnabled)
{
viSetAttribute(m_portHandle, VI_ATTR_TERMCHAR_EN, VI_FALSE);
viSetAttribute(m_portHandle, VI_ATTR_ASRL_END_IN, VI_ASRL_END_NONE);
}
}
/**
* Get the number of bytes currently available to read from the serial port.
*
* @return The number of bytes available to read.
*/
int32_t SerialPort::GetBytesReceived()
{
int32_t bytes = 0;
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viGetAttribute(m_portHandle, VI_ATTR_ASRL_AVAIL_NUM, &bytes);
wpi_setError(localStatus);
}
return bytes;
}
/**
* Output formatted text to the serial port.
*
* @bug All pointer-based parameters seem to return an error.
*
* @param writeFmt A string that defines the format of the output.
*/
void SerialPort::Printf(const char *writeFmt, ...)
{
if (!m_consoleModeEnabled)
{
va_list args;
va_start (args, writeFmt);
ViStatus localStatus = viVPrintf(m_portHandle, (ViString)writeFmt, args);
va_end (args);
wpi_setError(localStatus);
}
}
/**
* Input formatted text from the serial port.
*
* @bug All pointer-based parameters seem to return an error.
*
* @param readFmt A string that defines the format of the input.
*/
void SerialPort::Scanf(const char *readFmt, ...)
{
if (!m_consoleModeEnabled)
{
va_list args;
va_start (args, readFmt);
ViStatus localStatus = viVScanf(m_portHandle, (ViString)readFmt, args);
va_end (args);
wpi_setError(localStatus);
}
}
/**
* Read raw bytes out of the buffer.
*
* @param buffer Pointer to the buffer to store the bytes in.
* @param count The maximum number of bytes to read.
* @return The number of bytes actually read into the buffer.
*/
uint32_t SerialPort::Read(char *buffer, int32_t count)
{
uint32_t retCount = 0;
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viBufRead(m_portHandle, (ViPBuf)buffer, count, (ViPUInt32)&retCount);
switch (localStatus)
{
case VI_SUCCESS_TERM_CHAR:
case VI_SUCCESS_MAX_CNT:
case VI_ERROR_TMO: // Timeout
break;
default:
wpi_setError(localStatus);
}
}
return retCount;
}
/**
* Write raw bytes to the buffer.
*
* @param buffer Pointer to the buffer to read the bytes from.
* @param count The maximum number of bytes to write.
* @return The number of bytes actually written into the port.
*/
uint32_t SerialPort::Write(const char *buffer, int32_t count)
{
uint32_t retCount = 0;
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viBufWrite(m_portHandle, (ViPBuf)buffer, count, (ViPUInt32)&retCount);
wpi_setError(localStatus);
}
return retCount;
}
/**
* Configure the timeout of the serial port.
*
* This defines the timeout for transactions with the hardware.
* It will affect reads and very large writes.
*
* @param timeout The number of seconds to to wait for I/O.
*/
void SerialPort::SetTimeout(float timeout)
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viSetAttribute(m_portHandle, VI_ATTR_TMO_VALUE, (uint32_t)(timeout * 1e3));
wpi_setError(localStatus);
}
}
/**
* Specify the size of the input buffer.
*
* Specify the amount of data that can be stored before data
* from the device is returned to Read or Scanf. If you want
* data that is recieved to be returned immediately, set this to 1.
*
* It the buffer is not filled before the read timeout expires, all
* data that has been received so far will be returned.
*
* @param size The read buffer size.
*/
void SerialPort::SetReadBufferSize(uint32_t size)
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viSetBuf(m_portHandle, VI_READ_BUF, size);
wpi_setError(localStatus);
}
}
/**
* Specify the size of the output buffer.
*
* Specify the amount of data that can be stored before being
* transmitted to the device.
*
* @param size The write buffer size.
*/
void SerialPort::SetWriteBufferSize(uint32_t size)
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viSetBuf(m_portHandle, VI_WRITE_BUF, size);
wpi_setError(localStatus);
}
}
/**
* Specify the flushing behavior of the output buffer.
*
* When set to kFlushOnAccess, data is synchronously written to the serial port
* after each call to either Printf() or Write().
*
* When set to kFlushWhenFull, data will only be written to the serial port when
* the buffer is full or when Flush() is called.
*
* @param mode The write buffer mode.
*/
void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode)
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viSetAttribute(m_portHandle, VI_ATTR_WR_BUF_OPER_MODE, mode);
wpi_setError(localStatus);
}
}
/**
* Force the output buffer to be written to the port.
*
* This is used when SetWriteBufferMode() is set to kFlushWhenFull to force a
* flush before the buffer is full.
*/
void SerialPort::Flush()
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viFlush(m_portHandle, VI_WRITE_BUF);
wpi_setError(localStatus);
}
}
/**
* Reset the serial port driver to a known state.
*
* Empty the transmit and receive buffers in the device and formatted I/O.
*/
void SerialPort::Reset()
{
if (!m_consoleModeEnabled)
{
ViStatus localStatus = viClear(m_portHandle);
wpi_setError(localStatus);
}
}
//void SerialPort::_internalHandler(uint32_t port, uint32_t eventType, uint32_t event)
//{
//}
//ViStatus _VI_FUNCH ioCompleteHandler (ViSession vi, ViEventType eventType, ViEvent event, ViAddr userHandle)
//{
// ((SerialPort*) userHandle)->_internalHandler(vi, eventType, event);
// return VI_SUCCESS;
//}

161
wpilibc/src/main/native/Servo.cpp
View File

@@ -1,161 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Servo.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
constexpr float Servo::kMaxServoAngle;
constexpr float Servo::kMinServoAngle;
/**
* Common initialization code called by all constructors.
*
* InitServo() assigns defaults for the period multiplier for the servo PWM control signal, as
* well as the minimum and maximum PWM values supported by the servo.
*/
void Servo::InitServo()
{
m_table = NULL;
SetBounds(2.27, 1.513, 1.507, 1.5, .743);
SetPeriodMultiplier(kPeriodMultiplier_4X);
LiveWindow::GetInstance()->AddActuator("Servo", GetModuleNumber(), GetChannel(), this);
HALReport(HALUsageReporting::kResourceType_Servo, GetChannel(), GetModuleNumber() - 1);
}
/**
* Constructor that assumes the default digital module.
*
* @param channel The PWM channel on the digital module to which the servo is attached.
*/
Servo::Servo(uint32_t channel) : SafePWM(channel)
{
InitServo();
// printf("Done initializing servo %d\n", channel);
}
/**
* Constructor that specifies the digital module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel on the digital module to which the servo is attached (1..10).
*/
Servo::Servo(uint8_t moduleNumber, uint32_t channel) : SafePWM(moduleNumber, channel)
{
InitServo();
}
Servo::~Servo()
{
}
/**
* Set the servo position.
*
* Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
*
* @param value Position from 0.0 to 1.0.
*/
void Servo::Set(float value)
{
SetPosition(value);
}
/**
* Set the servo to offline.
*
* Set the servo raw value to 0 (undriven)
*/
void Servo::SetOffline() {
SetRaw(0);
}
/**
* Get the servo position.
*
* Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
*
* @return Position from 0.0 to 1.0.
*/
float Servo::Get()
{
return GetPosition();
}
/**
* Set the servo angle.
*
* Assume that the servo angle is linear with respect to the PWM value (big assumption, need to test).
*
* Servo angles that are out of the supported range of the servo simply "saturate" in that direction
* In other words, if the servo has a range of (X degrees to Y degrees) than angles of less than X
* result in an angle of X being set and angles of more than Y degrees result in an angle of Y being set.
*
* @param degrees The angle in degrees to set the servo.
*/
void Servo::SetAngle(float degrees)
{
if (degrees < kMinServoAngle)
{
degrees = kMinServoAngle;
}
else if (degrees > kMaxServoAngle)
{
degrees = kMaxServoAngle;
}
SetPosition(((float) (degrees - kMinServoAngle)) / GetServoAngleRange());
}
/**
* Get the servo angle.
*
* Assume that the servo angle is linear with respect to the PWM value (big assumption, need to test).
* @return The angle in degrees to which the servo is set.
*/
float Servo::GetAngle()
{
return (float)GetPosition() * GetServoAngleRange() + kMinServoAngle;
}
void Servo::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
Set(value.f);
}
void Servo::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", Get());
}
}
void Servo::StartLiveWindowMode() {
if (m_table != NULL) {
m_table->AddTableListener("Value", this, true);
}
}
void Servo::StopLiveWindowMode() {
if (m_table != NULL) {
m_table->RemoveTableListener(this);
}
}
std::string Servo::GetSmartDashboardType() {
return "Servo";
}
void Servo::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Servo::GetTable() {
return m_table;
}

149
wpilibc/src/main/native/SimpleRobot.cpp
View File

@@ -1,149 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SimpleRobot.h"
#include "DriverStation.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Timer.h"
#include "SmartDashboard/SmartDashboard.h"
#include "LiveWindow/LiveWindow.h"
#include "networktables/NetworkTable.h"
SimpleRobot::SimpleRobot()
: m_robotMainOverridden (true)
{
}
/**
* Robot-wide initialization code should go here.
*
* Programmers should override this method for default Robot-wide initialization which will
* be called each time the robot enters the disabled state.
*/
void SimpleRobot::RobotInit()
{
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Disabled should go here.
* Programmers should override this method to run code that should run while the field is
* disabled.
*/
void SimpleRobot::Disabled()
{
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Autonomous should go here.
* Programmers should override this method to run code that should run while the field is
* in the autonomous period. This will be called once each time the robot enters the
* autonomous state.
*/
void SimpleRobot::Autonomous()
{
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Operator control (tele-operated) code should go here.
* Programmers should override this method to run code that should run while the field is
* in the Operator Control (tele-operated) period. This is called once each time the robot
* enters the teleop state.
*/
void SimpleRobot::OperatorControl()
{
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Test program should go here.
* Programmers should override this method to run code that executes while the robot is
* in test mode. This will be called once whenever the robot enters test mode
*/
void SimpleRobot::Test()
{
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Robot main program for free-form programs.
*
* This should be overridden by user subclasses if the intent is to not use the Autonomous() and
* OperatorControl() methods. In that case, the program is responsible for sensing when to run
* the autonomous and operator control functions in their program.
*
* This method will be called immediately after the constructor is called. If it has not been
* overridden by a user subclass (i.e. the default version runs), then the Autonomous() and
* OperatorControl() methods will be called.
*/
void SimpleRobot::RobotMain()
{
m_robotMainOverridden = false;
}
/**
* Start a competition.
* This code needs to track the order of the field starting to ensure that everything happens
* in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
* or Test when the robot is enabled. After running the correct method, wait for some state to
* change, either the other mode starts or the robot is disabled. Then go back and wait for the
* robot to be enabled again.
*/
void SimpleRobot::StartCompetition()
{
LiveWindow *lw = LiveWindow::GetInstance();
HALReport(HALUsageReporting::kResourceType_Framework, HALUsageReporting::kFramework_Simple);
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
RobotMain();
if (!m_robotMainOverridden)
{
// first and one-time initialization
lw->SetEnabled(false);
RobotInit();
while (true)
{
if (IsDisabled())
{
m_ds->InDisabled(true);
Disabled();
m_ds->InDisabled(false);
while (IsDisabled()) m_ds->WaitForData();
}
else if (IsAutonomous())
{
m_ds->InAutonomous(true);
Autonomous();
m_ds->InAutonomous(false);
while (IsAutonomous() && IsEnabled()) m_ds->WaitForData();
}
else if (IsTest())
{
lw->SetEnabled(true);
m_ds->InTest(true);
Test();
m_ds->InTest(false);
while (IsTest() && IsEnabled()) m_ds->WaitForData();
lw->SetEnabled(false);
}
else
{
m_ds->InOperatorControl(true);
OperatorControl();
m_ds->InOperatorControl(false);
while (IsOperatorControl() && IsEnabled()) m_ds->WaitForData();
}
}
}
}

78
wpilibc/src/main/native/SmartDashboard/SendableChooser.cpp
View File

@@ -1,78 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SmartDashboard/SendableChooser.h"
#include "networktables2/type/StringArray.h"
#include <stdio.h>
static const char *kDefault = "default";
static const char *kOptions = "options";
static const char *kSelected = "selected";
SendableChooser::SendableChooser()
{
m_defaultChoice = "";
}
/**
* Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop,
* the object will appear as the given name.
* @param name the name of the option
* @param object the option
*/
void SendableChooser::AddObject(const char *name, void *object)
{
m_choices[name] = object;
}
/**
* Add the given object to the list of options and marks it as the default.
* Functionally, this is very close to {@link SendableChooser#AddObject(const char *name, void *object) AddObject(...)}
* except that it will use this as the default option if none other is explicitly selected.
* @param name the name of the option
* @param object the option
*/
void SendableChooser::AddDefault(const char *name, void *object)
{
m_defaultChoice = name;
AddObject(name, object);
}
/**
* Returns the selected option. If there is none selected, it will return the default. If there is none selected
* and no default, then it will return {@code NULL}.
* @return the option selected
*/
void *SendableChooser::GetSelected()
{
std::string selected = m_table->GetString(kSelected, m_defaultChoice);
if (selected == "")
return NULL;
else
return m_choices[selected];
}
void SendableChooser::InitTable(ITable* subtable) {
StringArray keys;
m_table = subtable;
if (m_table != NULL) {
std::map<std::string, void *>::iterator iter;
for (iter = m_choices.begin(); iter != m_choices.end(); iter++) {
keys.add(iter->first);
}
m_table->PutValue(kOptions, keys);
m_table->PutString(kDefault, m_defaultChoice);
}
}
ITable* SendableChooser::GetTable() {
return m_table;
}
std::string SendableChooser::GetSmartDashboardType() {
return "String Chooser";
}

179
wpilibc/src/main/native/SmartDashboard/SmartDashboard.cpp
View File

@@ -1,179 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SmartDashboard/SmartDashboard.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "SmartDashboard/NamedSendable.h"
#include "WPIErrors.h"
#include "networktables/NetworkTable.h"
ITable* SmartDashboard::m_table = NULL;
std::map<ITable *, Sendable *> SmartDashboard::m_tablesToData;
void SmartDashboard::init(){
m_table = NetworkTable::GetTable("SmartDashboard");
}
//TODO usage reporting
/**
* 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(std::string key, Sendable *data)
{
if (data == NULL)
{
//TODO wpi_setWPIErrorWithContext(NullParameter, "value");
return;
}
ITable* dataTable = m_table->GetSubTable(key);
dataTable->PutString("~TYPE~", data->GetSmartDashboardType());
data->InitTable(dataTable);
m_tablesToData[dataTable] = data;
}
/**
* 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(NamedSendable *value)
{
if (value == NULL)
{
//TODO wpi_setWPIErrorWithContext(NullParameter, "value");
return;
}
PutData(value->GetName(), value);
}
/**
* Returns the value at the specified key.
* @param keyName the key
* @return the value
*/
//TODO Sendable *SmartDashboard::GetData(std::string key)
/*{
ITable* subtable = m_table->GetSubTable(keyName);
Sendable *data = m_tablesToData[subtable];
if (data == NULL)
{
wpi_setWPIErrorWithContext(SmartDashboardMissingKey, keyName);
return NULL;
}
return data;
}*/
/**
* Maps the specified key to the specified complex value (such as an array) in this table.
* The key can not be NULL.
* The value can be retrieved by calling the RetrieveValue method with a key that is equal to the original key.
* @param keyName the key
* @param value the value
*/
void SmartDashboard::PutValue(std::string keyName, ComplexData& value)
{
m_table->PutValue(keyName, value);
}
/**
* Retrieves the complex value (such as an array) in this table into the complex data object
* The key can not be NULL.
* @param keyName the key
* @param value the object to retrieve the value into
*/
void SmartDashboard::RetrieveValue(std::string keyName, ComplexData& value)
{
m_table->RetrieveValue(keyName, value);
}
/**
* 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::PutBoolean(std::string keyName, bool value)
{
m_table->PutBoolean(keyName, value);
}
/**
* Returns the value at the specified key.
* @param keyName the key
* @return the value
*/
bool SmartDashboard::GetBoolean(std::string keyName)
{
return m_table->GetBoolean(keyName);
}
/**
* 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::PutNumber(std::string keyName, double value){
m_table->PutNumber(keyName, value);
}
/**
* Returns the value at the specified key.
* @param keyName the key
* @return the value
*/
double SmartDashboard::GetNumber(std::string keyName)
{
return m_table->GetNumber(keyName);
}
/**
* 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 SmartDashboard::PutString(std::string keyName, std::string value)
{
m_table->PutString(keyName, value);
}
/**
* Returns the value at the specified key.
* @param keyName the key
* @param value the buffer to fill with the value
* @param valueLen the size of the buffer pointed to by value
* @return the length of the string
*/
int SmartDashboard::GetString(std::string keyName, char *outBuffer, unsigned int bufferLen){
std::string value = m_table->GetString(keyName);
unsigned int i;
for(i = 0; i<bufferLen-1&&i<value.length(); ++i)
outBuffer[i] = (char)value.at(i);
outBuffer[i] = '\0';
return i;
}
/**
* Returns the value at the specified key.
* @param keyName the key
* @return the value
*/
std::string SmartDashboard::GetString(std::string keyName)
{
return m_table->GetString(keyName);
}

143
wpilibc/src/main/native/Solenoid.cpp
View File

@@ -1,143 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Solenoid.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
/**
* 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, solenoid_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);
HALReport(HALUsageReporting::kResourceType_Solenoid, m_channel, m_moduleNumber - 1);
}
/**
* Constructor.
*
* @param channel The channel on the solenoid module to control (1..8).
*/
Solenoid::Solenoid(uint32_t channel)
: SolenoidBase (GetDefaultSolenoidModule())
, m_channel (channel)
{
InitSolenoid();
}
/**
* Constructor.
*
* @param moduleNumber The solenoid module (1 or 2).
* @param channel The channel on the solenoid module to control (1..8).
*/
Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel)
: SolenoidBase (moduleNumber)
, m_channel (channel)
{
InitSolenoid();
}
/**
* Destructor.
*/
Solenoid::~Solenoid()
{
if (CheckSolenoidModule(m_moduleNumber))
{
m_allocated->Free((m_moduleNumber - 1) * kSolenoidChannels + m_channel - 1);
}
}
/**
* Set the value of a solenoid.
*
* @param on Turn the solenoid output off or on.
*/
void Solenoid::Set(bool on)
{
if (StatusIsFatal()) return;
uint8_t value = on ? 0xFF : 0x00;
uint8_t mask = 1 << (m_channel - 1);
SolenoidBase::Set(value, mask);
}
/**
* Read the current value of the solenoid.
*
* @return The current value of the solenoid.
*/
bool Solenoid::Get()
{
if (StatusIsFatal()) return false;
uint8_t value = GetAll() & ( 1 << (m_channel - 1));
return (value != 0);
}
void Solenoid::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
Set(value.b);
}
void Solenoid::UpdateTable() {
if (m_table != NULL) {
m_table->PutBoolean("Value", Get());
}
}
void Solenoid::StartLiveWindowMode() {
Set(false);
if (m_table != NULL) {
m_table->AddTableListener("Value", this, true);
}
}
void Solenoid::StopLiveWindowMode() {
Set(false);
if (m_table != NULL) {
m_table->RemoveTableListener(this);
}
}
std::string Solenoid::GetSmartDashboardType() {
return "Solenoid";
}
void Solenoid::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Solenoid::GetTable() {
return m_table;
}

66
wpilibc/src/main/native/SolenoidBase.cpp
View File

@@ -1,66 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "SolenoidBase.h"
// Needs to be global since the protected resource spans all Solenoid objects.
Resource *SolenoidBase::m_allocated = NULL;
/**
* Constructor
*
* @param moduleNumber The solenoid module (1 or 2).
*/
SolenoidBase::SolenoidBase(uint8_t moduleNumber)
: m_moduleNumber (moduleNumber)
{
for (uint32_t i = 0; i < kSolenoidChannels; i++)
{
void* port = getPortWithModule(moduleNumber, i+1);
int32_t status = 0;
m_ports[i] = initializeSolenoidPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
/**
* Destructor.
*/
SolenoidBase::~SolenoidBase()
{
}
/**
* Set the value of a solenoid.
*
* @param value The value you want to set on the module.
* @param mask The channels you want to be affected.
*/
void SolenoidBase::Set(uint8_t value, uint8_t mask)
{
int32_t status = 0;
for (int i = 0; i < 8; i++) { // XXX: Unhardcode
uint8_t local_mask = 1 << i;
if (mask & local_mask)
setSolenoid(m_ports[i], value & local_mask, &status);
}
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Read all 8 solenoids as a single byte
*
* @return The current value of all 8 solenoids on the module.
*/
uint8_t SolenoidBase::GetAll()
{
uint8_t value = 0;
int32_t status = 0;
for (int i = 0; i < 8; i++) { // XXX: Unhardcode
value |= getSolenoid(m_ports[i], &status) << i;
}
return value;
}

102
wpilibc/src/main/native/Talon.cpp
View File

@@ -1,102 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Talon.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
/**
* Common initialization code called by all constructors.
*
* Note that the Talon uses the following bounds for PWM values. These values should work reasonably well for
* most controllers, but if users experience issues such as asymmetric behavior around
* the deadband or inability to saturate the controller in either direction, calibration is recommended.
* The calibration procedure can be found in the Talon User Manual available from CTRE.
*
* - 211 = full "forward"
* - 133 = the "high end" of the deadband range
* - 129 = center of the deadband range (off)
* - 125 = the "low end" of the deadband range
* - 49 = full "reverse"
*/
void Talon::InitTalon() {
SetBounds(2.037, 1.539, 1.513, 1.487, .989);
SetPeriodMultiplier(kPeriodMultiplier_2X);
SetRaw(m_centerPwm);
HALReport(HALUsageReporting::kResourceType_Talon, GetChannel(), GetModuleNumber() - 1);
LiveWindow::GetInstance()->AddActuator("Talon", GetModuleNumber(), GetChannel(), this);
}
/**
* Constructor that assumes the default digital module.
*
* @param channel The PWM channel on the digital module that the Talon is attached to.
*/
Talon::Talon(uint32_t channel) : SafePWM(channel)
{
InitTalon();
}
/**
* Constructor that specifies the digital module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel on the digital module that the Talon is attached to (1..10).
*/
Talon::Talon(uint8_t moduleNumber, uint32_t channel) : SafePWM(moduleNumber, channel)
{
InitTalon();
}
Talon::~Talon()
{
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
* @param syncGroup Unused interface.
*/
void Talon::Set(float speed, uint8_t syncGroup)
{
SetSpeed(speed);
}
/**
* Get the recently set value of the PWM.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
float Talon::Get()
{
return GetSpeed();
}
/**
* Common interface for disabling a motor.
*/
void Talon::Disable()
{
SetRaw(kPwmDisabled);
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
void Talon::PIDWrite(float output)
{
Set(output);
}

205
wpilibc/src/main/native/Task.cpp
View File

@@ -1,205 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Task.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include <errno.h>
#include <string.h>
#include <stdio.h>
const uint32_t Task::kDefaultPriority;
/**
* Create but don't launch a task.
* @param name The name of the task. "FRC_" will be prepended to the task name.
* @param function The address of the function to run as the new task.
* @param priority The VxWorks priority for the task.
* @param stackSize The size of the stack for the task
*/
Task::Task(const char* name, FUNCPTR function, int32_t priority, uint32_t stackSize)
{
m_taskID = NULL_TASK;
m_function = function;
m_priority = priority;
m_stackSize = stackSize;
m_taskName = new char[strlen(name) + 5];
strcpy(m_taskName, "FRC_");
strcpy(m_taskName+4, name);
static int32_t instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_Task, instances, 0, m_taskName);
}
Task::~Task()
{
if (m_taskID != NULL_TASK) Stop();
delete [] m_taskName;
m_taskName = NULL;
}
/**
* Starts this task.
* If it is already running or unable to start, it fails and returns false.
*/
bool Task::Start(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint32_t arg3, uint32_t arg4,
uint32_t arg5, uint32_t arg6, uint32_t arg7, uint32_t arg8, uint32_t arg9)
{
m_taskID = spawnTask(m_taskName,
m_priority,
VXWORKS_FP_TASK, // options
m_stackSize, // stack size
m_function, // function to start
arg0, arg1, arg2, arg3, arg4, // parameter 1 - pointer to this class
arg5, arg6, arg7, arg8, arg9);// additional unused parameters
if (m_taskID == NULL_TASK) {
HandleError(ERROR);
return false;
}
return true;
}
/**
* Restarts a running task.
* If the task isn't started, it starts it.
* @return false if the task is running and we are unable to kill the previous instance
*/
bool Task::Restart()
{
return HandleError(restartTask(m_taskID));
}
/**
* Kills the running task.
* @returns true on success false if the task doesn't exist or we are unable to kill it.
*/
bool Task::Stop()
{
bool ok = true;
if (Verify())
{
ok = HandleError(deleteTask(m_taskID));
}
m_taskID = NULL_TASK;
return ok;
}
/**
* Returns true if the task is ready to execute (i.e. not suspended, delayed, or blocked).
* @return true if ready, false if not ready.
*/
bool Task::IsReady()
{
return isTaskReady(m_taskID);
}
/**
* Returns true if the task was explicitly suspended by calling Suspend()
* @return true if suspended, false if not suspended.
*/
bool Task::IsSuspended()
{
return isTaskSuspended(m_taskID);
}
/**
* Pauses a running task.
* Returns true on success, false if unable to pause or the task isn't running.
*/
bool Task::Suspend()
{
return HandleError(suspendTask(m_taskID));
}
/**
* Resumes a paused task.
* Returns true on success, false if unable to resume or if the task isn't running/paused.
*/
bool Task::Resume()
{
return HandleError(resumeTask(m_taskID));
}
/**
* Verifies a task still exists.
* @returns true on success.
*/
bool Task::Verify()
{
return verifyTaskID(m_taskID) == OK;
}
/**
* Gets the priority of a task.
* @returns task priority or 0 if an error occured
*/
int32_t Task::GetPriority()
{
if (HandleError(getTaskPriority(m_taskID, &m_priority)))
return m_priority;
else
return 0;
}
/**
* This routine changes a task's priority to a specified priority.
* Priorities range from 0, the highest priority, to 255, the lowest priority.
* Default task priority is 100.
* @param priority The priority the task should run at.
* @returns true on success.
*/
bool Task::SetPriority(int32_t priority)
{
m_priority = priority;
return HandleError(setTaskPriority(m_taskID, m_priority));
}
/**
* Returns the name of the task.
* @returns Pointer to the name of the task or NULL if not allocated
*/
const char* Task::GetName()
{
return m_taskName;
}
/**
* Get the ID of a task
* @returns Task ID of this task. Task::kInvalidTaskID (-1) if the task has not been started or has already exited.
*/
TASK Task::GetID()
{
if (Verify())
return m_taskID;
return NULL_TASK;
}
/**
* Handles errors generated by task related code.
*/
bool Task::HandleError(STATUS results)
{
if (results != ERROR) return true;
int errsv = errno;
if (errsv == HAL_objLib_OBJ_ID_ERROR) {
wpi_setWPIErrorWithContext(TaskIDError, m_taskName);
} else if (errsv == HAL_objLib_OBJ_DELETED) {
wpi_setWPIErrorWithContext(TaskDeletedError, m_taskName);
} else if (errsv == HAL_taskLib_ILLEGAL_OPTIONS) {
wpi_setWPIErrorWithContext(TaskOptionsError, m_taskName);
} else if (errsv == HAL_memLib_NOT_ENOUGH_MEMORY) {
wpi_setWPIErrorWithContext(TaskMemoryError, m_taskName);
} else if (errsv == HAL_taskLib_ILLEGAL_PRIORITY) {
wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName);
} else {
printf("ERROR: errno=%i", errsv);
wpi_setWPIErrorWithContext(TaskError, m_taskName);
}
return false;
}

202
wpilibc/src/main/native/Timer.cpp
View File

@@ -1,202 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Timer.h"
#include <time.h>
#include "HAL/HAL.h"
#include "HAL/cpp/Synchronized.h"
#include "Utility.h"
/**
* Pause the task for a specified time.
*
* Pause the execution of the program for a specified period of time given in seconds.
* Motors will continue to run at their last assigned values, and sensors will continue to
* update. Only the task containing the wait will pause until the wait time is expired.
*
* @param seconds Length of time to pause, in seconds.
*/
void Wait(double seconds)
{
if (seconds < 0.0) return;
delaySeconds(seconds);
}
/*
* Return the FPGA system clock time in seconds.
* This is deprecated and just forwards to Timer::GetFPGATimestamp().
* @returns Robot running time in seconds.
*/
double GetClock()
{
return Timer::GetFPGATimestamp();
}
/**
* @brief Gives real-time clock system time with nanosecond resolution
* @return The time, just in case you want the robot to start autonomous at 8pm on Saturday.
*/
double GetTime()
{
struct timespec tp;
clock_gettime(CLOCK_REALTIME,&tp);
double realTime = (double)tp.tv_sec + (double)((double)tp.tv_nsec*1e-9);
return (realTime);
}
/**
* Create a new timer object.
*
* Create a new timer object and reset the time to zero. The timer is initially not running and
* must be started.
*/
Timer::Timer()
: m_startTime (0.0)
, m_accumulatedTime (0.0)
, m_running (false)
, m_semaphore (0)
{
//Creates a semaphore to control access to critical regions.
//Initially 'open'
m_semaphore = initializeMutexNormal();
Reset();
}
Timer::~Timer()
{
deleteMutex(m_semaphore);
}
/**
* Get the current time from the timer. If the clock is running it is derived from
* the current system clock the start time stored in the timer class. If the clock
* is not running, then return the time when it was last stopped.
*
* @return unsigned Current time value for this timer in seconds
*/
double Timer::Get()
{
double result;
double currentTime = GetFPGATimestamp();
Synchronized sync(m_semaphore);
if(m_running)
{
// This math won't work if the timer rolled over (71 minutes after boot).
// TODO: Check for it and compensate.
result = (currentTime - m_startTime) + m_accumulatedTime;
}
else
{
result = m_accumulatedTime;
}
return result;
}
/**
* Reset the timer by setting the time to 0.
*
* Make the timer startTime the current time so new requests will be relative to now
*/
void Timer::Reset()
{
Synchronized sync(m_semaphore);
m_accumulatedTime = 0;
m_startTime = GetFPGATimestamp();
}
/**
* Start the timer running.
* Just set the running flag to true indicating that all time requests should be
* relative to the system clock.
*/
void Timer::Start()
{
Synchronized sync(m_semaphore);
if (!m_running)
{
m_startTime = GetFPGATimestamp();
m_running = true;
}
}
/**
* Stop the timer.
* This computes the time as of now and clears the running flag, causing all
* subsequent time requests to be read from the accumulated time rather than
* looking at the system clock.
*/
void Timer::Stop()
{
double temp = Get();
Synchronized sync(m_semaphore);
if (m_running)
{
m_accumulatedTime = temp;
m_running = false;
}
}
/**
* Check if the period specified has passed and if it has, advance the start
* time by that period. This is useful to decide if it's time to do periodic
* work without drifting later by the time it took to get around to checking.
*
* @param period The period to check for (in seconds).
* @return If the period has passed.
*/
bool Timer::HasPeriodPassed(double period)
{
if (Get() > period)
{
Synchronized sync(m_semaphore);
// Advance the start time by the period.
// Don't set it to the current time... we want to avoid drift.
m_startTime += period;
return true;
}
return false;
}
/*
* Return the FPGA system clock time in seconds.
*
* Return the time from the FPGA hardware clock in seconds since the FPGA
* started.
* Rolls over after 71 minutes.
* @returns Robot running time in seconds.
*/
double Timer::GetFPGATimestamp()
{
// FPGA returns the timestamp in microseconds
// Call the helper GetFPGATime() in Utility.cpp
return GetFPGATime() * 1.0e-6;
}
// Internal function that reads the PPC timestamp counter.
extern "C"
{
uint32_t niTimestamp32(void);
uint64_t niTimestamp64(void);
}
/*
* Return the PowerPC timestamp since boot in seconds.
*
* This is lower overhead than GetFPGATimestamp() but not synchronized with other FPGA timestamps.
* @returns Robot running time in seconds.
*/
double Timer::GetPPCTimestamp()
{
// PPC system clock is 33MHz
// XXX: return niTimestamp64() / 33.0e6;
}

361
wpilibc/src/main/native/Ultrasonic.cpp
View File

@@ -1,361 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Ultrasonic.h"
#include "Counter.h"
#include "DigitalInput.h"
#include "DigitalOutput.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
constexpr double Ultrasonic::kPingTime; ///< Time (sec) for the ping trigger pulse.
const uint32_t Ultrasonic::kPriority; ///< Priority that the ultrasonic round robin task runs.
constexpr double Ultrasonic::kMaxUltrasonicTime; ///< Max time (ms) between readings.
constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
Task Ultrasonic::m_task("UltrasonicChecker", (FUNCPTR)UltrasonicChecker); // task doing the round-robin automatic sensing
Ultrasonic *Ultrasonic::m_firstSensor = NULL; // head of the ultrasonic sensor list
bool Ultrasonic::m_automaticEnabled = false; // automatic round robin mode
SEMAPHORE_ID Ultrasonic::m_semaphore = 0;
/**
* Background task that goes through the list of ultrasonic sensors and pings each one in turn. The counter
* is configured to read the timing of the returned echo pulse.
*
* DANGER WILL ROBINSON, DANGER WILL ROBINSON:
* This code runs as a task and assumes that none of the ultrasonic sensors will change while it's
* running. If one does, then this will certainly break. Make sure to disable automatic mode before changing
* anything with the sensors!!
*/
void Ultrasonic::UltrasonicChecker()
{
Ultrasonic *u = NULL;
while (m_automaticEnabled)
{
if (u == NULL) u = m_firstSensor;
if (u == NULL) return;
if (u->IsEnabled())
u->m_pingChannel->Pulse(kPingTime); // do the ping
u = u->m_nextSensor;
Wait(0.1); // wait for ping to return
}
}
/**
* Initialize the Ultrasonic Sensor.
* This is the common code that initializes the ultrasonic sensor given that there
* are two digital I/O channels allocated. If the system was running in automatic mode (round robin)
* when the new sensor is added, it is stopped, the sensor is added, then automatic mode is
* restored.
*/
void Ultrasonic::Initialize()
{
m_table = NULL;
bool originalMode = m_automaticEnabled;
if (m_semaphore == 0) m_semaphore = initializeSemaphore(SEMAPHORE_FULL);
SetAutomaticMode(false); // kill task when adding a new sensor
takeSemaphore(m_semaphore); // link this instance on the list
{
m_nextSensor = m_firstSensor;
m_firstSensor = this;
}
giveSemaphore(m_semaphore);
m_counter = new Counter(m_echoChannel); // set up counter for this sensor
m_counter->SetMaxPeriod(1.0);
m_counter->SetSemiPeriodMode(true);
m_counter->Reset();
m_counter->Start();
m_enabled = true; // make it available for round robin scheduling
SetAutomaticMode(originalMode);
static int instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_Ultrasonic, instances);
LiveWindow::GetInstance()->AddSensor("Ultrasonic", m_echoChannel->GetModuleForRouting(), m_echoChannel->GetChannel(), this);
}
/**
* Create an instance of the Ultrasonic Sensor using the default module.
* This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
* constructor assumes that both digital I/O channels are in the default digital module.
* @param pingChannel The digital output channel that sends the pulse to initiate the sensor sending
* the ping.
* @param echoChannel The digital input channel that receives the echo. The length of time that the
* echo is high represents the round trip time of the ping, and the distance.
* @param units The units returned in either kInches or kMilliMeters
*/
Ultrasonic::Ultrasonic(uint32_t pingChannel, uint32_t echoChannel, DistanceUnit units)
{
m_pingChannel = new DigitalOutput(pingChannel);
m_echoChannel = new DigitalInput(echoChannel);
m_allocatedChannels = true;
m_units = units;
Initialize();
}
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutput
* for the ping channel.
* @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to determine the range.
* @param units The units returned in either kInches or kMilliMeters
*/
Ultrasonic::Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, DistanceUnit units)
{
if (pingChannel == NULL || echoChannel == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
m_allocatedChannels = false;
m_pingChannel = pingChannel;
m_echoChannel = echoChannel;
m_units = units;
Initialize();
}
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutput
* for the ping channel.
* @param pingChannel The digital output object that starts the sensor doing a ping. Requires a 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to determine the range.
* @param units The units returned in either kInches or kMilliMeters
*/
Ultrasonic::Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel, DistanceUnit units)
{
m_allocatedChannels = false;
m_pingChannel = &pingChannel;
m_echoChannel = &echoChannel;
m_units = units;
Initialize();
}
/**
* Create an instance of the Ultrasonic sensor using specified modules.
* This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
* constructors takes the channel and module slot for each of the required digital I/O channels.
* @param pingModuleNumber The digital module that the pingChannel is on.
* @param pingChannel The digital output channel that sends the pulse to initiate the sensor
* sending the ping.
* @param echoModuleNumber The digital module that the echoChannel is on.
* @param echoChannel The digital input channel that receives the echo. The length of time
* that the echo is high represents the round trip time of the ping, and the distance.
* @param units The units returned in either kInches or kMilliMeters
*/
Ultrasonic::Ultrasonic(uint8_t pingModuleNumber, uint32_t pingChannel,
uint8_t echoModuleNumber, uint32_t echoChannel, DistanceUnit units)
{
m_pingChannel = new DigitalOutput(pingModuleNumber, pingChannel);
m_echoChannel = new DigitalInput(echoModuleNumber, echoChannel);
m_allocatedChannels = true;
m_units = units;
Initialize();
}
/**
* Destructor for the ultrasonic sensor.
* Delete the instance of the ultrasonic sensor by freeing the allocated digital channels.
* If the system was in automatic mode (round robin), then it is stopped, then started again
* after this sensor is removed (provided this wasn't the last sensor).
*/
Ultrasonic::~Ultrasonic()
{
bool wasAutomaticMode = m_automaticEnabled;
SetAutomaticMode(false);
if (m_allocatedChannels)
{
delete m_pingChannel;
delete m_echoChannel;
}
wpi_assert(m_firstSensor != NULL);
takeSemaphore(m_semaphore);
{
if (this == m_firstSensor)
{
m_firstSensor = m_nextSensor;
if (m_firstSensor == NULL)
{
SetAutomaticMode(false);
}
}
else
{
wpi_assert(m_firstSensor->m_nextSensor != NULL);
for (Ultrasonic *s = m_firstSensor; s != NULL; s = s->m_nextSensor)
{
if (this == s->m_nextSensor)
{
s->m_nextSensor = s->m_nextSensor->m_nextSensor;
break;
}
}
}
}
giveSemaphore(m_semaphore);
if (m_firstSensor != NULL && wasAutomaticMode)
SetAutomaticMode(true);
}
/**
* Turn Automatic mode on/off.
* When in Automatic mode, all sensors will fire in round robin, waiting a set
* time between each sensor.
* @param enabling Set to true if round robin scheduling should start for all the ultrasonic sensors. This
* scheduling method assures that the sensors are non-interfering because no two sensors fire at the same time.
* If another scheduling algorithm is preffered, it can be implemented by pinging the sensors manually and waiting
* for the results to come back.
*/
void Ultrasonic::SetAutomaticMode(bool enabling)
{
if (enabling == m_automaticEnabled)
return; // ignore the case of no change
m_automaticEnabled = enabling;
if (enabling)
{
// enabling automatic mode.
// Clear all the counters so no data is valid
for (Ultrasonic *u = m_firstSensor; u != NULL; u = u->m_nextSensor)
{
u->m_counter->Reset();
}
// Start round robin task
wpi_assert(m_task.Verify() == false); // should be false since was previously disabled
m_task.Start();
}
else
{
// disabling automatic mode. Wait for background task to stop running.
while (m_task.Verify())
Wait(0.15); // just a little longer than the ping time for round-robin to stop
// clear all the counters (data now invalid) since automatic mode is stopped
for (Ultrasonic *u = m_firstSensor; u != NULL; u = u->m_nextSensor)
{
u->m_counter->Reset();
}
m_task.Stop();
}
}
/**
* Single ping to ultrasonic sensor.
* Send out a single ping to the ultrasonic sensor. This only works if automatic (round robin)
* mode is disabled. A single ping is sent out, and the counter should count the semi-period
* when it comes in. The counter is reset to make the current value invalid.
*/
void Ultrasonic::Ping()
{
// TODO: Either assert or disable, not both.
wpi_assert(!m_automaticEnabled);
SetAutomaticMode(false); // turn off automatic round robin if pinging single sensor
m_counter->Reset(); // reset the counter to zero (invalid data now)
m_pingChannel->Pulse(kPingTime); // do the ping to start getting a single range
}
/**
* Check if there is a valid range measurement.
* The ranges are accumulated in a counter that will increment on each edge of the echo (return)
* signal. If the count is not at least 2, then the range has not yet been measured, and is invalid.
*/
bool Ultrasonic::IsRangeValid()
{
return m_counter->Get() > 1;
}
/**
* Get the range in inches from the ultrasonic sensor.
* @return double Range in inches of the target returned from the ultrasonic sensor. If there is
* no valid value yet, i.e. at least one measurement hasn't completed, then return 0.
*/
double Ultrasonic::GetRangeInches()
{
if (IsRangeValid())
return m_counter->GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
else
return 0;
}
/**
* Get the range in millimeters from the ultrasonic sensor.
* @return double Range in millimeters of the target returned by the ultrasonic sensor.
* If there is no valid value yet, i.e. at least one measurement hasn't complted, then return 0.
*/
double Ultrasonic::GetRangeMM()
{
return GetRangeInches() * 25.4;
}
/**
* Get the range in the current DistanceUnit for the PIDSource base object.
*
* @return The range in DistanceUnit
*/
double Ultrasonic::PIDGet()
{
switch(m_units)
{
case Ultrasonic::kInches:
return GetRangeInches();
case Ultrasonic::kMilliMeters:
return GetRangeMM();
default:
return 0.0;
}
}
/**
* Set the current DistanceUnit that should be used for the PIDSource base object.
*
* @param units The DistanceUnit that should be used.
*/
void Ultrasonic::SetDistanceUnits(DistanceUnit units)
{
m_units = units;
}
/**
* Get the current DistanceUnit that is used for the PIDSource base object.
*
* @return The type of DistanceUnit that is being used.
*/
Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits()
{
return m_units;
}
void Ultrasonic::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetRangeInches());
}
}
void Ultrasonic::StartLiveWindowMode() {
}
void Ultrasonic::StopLiveWindowMode() {
}
std::string Ultrasonic::GetSmartDashboardType() {
return "Ultrasonic";
}
void Ultrasonic::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Ultrasonic::GetTable() {
return m_table;
}

282
wpilibc/src/main/native/Utility.cpp
View File

@@ -1,282 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Utility.h"
//#include "NetworkCommunication/FRCComm.h"
#include "HAL/HAL.h"
#include "HAL/cpp/StackTrace.h"
#include "Task.h"
#include <stdio.h>
#include <string.h>
#include "nivision.h"
static bool stackTraceEnabled = false;
static bool suspendOnAssertEnabled = false;
/**
* Enable Stack trace after asserts.
*/
void wpi_stackTraceOnAssertEnable(bool enabled)
{
stackTraceEnabled = enabled;
}
/**
* Enable suspend on asssert.
* If enabled, the user task will be suspended whenever an assert fails. This
* will allow the user to attach to the task with the debugger and examine variables
* around the failure.
*/
void wpi_suspendOnAssertEnabled(bool enabled)
{
suspendOnAssertEnabled = enabled;
}
static void wpi_handleTracing()
{
if (stackTraceEnabled)
{
printf("\n-----------<Stack Trace>----------------\n");
printCurrentStackTrace();
}
printf("\n");
}
/**
* Assert implementation.
* This allows breakpoints to be set on an assert.
* The users don't call this, but instead use the wpi_assert macros in Utility.h.
*/
bool wpi_assert_impl(bool conditionValue,
const char *conditionText,
const char *message,
const char *fileName,
uint32_t lineNumber,
const char *funcName)
{
if (!conditionValue)
{
// Error string buffer
char error[256];
// If an error message was specified, include it
// Build error string
if(message != NULL) {
sprintf(error, "Assertion failed: \"%s\", \"%s\" failed in %s() in %s at line %ld\n",
message, conditionText, funcName, fileName, lineNumber);
} else {
sprintf(error, "Assertion failed: \"%s\" in %s() in %s at line %ld\n",
conditionText, funcName, fileName, lineNumber);
}
// Print to console and send to remote dashboard
printf("\n\n>>>>%s", error);
HALSetErrorData(error, strlen(error), 100);
wpi_handleTracing();
if (suspendOnAssertEnabled) suspendTask(0);
}
return conditionValue;
}
/**
* Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
* This should not be called directly; it should only be used by wpi_assertEqual_impl
* and wpi_assertNotEqual_impl.
*/
void wpi_assertEqual_common_impl(int valueA,
int valueB,
const char *equalityType,
const char *message,
const char *fileName,
uint32_t lineNumber,
const char *funcName)
{
// Error string buffer
char error[256];
// If an error message was specified, include it
// Build error string
if(message != NULL) {
sprintf(error, "Assertion failed: \"%s\", \"%d\" %s \"%d\" in %s() in %s at line %ld\n",
message, valueA, equalityType, valueB, funcName, fileName, lineNumber);
} else {
sprintf(error, "Assertion failed: \"%d\" %s \"%d\" in %s() in %s at line %ld\n",
valueA, equalityType, valueB, funcName, fileName, lineNumber);
}
// Print to console and send to remote dashboard
printf("\n\n>>>>%s", error);
HALSetErrorData(error, strlen(error), 100);
wpi_handleTracing();
if (suspendOnAssertEnabled) suspendTask(0);
}
/**
* Assert equal implementation.
* This determines whether the two given integers are equal. If not,
* the value of each is printed along with an optional message string.
* The users don't call this, but instead use the wpi_assertEqual macros in Utility.h.
*/
bool wpi_assertEqual_impl(int valueA,
int valueB,
const char *message,
const char *fileName,
uint32_t lineNumber,
const char *funcName)
{
if(!(valueA == valueB))
{
wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName, lineNumber, funcName);
}
return valueA == valueB;
}
/**
* Assert not equal implementation.
* This determines whether the two given integers are equal. If so,
* the value of each is printed along with an optional message string.
* The users don't call this, but instead use the wpi_assertNotEqual macros in Utility.h.
*/
bool wpi_assertNotEqual_impl(int valueA,
int valueB,
const char *message,
const char *fileName,
uint32_t lineNumber,
const char *funcName)
{
if(!(valueA != valueB))
{
wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName, lineNumber, funcName);
}
return valueA != valueB;
}
/**
* Return the FPGA Version number.
* For now, expect this to be competition year.
* @return FPGA Version number.
*/
uint16_t GetFPGAVersion()
{
int32_t status = 0;
uint16_t version = getFPGAVersion(&status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
return version;
}
/**
* Return the FPGA Revision number.
* The format of the revision is 3 numbers.
* The 12 most significant bits are the Major Revision.
* the next 8 bits are the Minor Revision.
* The 12 least significant bits are the Build Number.
* @return FPGA Revision number.
*/
uint32_t GetFPGARevision()
{
int32_t status = 0;
uint32_t revision = getFPGARevision(&status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
return revision;
}
/**
* Read the microsecond-resolution timer on the FPGA.
*
* @return The current time in microseconds according to the FPGA (since FPGA reset).
*/
uint32_t GetFPGATime()
{
int32_t status = 0;
uint32_t time = getFPGATime(&status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
return time;
}
// RT hardware access functions exported from ni_emb.out
extern "C"
{
int32_t UserSwitchInput(int32_t nSwitch);
int32_t LedInput(int32_t led);
int32_t LedOutput(int32_t led, int32_t value);
}
/**
* Read the value of the USER1 DIP switch on the cRIO.
*/
int32_t GetRIOUserSwitch()
{
int32_t switchValue = UserSwitchInput(0);
wpi_assert(switchValue >= 0);
return switchValue > 0;
}
/**
* Set the state of the USER1 status LED on the cRIO.
*/
void SetRIOUserLED(uint32_t state)
{
LedOutput(0, state > 0);
}
/**
* Get the current state of the USER1 status LED on the cRIO.
* @return The curent state of the USER1 LED.
*/
int32_t GetRIOUserLED()
{
return LedInput(0);
}
/**
* Toggle the state of the USER1 status LED on the cRIO.
* @return The new state of the USER1 LED.
*/
int32_t ToggleRIOUserLED()
{
int32_t ledState = !GetRIOUserLED();
SetRIOUserLED(ledState);
return ledState;
}
/**
* Set the state of the FPGA status LED on the cRIO.
*/
void SetRIO_FPGA_LED(uint32_t state)
{
int32_t status = 0;
setFPGALED(state, &status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Get the current state of the FPGA status LED on the cRIO.
* @return The curent state of the FPGA LED.
*/
int32_t GetRIO_FPGA_LED()
{
int32_t status = 0;
int32_t state = getFPGALED(&status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
return state;
}
/**
* Toggle the state of the FPGA status LED on the cRIO.
* @return The new state of the FPGA LED.
*/
int32_t ToggleRIO_FPGA_LED()
{
int32_t ledState = !GetRIO_FPGA_LED();
SetRIO_FPGA_LED(ledState);
return ledState;
}

104
wpilibc/src/main/native/Victor.cpp
View File

@@ -1,104 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Victor.h"
#include "DigitalModule.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
/**
* Common initialization code called by all constructors.
*
* Note that the Victor uses the following bounds for PWM values. These values were determined
* empirically and optimized for the Victor 888. These values should work reasonably well for
* Victor 884 controllers as well but if users experience issues such as asymmetric behavior around
* the deadband or inability to saturate the controller in either direction, calibration is recommended.
* The calibration procedure can be found in the Victor 884 User Manual available from IFI.
*
* - 206 = full "forward"
* - 131 = the "high end" of the deadband range
* - 128 = center of the deadband range (off)
* - 125 = the "low end" of the deadband range
* - 56 = full "reverse"
*/
void Victor::InitVictor() {
SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
SetPeriodMultiplier(kPeriodMultiplier_2X);
SetRaw(m_centerPwm);
LiveWindow::GetInstance()->AddActuator("Victor", GetModuleNumber(), GetChannel(), this);
HALReport(HALUsageReporting::kResourceType_Victor, GetChannel(), GetModuleNumber() - 1);
}
/**
* Constructor that assumes the default digital module.
*
* @param channel The PWM channel on the digital module that the Victor is attached to.
*/
Victor::Victor(uint32_t channel) : SafePWM(channel)
{
InitVictor();
}
/**
* Constructor that specifies the digital module.
*
* @param moduleNumber The digital module (1 or 2).
* @param channel The PWM channel on the digital module that the Victor is attached to (1..10).
*/
Victor::Victor(uint8_t moduleNumber, uint32_t channel) : SafePWM(moduleNumber, channel)
{
InitVictor();
}
Victor::~Victor()
{
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
* @param syncGroup Unused interface.
*/
void Victor::Set(float speed, uint8_t syncGroup)
{
SetSpeed(speed);
}
/**
* Get the recently set value of the PWM.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
float Victor::Get()
{
return GetSpeed();
}
/**
* Common interface for disabling a motor.
*/
void Victor::Disable()
{
SetRaw(kPwmDisabled);
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
void Victor::PIDWrite(float output)
{
Set(output);
}

502
wpilibc/src/main/native/Vision/AxisCamera.cpp
View File

@@ -1,502 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Vision/AxisCamera.h"
#include <string.h>
#include "NetworkCommunication/UsageReporting.h"
#include "HAL/cpp/Synchronized.h"
#include "Vision/PCVideoServer.h"
#include "WPIErrors.h"
/** Private NI function to decode JPEG */
IMAQ_FUNC int Priv_ReadJPEGString_C(Image* _image, const unsigned char* _string, uint32_t _stringLength);
// Max packet without jumbo frames is 1500... add 36 because??
#define kMaxPacketSize 1536
#define kImageBufferAllocationIncrement 1000
AxisCamera *AxisCamera::_instance = NULL;
/**
* AxisCamera constructor
*/
AxisCamera::AxisCamera(const char *ipAddress)
: AxisCameraParams(ipAddress)
, m_cameraSocket(ERROR)
, m_protectedImageBuffer(NULL)
, m_protectedImageBufferLength(0)
, m_protectedImageSize(0)
, m_protectedImageSem(NULL)
, m_freshImage(false)
, m_imageStreamTask("cameraTask", (FUNCPTR)s_ImageStreamTaskFunction)
, m_videoServer(NULL)
{
m_protectedImageSem = initializeMutex(SEMAPHORE_Q_PRIORITY | SEMAPHORE_INVERSION_SAFE | SEMAPHORE_DELETE_SAFE);
#if JAVA_CAMERA_LIB != 1
nUsageReporting::report(nUsageReporting::kResourceType_AxisCamera, ipAddress == NULL ? 1 : 2);
#endif
if (!StatusIsFatal())
m_imageStreamTask.Start((int)this);
}
/**
* Destructor
*/
AxisCamera::~AxisCamera()
{
delete m_videoServer;
m_videoServer = NULL;
m_imageStreamTask.Stop();
close(m_cameraSocket);
SemSet_t::iterator it = m_newImageSemSet.begin();
SemSet_t::iterator end = m_newImageSemSet.end();
for (;it != end; it++)
{
deleteSemaphore(*it);
}
m_newImageSemSet.clear();
deleteMutex(m_protectedImageSem);
}
/**
* Get a pointer to the AxisCamera object, if the object does not exist, create it
* To use the camera on port 2 of a cRIO-FRC, pass "192.168.0.90" to the first GetInstance call.
* @return reference to AxisCamera object
*/
AxisCamera &AxisCamera::GetInstance(const char *cameraIP)
{
if (NULL == _instance)
{
_instance = new AxisCamera(cameraIP);
_instance->m_videoServer = new PCVideoServer();
}
return *_instance;
}
/**
* Called by Java to delete the camera... how thoughtful
*/
void AxisCamera::DeleteInstance()
{
delete _instance;
_instance = NULL;
}
/**
* Return true if the latest image from the camera has not been retrieved by calling GetImage() yet.
* @return true if the image has not been retrieved yet.
*/
bool AxisCamera::IsFreshImage()
{
return m_freshImage;
}
/**
* Get the semaphore to be used to synchronize image access with camera acquisition
*
* Call semTake on the returned semaphore to block until a new image is acquired.
*
* The semaphore is owned by the AxisCamera class and will be deleted when the class is destroyed.
* @return A semaphore to notify when new image is received
*/
SEMAPHORE_ID AxisCamera::GetNewImageSem()
{
SEMAPHORE_ID sem = initializeSemaphore(SEMAPHORE_Q_PRIORITY, SEMAPHORE_EMPTY);
m_newImageSemSet.insert(sem);
return sem;
}
/**
* Get an image from the camera and store it in the provided image.
* @param image The imaq image to store the result in. This must be an HSL or RGB image
* This function is called by Java.
* @return 1 upon success, zero on a failure
*/
int AxisCamera::GetImage(Image* imaqImage)
{
if (m_protectedImageBuffer == NULL)
return 0;
Synchronized sync(m_protectedImageSem);
Priv_ReadJPEGString_C(imaqImage,
(unsigned char*)m_protectedImageBuffer, m_protectedImageSize);
m_freshImage = false;
return 1;
}
#if JAVA_CAMERA_LIB != 1
/**
* Get an image from the camera and store it in the provided image.
* @param image The image to store the result in. This must be an HSL or RGB image
* @return 1 upon success, zero on a failure
*/
int AxisCamera::GetImage(ColorImage* image)
{
return GetImage(image->GetImaqImage());
}
/**
* Instantiate a new image object and fill it with the latest image from the camera.
*
* The returned pointer is owned by the caller and is their responsibility to delete.
* @return a pointer to an HSLImage object
*/
HSLImage* AxisCamera::GetImage()
{
HSLImage *image = new HSLImage();
GetImage(image);
return image;
}
#endif
/**
* 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, int &destImageSize, int &destImageBufferSize)
{
Synchronized sync(m_protectedImageSem);
if (destImage == NULL)
wpi_setWPIErrorWithContext(NullParameter, "destImage must not be NULL");
if (m_protectedImageBuffer == NULL || m_protectedImageSize <= 0)
return 0; // if no source image
if (destImageBufferSize < m_protectedImageSize) // if current destination buffer too small
{
if (*destImage != NULL) delete [] *destImage;
destImageBufferSize = m_protectedImageSize + kImageBufferAllocationIncrement;
*destImage = new char[destImageBufferSize];
if (*destImage == NULL) return 0;
}
// copy this image into destination buffer
if (*destImage == NULL)
{
wpi_setWPIErrorWithContext(NullParameter, "*destImage must not be NULL");
}
// TODO: Is this copy realy necessary... perhaps we can simply transmit while holding the protected buffer
memcpy(*destImage, m_protectedImageBuffer, m_protectedImageSize);
destImageSize = m_protectedImageSize;
return 1;
}
/**
* Static interface that will cause an instantiation if necessary.
* This static stub is directly spawned as a task to read images from the camera.
*/
int AxisCamera::s_ImageStreamTaskFunction(AxisCamera *thisPtr)
{
return thisPtr->ImageStreamTaskFunction();
}
/**
* Task spawned by AxisCamera constructor to receive images from cam
* If setNewImageSem has been called, this function does a semGive on each new image
* Images can be accessed by calling getImage()
*/
int AxisCamera::ImageStreamTaskFunction()
{
// Loop on trying to setup the camera connection. This happens in a background
// thread so it shouldn't effect the operation of user programs.
while (1)
{
const char *requestString = "GET /mjpg/video.mjpg HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";
takeSemaphore(m_socketPossessionSem, SEMAPHORE_WAIT_FOREVER);
m_cameraSocket = CreateCameraSocket(requestString);
if (m_cameraSocket == ERROR)
{
// Don't hammer the camera if it isn't ready.
giveSemaphore(m_socketPossessionSem);
delayTicks(1000);
}
else
{
ReadImagesFromCamera();
}
}
return 0;
}
/**
* This function actually reads the images from the camera.
*/
int AxisCamera::ReadImagesFromCamera()
{
char *imgBuffer = NULL;
int imgBufferLength = 0;
//Infinite loop, task deletion handled by taskDeleteHook
// Socket cleanup handled by destructor
// TODO: these recv calls must be non-blocking. Otherwise if the camera
// fails during a read, the code hangs and never retries when the camera comes
// back up.
int counter = 2;
while (1)
{
char initialReadBuffer[kMaxPacketSize] = "";
char intermediateBuffer[1];
char *trailingPtr = initialReadBuffer;
int trailingCounter = 0;
while (counter)
{
// TODO: fix me... this cannot be the most efficient way to approach this, reading one byte at a time.
if(recv(m_cameraSocket, intermediateBuffer, 1, 0) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to read image header");
close (m_cameraSocket);
return ERROR;
}
strncat(initialReadBuffer, intermediateBuffer, 1);
// trailingCounter ensures that we start looking for the 4 byte string after
// there is at least 4 bytes total. Kind of obscure.
// look for 2 blank lines (\r\n)
if (NULL != strstr(trailingPtr, "\r\n\r\n"))
{
--counter;
}
if (++trailingCounter >= 4)
{
trailingPtr++;
}
}
counter = 1;
char *contentLength = strstr(initialReadBuffer, "Content-Length: ");
if (contentLength == NULL)
{
wpi_setWPIErrorWithContext(IncompatibleMode, "No content-length token found in packet");
close(m_cameraSocket);
return ERROR;
}
contentLength = contentLength + 16; // skip past "content length"
int readLength = atol(contentLength); // get the image byte count
// Make sure buffer is large enough
if (imgBufferLength < readLength)
{
if (imgBuffer) delete[] imgBuffer;
imgBufferLength = readLength + kImageBufferAllocationIncrement;
imgBuffer = new char[imgBufferLength];
if (imgBuffer == NULL)
{
imgBufferLength = 0;
continue;
}
}
// Read the image data for "Content-Length" bytes
int bytesRead = 0;
int remaining = readLength;
while(bytesRead < readLength)
{
int bytesThisRecv = recv(m_cameraSocket, &imgBuffer[bytesRead], remaining, 0);
bytesRead += bytesThisRecv;
remaining -= bytesThisRecv;
}
// Update image
UpdatePublicImageFromCamera(imgBuffer, readLength);
if (takeSemaphore(m_paramChangedSem, SEMAPHORE_NO_WAIT) == OK)
{
// params need to be updated: close the video stream; release the camera.
close(m_cameraSocket);
giveSemaphore(m_socketPossessionSem);
return 0;
}
}
}
/**
* Copy the image from private buffer to shared buffer.
* @param imgBuffer The buffer containing the image
* @param bufLength The length of the image
*/
void AxisCamera::UpdatePublicImageFromCamera(char *imgBuffer, int imgSize)
{
{
Synchronized sync(m_protectedImageSem);
// Adjust the buffer size if current destination buffer is too small.
if (m_protectedImageBufferLength < imgSize)
{
if (m_protectedImageBuffer != NULL) delete [] m_protectedImageBuffer;
m_protectedImageBufferLength = imgSize + kImageBufferAllocationIncrement;
m_protectedImageBuffer = new char[m_protectedImageBufferLength];
if (m_protectedImageBuffer == NULL)
{
m_protectedImageBufferLength = 0;
return;
}
}
memcpy(m_protectedImageBuffer, imgBuffer, imgSize);
m_protectedImageSize = imgSize;
}
m_freshImage = true;
// Notify everyone who is interested.
SemSet_t::iterator it = m_newImageSemSet.begin();
SemSet_t::iterator end = m_newImageSemSet.end();
for (;it != end; it++)
{
giveSemaphore(*it);
}
}
/**
* Implement the pure virtual interface so that when parameter changes require a restart, the image task can be bounced.
*/
void AxisCamera::RestartCameraTask()
{
m_imageStreamTask.Stop();
m_imageStreamTask.Start((int)this);
}
#if JAVA_CAMERA_LIB == 1
// C bindings used by Java
// These need to stay as is or Java has to change
void AxisCameraStart(const char *IPAddress)
{
#ifdef SVN_REV
if (strlen(SVN_REV))
{
printf("JavaCameraLib was compiled from SVN revision %s\n", SVN_REV);
}
else
{
printf("JavaCameraLib was compiled from a location that is not source controlled.\n");
}
#else
printf("JavaCameraLib was compiled without -D'SVN_REV=nnnn'\n");
#endif
AxisCamera::GetInstance(IPAddress);
}
int AxisCameraGetImage (Image* image)
{
return AxisCamera::GetInstance().GetImage(image);
}
void AxisCameraWriteBrightness(int brightness)
{
AxisCamera::GetInstance().WriteBrightness(brightness);
}
int AxisCameraGetBrightness()
{
return AxisCamera::GetInstance().GetBrightness();
}
void AxisCameraWriteWhiteBalance(AxisCameraParams::WhiteBalance_t whiteBalance)
{
AxisCamera::GetInstance().WriteWhiteBalance(whiteBalance);
}
AxisCameraParams::WhiteBalance_t AxisCameraGetWhiteBalance()
{
return AxisCamera::GetInstance().GetWhiteBalance();
}
void AxisCameraWriteColorLevel(int colorLevel)
{
AxisCamera::GetInstance().WriteColorLevel(colorLevel);
}
int AxisCameraGetColorLevel()
{
return AxisCamera::GetInstance().GetColorLevel();
}
void AxisCameraWriteExposureControl(AxisCameraParams::Exposure_t exposure)
{
AxisCamera::GetInstance().WriteExposureControl(exposure);
}
AxisCameraParams::Exposure_t AxisCameraGetExposureControl()
{
return AxisCamera::GetInstance().GetExposureControl();
}
void AxisCameraWriteExposurePriority(int exposure)
{
AxisCamera::GetInstance().WriteExposurePriority(exposure);
}
int AxisCameraGetExposurePriority()
{
return AxisCamera::GetInstance().GetExposurePriority();
}
void AxisCameraWriteMaxFPS(int maxFPS)
{
AxisCamera::GetInstance().WriteMaxFPS(maxFPS);
}
int AxisCameraGetMaxFPS()
{
return AxisCamera::GetInstance().GetMaxFPS();
}
void AxisCameraWriteResolution(AxisCameraParams::Resolution_t resolution)
{
AxisCamera::GetInstance().WriteResolution(resolution);
}
AxisCameraParams::Resolution_t AxisCameraGetResolution()
{
return AxisCamera::GetInstance().GetResolution();
}
void AxisCameraWriteCompression(int compression)
{
AxisCamera::GetInstance().WriteCompression(compression);
}
int AxisCameraGetCompression()
{
return AxisCamera::GetInstance().GetCompression();
}
void AxisCameraWriteRotation(AxisCameraParams::Rotation_t rotation)
{
AxisCamera::GetInstance().WriteRotation(rotation);
}
AxisCameraParams::Rotation_t AxisCameraGetRotation()
{
return AxisCamera::GetInstance().GetRotation();
}
void AxisCameraDeleteInstance()
{
AxisCamera::DeleteInstance();
}
int AxisCameraFreshImage()
{
return AxisCamera::GetInstance().IsFreshImage();
}
#endif // JAVA_CAMERA_LIB == 1

469
wpilibc/src/main/native/Vision/AxisCameraParams.cpp
View File

@@ -1,469 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Vision/AxisCameraParams.h"
#include "Vision/AxisCamera.h"
#include <inetLib.h>
#include "pcre.h"
#include <sockLib.h>
#include <string.h>
#include "HAL/cpp/Synchronized.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"
#if JAVA_CAMERA_LIB != 1
#include "DriverStation.h"
#endif
static const char *const kRotationChoices[] = {"0", "180"};
static const char *const kResolutionChoices[] = {"640x480", "640x360", "320x240", "160x120"};
static const char *const kExposureControlChoices[] = { "automatic", "hold", "flickerfree50", "flickerfree60" };
static const char *const kWhiteBalanceChoices[] = { "auto", "holdwb", "fixed_outdoor1",
"fixed_outdoor2", "fixed_indoor", "fixed_fluor1", "fixed_fluor2" };
/**
* AxisCamera constructor
*/
AxisCameraParams::AxisCameraParams(const char* ipAddress)
: m_paramTask("paramTask", (FUNCPTR) s_ParamTaskFunction)
, m_paramChangedSem (NULL)
, m_socketPossessionSem (NULL)
, m_brightnessParam (NULL)
, m_compressionParam (NULL)
, m_exposurePriorityParam (NULL)
, m_colorLevelParam (NULL)
, m_maxFPSParam (NULL)
, m_rotationParam (NULL)
, m_resolutionParam (NULL)
, m_exposureControlParam (NULL)
, m_whiteBalanceParam (NULL)
{
if (ipAddress == NULL || strlen(ipAddress) == 0)
{
#if JAVA_CAMERA_LIB == 1
wpi_setWPIErrorWithContext(ParameterOutOfRange, "IP Address must be specified");
return;
#else
DriverStation *ds = DriverStation::GetInstance();
ds->WaitForData();
uint16_t teamNumber = ds->GetTeamNumber();
char cameraIP[16];
snprintf(cameraIP, 16, "10.%d.%d.11", teamNumber / 100, teamNumber % 100);
m_ipAddress = inet_addr(cameraIP);
#endif
}
else
{
m_ipAddress = inet_addr((char*)ipAddress);
}
if (m_ipAddress == (u_long)ERROR)
{
wpi_setErrnoError();
return;
}
m_brightnessParam = new IntCameraParameter("ImageSource.I0.Sensor.Brightness=%i",
"root.ImageSource.I0.Sensor.Brightness=(.*)", false);
m_parameters.push_back(m_brightnessParam);
m_colorLevelParam = new IntCameraParameter("ImageSource.I0.Sensor.ColorLevel=%i",
"root.ImageSource.I0.Sensor.ColorLevel=(.*)", false);
m_parameters.push_back(m_colorLevelParam);
m_exposurePriorityParam = new IntCameraParameter("ImageSource.I0.Sensor.exposurePriority=%i",
"root.ImageSource.I0.Sensor.ExposurePriority=(.*)", false);
m_parameters.push_back(m_exposurePriorityParam);
m_compressionParam = new IntCameraParameter("Image.I0.Appearance.Compression=%i",
"root.Image.I0.Appearance.Compression=(.*)", true);
m_parameters.push_back(m_compressionParam);
m_maxFPSParam = new IntCameraParameter("Image.I0.Stream.FPS=%i",
"root.Image.I0.Stream.FPS=(.*)", false);
m_parameters.push_back(m_maxFPSParam);
m_rotationParam = new EnumCameraParameter("Image.I0.Appearance.Rotation=%s",
"root.Image.I0.Appearance.Rotation=(.*)", true, kRotationChoices, sizeof(kRotationChoices)/sizeof(kRotationChoices[0]));
m_parameters.push_back(m_rotationParam);
m_resolutionParam = new EnumCameraParameter("Image.I0.Appearance.Resolution=%s",
"root.Image.I0.Appearance.Resolution=(.*)", true, kResolutionChoices, sizeof(kResolutionChoices)/sizeof(kResolutionChoices[0]));
m_parameters.push_back(m_resolutionParam);
m_exposureControlParam = new EnumCameraParameter("ImageSource.I0.Sensor.Exposure=%s",
"root.ImageSource.I0.Sensor.Exposure=(.*)", false, kExposureControlChoices, sizeof(kExposureControlChoices)/sizeof(kExposureControlChoices[0]));
m_parameters.push_back(m_exposureControlParam);
m_whiteBalanceParam = new EnumCameraParameter("ImageSource.IO.Sensor.WhiteBalance=%s",
"root.ImageSource.I0.Sensor.WhiteBalance=(.*)", false, kWhiteBalanceChoices, sizeof(kWhiteBalanceChoices)/sizeof(kWhiteBalanceChoices[0]));
m_parameters.push_back(m_whiteBalanceParam);
m_paramChangedSem = initializeSemaphore(SEMAPHORE_Q_PRIORITY, SEMAPHORE_EMPTY);
m_socketPossessionSem = initializeSemaphore(SEMAPHORE_Q_PRIORITY, SEMAPHORE_FULL);
m_paramTask.Start((int)this);
}
/**
* Destructor
*/
AxisCameraParams::~AxisCameraParams()
{
m_paramTask.Stop();
deleteSemaphore(m_socketPossessionSem);
deleteSemaphore(m_paramChangedSem);
delete m_whiteBalanceParam;
delete m_exposureControlParam;
delete m_resolutionParam;
delete m_rotationParam;
delete m_maxFPSParam;
delete m_compressionParam;
delete m_exposurePriorityParam;
delete m_colorLevelParam;
delete m_brightnessParam;
}
/**
* Static function to start the parameter updating task
*/
int AxisCameraParams::s_ParamTaskFunction(AxisCameraParams* thisPtr)
{
return thisPtr->ParamTaskFunction();
}
/**
* Main loop of the parameter task.
* This loop runs continuously checking parameters from the camera for
* posted changes and updating them if necessary.
*/
// TODO: need to synchronize the actual setting of parameters (the assignment statement)
int AxisCameraParams::ParamTaskFunction()
{
static bool firstTime = true;
while (true)
{
takeSemaphore(m_socketPossessionSem, SEMAPHORE_WAIT_FOREVER);
if (firstTime)
{
while (ReadCamParams() == 0);
firstTime = false;
}
bool restartRequired = false;
ParameterVector_t::iterator it = m_parameters.begin();
ParameterVector_t::iterator end = m_parameters.end();
for(; it != end; it++)
{
bool changed = false;
char param[150];
restartRequired |= (*it)->CheckChanged(changed, param);
if (changed)
{
UpdateCamParam(param);
}
}
if (restartRequired)
{
RestartCameraTask();
}
giveSemaphore(m_socketPossessionSem);
}
return 0;
}
/**
* Write the brightness value to the camera.
* @param brightness valid values 0 .. 100
*/
void AxisCameraParams::WriteBrightness(int brightness)
{
m_brightnessParam->SetValue(brightness);
giveSemaphore(m_paramChangedSem);
}
/**
* Get the brightness value.
* @return Brightness value from the camera.
*/
int AxisCameraParams::GetBrightness()
{
return m_brightnessParam->GetValue();
}
/**
* Set the white balance value.
* @param whiteBalance Valid values from the WhiteBalance_t enum.
*/
void AxisCameraParams::WriteWhiteBalance(WhiteBalance_t whiteBalance)
{
m_whiteBalanceParam->SetValue(whiteBalance);
giveSemaphore(m_paramChangedSem);
}
/**
* Retrieve the current white balance parameter.
* @return The white balance value.
*/
AxisCameraParams::WhiteBalance_t AxisCameraParams::GetWhiteBalance()
{
return (WhiteBalance_t) m_whiteBalanceParam->GetValue();
}
/**
* Write the color level to the camera.
* @param colorLevel valid values are 0 .. 100
*/
void AxisCameraParams::WriteColorLevel(int colorLevel)
{
m_colorLevelParam->SetValue(colorLevel);
giveSemaphore(m_paramChangedSem);
}
/**
* Retrieve the color level from the camera.
* @Returns the camera color level.
*/
int AxisCameraParams::GetColorLevel()
{
return m_colorLevelParam->GetValue();
}
/**
* Write the exposure control value to the camera.
* @param exposureControl A mode to write in the Exposure_t enum.
*/
void AxisCameraParams::WriteExposureControl(Exposure_t exposureControl)
{
m_exposureControlParam->SetValue(exposureControl);
giveSemaphore(m_paramChangedSem);
}
/**
* Get the exposure value from the camera.
* @returns the exposure value from the camera.
*/
AxisCameraParams::Exposure_t AxisCameraParams::GetExposureControl()
{
return (Exposure_t) m_exposureControlParam->GetValue();
}
/**
* Write resolution value to camera.
* @param resolution The camera resolution value to write to the camera. Use the Resolution_t enum.
*/
void AxisCameraParams::WriteResolution(Resolution_t resolution)
{
m_resolutionParam->SetValue(resolution);
giveSemaphore(m_paramChangedSem);
}
/**
* Get the resolution value from the camera.
* @returns resultion value for the camera.
*/
AxisCameraParams::Resolution_t AxisCameraParams::GetResolution()
{
return (Resolution_t) m_resolutionParam->GetValue();
}
/**
* Write the exposre priority value to the camera.
* @param exposurePriority Valid values are 0, 50, 100.
* 0 = Prioritize image quality
* 50 = None
* 100 = Prioritize frame rate
*/
void AxisCameraParams::WriteExposurePriority(int exposurePriority)
{
m_exposurePriorityParam->SetValue(exposurePriority);
giveSemaphore(m_paramChangedSem);
}
int AxisCameraParams::GetExposurePriority()
{
return m_exposurePriorityParam->GetValue();
}
/**
* Write the rotation value to the camera.
* If you mount your camera upside down, use this to adjust the image for you.
* @param rotation The image from the Rotation_t enum in AxisCameraParams (kRotation_0 or kRotation_180)
*/
void AxisCameraParams::WriteRotation(Rotation_t rotation)
{
m_rotationParam->SetValue(rotation);
giveSemaphore(m_paramChangedSem);
}
/**
* Get the rotation value from the camera.
* @return The rotation value from the camera (Rotation_t).
*/
AxisCameraParams::Rotation_t AxisCameraParams::GetRotation()
{
return (Rotation_t) m_rotationParam->GetValue();
}
/**
* Write the compression value to the camera.
* @param compression Values between 0 and 100.
*/
void AxisCameraParams::WriteCompression(int compression)
{
m_compressionParam->SetValue(compression);
giveSemaphore(m_paramChangedSem);
}
/**
* Get the compression value from the camera.
* @return The cached compression value from the camera.
*/
int AxisCameraParams::GetCompression()
{
return m_compressionParam->GetValue();
}
/**
* Write the maximum frames per second that the camera should send
* Write 0 to send as many as possible.
* @param maxFPS The number of frames the camera should send in a second, exposure permitting.
*/
void AxisCameraParams::WriteMaxFPS(int maxFPS)
{
m_maxFPSParam->SetValue(maxFPS);
giveSemaphore(m_paramChangedSem);
}
/**
* Get the max number of frames per second that the camera will send
* @return Maximum frames per second.
*/
int AxisCameraParams::GetMaxFPS()
{
return m_maxFPSParam->GetValue();
}
/**
* Update a camera parameter.
* Write a camera parameter to the camera when it has bene changed.
* @param param the string to insert into the http request.
* @returns 0 if it failed, otherwise nonzero.
*/
int AxisCameraParams::UpdateCamParam(const char* param)
{
const char *requestString =
"GET /axis-cgi/admin/param.cgi?action=update&%s HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";
char completedRequest[1024];
sprintf(completedRequest, requestString, param);
// Send request
int camSocket = CreateCameraSocket(completedRequest);
if (camSocket == ERROR)
{
printf("UpdateCamParam failed: %s\n", param);
return 0;
}
close(camSocket);
return 1;
}
/**
* Read the full param list from camera, use regular expressions to find the bits we care about
* assign values to member variables.
*/
int AxisCameraParams::ReadCamParams()
{
const char * requestString =
"GET /axis-cgi/admin/param.cgi?action=list HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";
int camSocket = CreateCameraSocket(requestString);
if (camSocket == ERROR)
{
return 0;
}
// Allocate on the heap since it is very large and only needed once
char *readBuffer = new char[27000];
int totalRead = 0;
while (1)
{
wpi_assert(totalRead < 26000);
int bytesRead = recv(camSocket, &readBuffer[totalRead], 1000, 0);
if (bytesRead == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to read image header");
close(camSocket);
return 0;
}
else if (bytesRead <= 0)
{
break;
}
totalRead += bytesRead;
}
readBuffer[totalRead] = '\0';
ParameterVector_t::iterator it = m_parameters.begin();
ParameterVector_t::iterator end = m_parameters.end();
for(; it != end; it++)
{
(*it)->GetParamFromString(readBuffer, totalRead);
}
close(camSocket);
delete [] readBuffer;
return 1;
}
/*
* Create a socket connected to camera
* Used to create a connection to the camera by both AxisCameraParams and AxisCamera.
* @param requestString The initial request string to send upon successful connection.
* @return ERROR if failed, socket handle if successful.
*/
int AxisCameraParams::CreateCameraSocket(const char *requestString)
{
int sockAddrSize;
struct sockaddr_in serverAddr;
int camSocket;
/* create socket */
if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to create the camera socket");
return ERROR;
}
sockAddrSize = sizeof(struct sockaddr_in);
bzero((char *) &serverAddr, sockAddrSize);
serverAddr.sin_family = AF_INET;
serverAddr.sin_len = (u_char) sockAddrSize;
serverAddr.sin_port = htons(80);
serverAddr.sin_addr.s_addr = m_ipAddress;
/* connect to server */
struct timeval connectTimeout;
connectTimeout.tv_sec = 5;
connectTimeout.tv_usec = 0;
if (connectWithTimeout(camSocket, (struct sockaddr *) &serverAddr, sockAddrSize, &connectTimeout) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to connect to the camera");
close(camSocket);
return ERROR;
}
int sent = send(camSocket, requestString, strlen(requestString), 0);
if (sent == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to send a request to the camera");
close(camSocket);
return ERROR;
}
return camSocket;
}

223
wpilibc/src/main/native/Vision/BinaryImage.cpp
View File

@@ -1,223 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "BinaryImage.h"
#include "WPIErrors.h"
#include <cstring>
/** Private NI function needed to write to the VxWorks target */
IMAQ_FUNC int Priv_SetWriteFileAllowed(uint32_t enable);
BinaryImage::BinaryImage() : MonoImage()
{
}
BinaryImage::~BinaryImage()
{
}
/**
* Get then number of particles for the image.
* @returns the number of particles found for the image.
*/
int BinaryImage::GetNumberParticles()
{
int numParticles = 0;
int success = imaqCountParticles(m_imaqImage, 1, &numParticles);
wpi_setImaqErrorWithContext(success, "Error counting particles");
return numParticles;
}
/**
* Get a single particle analysis report.
* Get one (of possibly many) particle analysis reports for an image.
* @param particleNumber Which particle analysis report to return.
* @returns the selected particle analysis report
*/
ParticleAnalysisReport BinaryImage::GetParticleAnalysisReport(int particleNumber)
{
ParticleAnalysisReport par;
GetParticleAnalysisReport(particleNumber, &par);
return par;
}
/**
* Get a single particle analysis report.
* Get one (of possibly many) particle analysis reports for an image.
* This version could be more efficient when copying many reports.
* @param particleNumber Which particle analysis report to return.
* @param par the selected particle analysis report
*/
void BinaryImage::GetParticleAnalysisReport(int particleNumber, ParticleAnalysisReport *par)
{
int success;
int numParticles = 0;
success = imaqGetImageSize(m_imaqImage, &par->imageWidth, &par->imageHeight);
wpi_setImaqErrorWithContext(success, "Error getting image size");
if (StatusIsFatal())
return;
success = imaqCountParticles(m_imaqImage, 1, &numParticles);
wpi_setImaqErrorWithContext(success, "Error counting particles");
if (StatusIsFatal())
return;
if (particleNumber >= numParticles)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "particleNumber");
return;
}
par->particleIndex = particleNumber;
// Don't bother measuring the rest of the particle if one fails
bool good = ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_X, &par->center_mass_x);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_Y, &par->center_mass_y);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA, &par->particleArea);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_TOP, &par->boundingRect.top);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_LEFT, &par->boundingRect.left);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_HEIGHT, &par->boundingRect.height);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_WIDTH, &par->boundingRect.width);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_IMAGE_AREA, &par->particleToImagePercent);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA, &par->particleQuality);
if (good)
{
/* normalized position (-1 to 1) */
par->center_mass_x_normalized = NormalizeFromRange(par->center_mass_x, par->imageWidth);
par->center_mass_y_normalized = NormalizeFromRange(par->center_mass_y, par->imageHeight);
}
}
/**
* Get an ordered vector of particles for the image.
* Create a vector of particle analysis reports sorted by size for an image.
* The vector contains the actual report structures.
* @returns a pointer to the vector of particle analysis reports. The caller must delete the
* vector when finished using it.
*/
vector<ParticleAnalysisReport>* BinaryImage::GetOrderedParticleAnalysisReports()
{
vector<ParticleAnalysisReport>* particles = new vector<ParticleAnalysisReport>;
int particleCount = GetNumberParticles();
for(int particleIndex = 0; particleIndex < particleCount; particleIndex++)
{
particles->push_back(GetParticleAnalysisReport(particleIndex));
}
// TODO: This is pretty inefficient since each compare in the sort copies
// both reports being compared... do it manually instead... while we're
// at it, we should provide a version that allows a preallocated buffer of
// ParticleAnalysisReport structures
sort(particles->begin(), particles->end(), CompareParticleSizes);
return particles;
}
/**
* Write a binary image to flash.
* Writes the binary image to flash on the cRIO for later inspection.
* @param fileName the name of the image file written to the flash.
*/
void BinaryImage::Write(const char *fileName)
{
RGBValue colorTable[256];
Priv_SetWriteFileAllowed(1);
memset(colorTable, 0, sizeof(colorTable));
colorTable[0].R = 0;
colorTable[1].R = 255;
colorTable[0].G = colorTable[1].G = 0;
colorTable[0].B = colorTable[1].B = 0;
colorTable[0].alpha = colorTable[1].alpha = 0;
imaqWriteFile(m_imaqImage, fileName, colorTable);
}
/**
* Measure a single parameter for an image.
* Get the measurement for a single parameter about an image by calling the imaqMeasureParticle
* function for the selected parameter.
* @param particleNumber which particle in the set of particles
* @param whatToMeasure the imaq MeasurementType (what to measure)
* @param result the value of the measurement
* @returns false on failure, true on success
*/
bool BinaryImage::ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, int *result)
{
double resultDouble;
bool success = ParticleMeasurement(particleNumber, whatToMeasure, &resultDouble);
*result = (int)resultDouble;
return success;
}
/**
* Measure a single parameter for an image.
* Get the measurement for a single parameter about an image by calling the imaqMeasureParticle
* function for the selected parameter.
* @param particleNumber which particle in the set of particles
* @param whatToMeasure the imaq MeasurementType (what to measure)
* @param result the value of the measurement
* @returns true on failure, false on success
*/
bool BinaryImage::ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, double *result)
{
int success;
success = imaqMeasureParticle(m_imaqImage, particleNumber, 0, whatToMeasure, result);
wpi_setImaqErrorWithContext(success, "Error measuring particle");
return !StatusIsFatal();
}
//Normalizes to [-1,1]
double BinaryImage::NormalizeFromRange(double position, int range)
{
return (position * 2.0 / (double)range) - 1.0;
}
/**
* The compare helper function for sort.
* This function compares two particle analysis reports as a helper for the sort function.
* @param particle1 The first particle to compare
* @param particle2 the second particle to compare
* @returns true if particle1 is greater than particle2
*/
bool BinaryImage::CompareParticleSizes(ParticleAnalysisReport particle1, ParticleAnalysisReport particle2)
{
//we want descending sort order
return particle1.particleToImagePercent > particle2.particleToImagePercent;
}
BinaryImage *BinaryImage::RemoveSmallObjects(bool connectivity8, int erosions)
{
BinaryImage *result = new BinaryImage();
int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage, connectivity8, erosions, IMAQ_KEEP_LARGE, NULL);
wpi_setImaqErrorWithContext(success, "Error in RemoveSmallObjects");
return result;
}
BinaryImage *BinaryImage::RemoveLargeObjects(bool connectivity8, int erosions)
{
BinaryImage *result = new BinaryImage();
int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage, connectivity8, erosions, IMAQ_KEEP_SMALL, NULL);
wpi_setImaqErrorWithContext(success, "Error in RemoveLargeObjects");
return result;
}
BinaryImage *BinaryImage::ConvexHull(bool connectivity8)
{
BinaryImage *result = new BinaryImage();
int success = imaqConvexHull(result->GetImaqImage(), m_imaqImage, connectivity8);
wpi_setImaqErrorWithContext(success, "Error in convex hull operation");
return result;
}
BinaryImage *BinaryImage::ParticleFilter(ParticleFilterCriteria2 *criteria, int criteriaCount)
{
BinaryImage *result = new BinaryImage();
int numParticles;
ParticleFilterOptions2 filterOptions = {0, 0, 0, 1};
int success = imaqParticleFilter4(result->GetImaqImage(), m_imaqImage, criteria, criteriaCount, &filterOptions, NULL, &numParticles);
wpi_setImaqErrorWithContext(success, "Error in particle filter operation");
return result;
}

465
wpilibc/src/main/native/Vision/ColorImage.cpp
View File

@@ -1,465 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "ColorImage.h"
#include "WPIErrors.h"
ColorImage::ColorImage(ImageType type) : ImageBase(type)
{
}
ColorImage::~ColorImage()
{
}
/**
* Perform a threshold operation on a ColorImage.
* Perform a threshold operation on a ColorImage using the ColorMode supplied
* as a parameter.
* @param colorMode The type of colorspace this operation should be performed in
* @returns a pointer to a binary image
*/
BinaryImage *ColorImage::ComputeThreshold(ColorMode colorMode,
int low1, int high1,
int low2, int high2,
int low3, int high3)
{
BinaryImage *result = new BinaryImage();
Range range1 = {low1, high1},
range2 = {low2, high2},
range3 = {low3, high3};
int success = imaqColorThreshold(result->GetImaqImage(), m_imaqImage, 1, colorMode, &range1, &range2, &range3);
wpi_setImaqErrorWithContext(success, "ImaqThreshold error");
return result;
}
/**
* Perform a threshold in RGB space.
* @param redLow Red low value
* @param redHigh Red high value
* @param greenLow Green low value
* @param greenHigh Green high value
* @param blueLow Blue low value
* @param blueHigh Blue high value
* @returns A pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdRGB(int redLow, int redHigh, int greenLow, int greenHigh, int blueLow, int blueHigh)
{
return ComputeThreshold(IMAQ_RGB, redLow, redHigh, greenLow, greenHigh, blueLow, blueHigh);
}
/**
* Perform a threshold in RGB space.
* @param threshold a reference to the Threshold object to use.
* @returns A pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdRGB(Threshold &t)
{
return ComputeThreshold(IMAQ_RGB, t.plane1Low, t.plane1High,
t.plane2Low, t.plane2High,
t.plane3Low, t.plane3High);
}
/**
* Perform a threshold in HSL space.
* @param hueLow Low value for hue
* @param hueHigh High value for hue
* @param saturationLow Low value for saturation
* @param saturationHigh High value for saturation
* @param luminenceLow Low value for luminence
* @param luminenceHigh High value for luminence
* @returns a pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdHSL(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int luminenceLow, int luminenceHigh)
{
return ComputeThreshold(IMAQ_HSL, hueLow, hueHigh, saturationLow, saturationHigh, luminenceLow, luminenceHigh);
}
/**
* Perform a threshold in HSL space.
* @param threshold a reference to the Threshold object to use.
* @returns A pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdHSL(Threshold &t)
{
return ComputeThreshold(IMAQ_HSL, t.plane1Low, t.plane1High,
t.plane2Low, t.plane2High,
t.plane3Low, t.plane3High);
}
/**
* Perform a threshold in HSV space.
* @param hueLow Low value for hue
* @param hueHigh High value for hue
* @param saturationLow Low value for saturation
* @param saturationHigh High value for saturation
* @param valueLow Low value
* @param valueHigh High value
* @returns a pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdHSV(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int valueLow, int valueHigh)
{
return ComputeThreshold(IMAQ_HSV, hueLow, hueHigh, saturationLow, saturationHigh, valueLow, valueHigh);
}
/**
* Perform a threshold in HSV space.
* @param threshold a reference to the Threshold object to use.
* @returns A pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdHSV(Threshold &t)
{
return ComputeThreshold(IMAQ_HSV, t.plane1Low, t.plane1High,
t.plane2Low, t.plane2High,
t.plane3Low, t.plane3High);
}
/**
* Perform a threshold in HSI space.
* @param hueLow Low value for hue
* @param hueHigh High value for hue
* @param saturationLow Low value for saturation
* @param saturationHigh High value for saturation
* @param valueLow Low intensity
* @param valueHigh High intensity
* @returns a pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdHSI(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int intensityLow, int intensityHigh)
{
return ComputeThreshold(IMAQ_HSI, hueLow, hueHigh, saturationLow, saturationHigh, intensityLow, intensityHigh);
}
/**
* Perform a threshold in HSI space.
* @param threshold a reference to the Threshold object to use.
* @returns A pointer to a BinaryImage that represents the result of the threshold operation.
*/
BinaryImage *ColorImage::ThresholdHSI(Threshold &t)
{
return ComputeThreshold(IMAQ_HSI, t.plane1Low, t.plane1High,
t.plane2Low, t.plane2High,
t.plane3Low, t.plane3High);
}
/**
* Extract a color plane from the image
* @param mode The ColorMode to use for the plane extraction
* @param planeNumber Which plane is to be extracted
* @returns A pointer to a MonoImage that represents the extracted plane.
*/
MonoImage *ColorImage::ExtractColorPlane(ColorMode mode, int planeNumber)
{
MonoImage *result = new MonoImage();
if (m_imaqImage == NULL)
wpi_setWPIError(NullParameter);
int success = imaqExtractColorPlanes(m_imaqImage,
mode,
(planeNumber == 1) ? result->GetImaqImage() : NULL,
(planeNumber == 2) ? result->GetImaqImage() : NULL,
(planeNumber == 3) ? result->GetImaqImage() : NULL);
wpi_setImaqErrorWithContext(success, "Imaq ExtractColorPlanes failed");
return result;
}
/*
* Extract the first color plane for an image.
* @param mode The color mode in which to operate
* @returns a pointer to a MonoImage that is the extracted plane.
*/
MonoImage *ColorImage::ExtractFirstColorPlane(ColorMode mode)
{
return ExtractColorPlane(mode, 1);
}
/*
* Extract the second color plane for an image.
* @param mode The color mode in which to operate
* @returns a pointer to a MonoImage that is the extracted plane.
*/
MonoImage *ColorImage::ExtractSecondColorPlane(ColorMode mode)
{
return ExtractColorPlane(mode, 2);
}
/*
* Extract the third color plane for an image.
* @param mode The color mode in which to operate
* @returns a pointer to a MonoImage that is the extracted plane.
*/
MonoImage *ColorImage::ExtractThirdColorPlane(ColorMode mode)
{
return ExtractColorPlane(mode, 3);
}
/*
* Extract the red plane from an RGB image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetRedPlane()
{
return ExtractFirstColorPlane(IMAQ_RGB);
}
/*
* Extract the green plane from an RGB image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetGreenPlane()
{
return ExtractSecondColorPlane(IMAQ_RGB);
}
/*
* Extract the blue plane from an RGB image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetBluePlane()
{
return ExtractThirdColorPlane(IMAQ_RGB);
}
/*
* Extract the Hue plane from an HSL image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetHSLHuePlane()
{
return ExtractFirstColorPlane(IMAQ_HSL);
}
/*
* Extract the Hue plane from an HSV image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetHSVHuePlane()
{
return ExtractFirstColorPlane(IMAQ_HSV);
}
/*
* Extract the Hue plane from an HSI image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetHSIHuePlane()
{
return ExtractFirstColorPlane(IMAQ_HSI);
}
/*
* Extract the Luminance plane from an HSL image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetLuminancePlane()
{
return ExtractThirdColorPlane(IMAQ_HSL);
}
/*
* Extract the Value plane from an HSV image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetValuePlane()
{
return ExtractThirdColorPlane(IMAQ_HSV);
}
/*
* Extract the Intensity plane from an HSI image.
* @returns a pointer to a MonoImage that is the extraced plane.
*/
MonoImage *ColorImage::GetIntensityPlane()
{
return ExtractThirdColorPlane(IMAQ_HSI);
}
/**
* Replace a plane in the ColorImage with a MonoImage
* Replaces a single plane in the image with a MonoImage
* @param mode The ColorMode in which to operate
* @param plane The pointer to the replacement plane as a MonoImage
* @param planeNumber The plane number (1, 2, 3) to replace
*/
void ColorImage::ReplacePlane(ColorMode mode, MonoImage *plane, int planeNumber) {
int success = imaqReplaceColorPlanes(m_imaqImage,
(const Image*) m_imaqImage,
mode,
(planeNumber == 1) ? plane->GetImaqImage() : NULL,
(planeNumber == 2) ? plane->GetImaqImage() : NULL,
(planeNumber == 3) ? plane->GetImaqImage() : NULL);
wpi_setImaqErrorWithContext(success, "Imaq ReplaceColorPlanes failed");
}
/**
* Replace the first color plane with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceFirstColorPlane(ColorMode mode, MonoImage *plane)
{
ReplacePlane(mode, plane, 1);
}
/**
* Replace the second color plane with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceSecondColorPlane(ColorMode mode, MonoImage *plane)
{
ReplacePlane(mode, plane, 2);
}
/**
* Replace the third color plane with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceThirdColorPlane(ColorMode mode, MonoImage *plane)
{
ReplacePlane(mode, plane, 3);
}
/**
* Replace the red color plane with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceRedPlane(MonoImage *plane)
{
ReplaceFirstColorPlane(IMAQ_RGB, plane);
}
/**
* Replace the green color plane with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceGreenPlane(MonoImage *plane)
{
ReplaceSecondColorPlane(IMAQ_RGB, plane);
}
/**
* Replace the blue color plane with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceBluePlane(MonoImage *plane)
{
ReplaceThirdColorPlane(IMAQ_RGB, plane);
}
/**
* Replace the Hue color plane in a HSL image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceHSLHuePlane(MonoImage *plane)
{
return ReplaceFirstColorPlane(IMAQ_HSL, plane);
}
/**
* Replace the Hue color plane in a HSV image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceHSVHuePlane(MonoImage *plane)
{
return ReplaceFirstColorPlane(IMAQ_HSV, plane);
}
/**
* Replace the first Hue plane in a HSI image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceHSIHuePlane(MonoImage *plane)
{
return ReplaceFirstColorPlane(IMAQ_HSI, plane);
}
/**
* Replace the Saturation color plane in an HSL image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceHSLSaturationPlane(MonoImage *plane)
{
return ReplaceSecondColorPlane(IMAQ_HSL, plane);
}
/**
* Replace the Saturation color plane in a HSV image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceHSVSaturationPlane(MonoImage *plane)
{
return ReplaceSecondColorPlane(IMAQ_HSV, plane);
}
/**
* Replace the Saturation color plane in a HSI image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceHSISaturationPlane(MonoImage *plane)
{
return ReplaceSecondColorPlane(IMAQ_HSI, plane);
}
/**
* Replace the Luminance color plane in an HSL image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceLuminancePlane(MonoImage *plane)
{
return ReplaceThirdColorPlane(IMAQ_HSL, plane);
}
/**
* Replace the Value color plane in an HSV with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceValuePlane(MonoImage *plane)
{
return ReplaceThirdColorPlane(IMAQ_HSV, plane);
}
/**
* Replace the Intensity color plane in a HSI image with a MonoImage.
* @param mode The color mode in which to operate.
* @param plane A pointer to a MonoImage that will replace the specified color plane.
*/
void ColorImage::ReplaceIntensityPlane(MonoImage *plane)
{
return ReplaceThirdColorPlane(IMAQ_HSI, plane);
}
//TODO: frcColorEqualize(Image* dest, const Image* source, int colorEqualization) needs to be modified
//The colorEqualization parameter is discarded and is set to TRUE in the call to imaqColorEqualize.
void ColorImage::Equalize(bool allPlanes)
{
// Note that this call uses NI-defined TRUE and FALSE
int success = imaqColorEqualize(m_imaqImage, (const Image*) m_imaqImage, (allPlanes) ? TRUE : FALSE);
wpi_setImaqErrorWithContext(success, "Imaq ColorEqualize error");
}
void ColorImage::ColorEqualize()
{
Equalize(true);
}
void ColorImage::LuminanceEqualize()
{
Equalize(false);
}

67
wpilibc/src/main/native/Vision/EnumCameraParameter.cpp
View File

@@ -1,67 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "EnumCameraParameter.h"
#include <stdio.h>
#include <string.h>
/**
* Constructor for an enumeration camera parameter.
* Enumeration camera parameters have lists of value choices and strings that go
* with them. There are also C++ enumerations to go along with them.
* @param setString The string for an HTTP request to set the value.
* @param getString The string for an HTTP request to get the value.
* @param choices An array of strings of the parameter choices set in the http strings.
* @param numChoices The number of choices in the enumeration set.
*/
EnumCameraParameter::EnumCameraParameter(const char *setString, const char *getString, bool requiresRestart,
const char *const*choices, int numChoices)
: IntCameraParameter(setString, getString, requiresRestart)
{
m_enumValues = choices;
m_numChoices = numChoices;
}
/*
* Check if an enumeration camera parameter has changed.
* Check if the parameter has changed and update the camera if it has. This is called
* from a loop in the parameter checker task.
* @returns true if the camera needs to restart
*/
bool EnumCameraParameter::CheckChanged(bool &changed, char *param)
{
changed = m_changed;
if (m_changed)
{
m_changed = false;
sprintf(param, m_setString, m_enumValues[m_value]);
return m_requiresRestart;
}
return false;
}
/**
* Extract the parameter value from a string.
* Extract the parameter value from the camera status message.
* @param string The string returned from the camera.
* @param length The length of the string from the camera.
*/
void EnumCameraParameter::GetParamFromString(const char *string, int stringLength)
{
char resultString[50];
if (SearchForParam(m_getString, string, stringLength, resultString) < 0) return;
for (int i = 0; i < m_numChoices; i++)
{
if (strcmp(resultString, m_enumValues[i]) == 0)
{
if (!m_changed) // don't change parameter that's been set in code
{
m_value = i;
}
}
}
}

28
wpilibc/src/main/native/Vision/HSLImage.cpp
View File

@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "HSLImage.h"
/**
* Create a new image that uses the Hue, Saturation, and Luminance planes.
*/
HSLImage::HSLImage() : ColorImage(IMAQ_IMAGE_HSL)
{
}
/**
* Create a new image by loading a file.
* @param fileName The path of the file to load.
*/
HSLImage::HSLImage(const char *fileName) : ColorImage(IMAQ_IMAGE_HSL)
{
int success = imaqReadFile(m_imaqImage, fileName, NULL, NULL);
wpi_setImaqErrorWithContext(success, "Imaq ReadFile error");
}
HSLImage::~HSLImage()
{
}

77
wpilibc/src/main/native/Vision/ImageBase.cpp
View File

@@ -1,77 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "ImageBase.h"
#include "nivision.h"
/** Private NI function needed to write to the VxWorks target */
IMAQ_FUNC int Priv_SetWriteFileAllowed(uint32_t enable);
/**
* Create a new instance of an ImageBase.
* Imagebase is the base of all the other image classes. The constructor
* creates any type of image and stores the pointer to it in the class.
* @param type The type of image to create
*/
ImageBase::ImageBase(ImageType type)
{
m_imaqImage = imaqCreateImage(type, DEFAULT_BORDER_SIZE);
}
/**
* Frees memory associated with an ImageBase.
* Destructor frees the imaq image allocated with the class.
*/
ImageBase::~ImageBase()
{
if(m_imaqImage)
imaqDispose(m_imaqImage);
}
/**
* Writes an image to a file with the given filename.
* Write the image to a file in the flash on the cRIO.
* @param fileName The name of the file to write
*/
void ImageBase::Write(const char *fileName)
{
Priv_SetWriteFileAllowed(1);
int success = imaqWriteFile(m_imaqImage, fileName, NULL);
wpi_setImaqErrorWithContext(success, "Imaq Image writeFile error");
}
/**
* Gets the height of an image.
* @return The height of the image in pixels.
*/
int ImageBase::GetHeight()
{
int height;
imaqGetImageSize(m_imaqImage, NULL, &height);
return height;
}
/**
* Gets the width of an image.
* @return The width of the image in pixels.
*/
int ImageBase::GetWidth()
{
int width;
imaqGetImageSize(m_imaqImage, &width, NULL);
return width;
}
/**
* Access the internal IMAQ Image data structure.
*
* @return A pointer to the internal IMAQ Image data structure.
*/
Image *ImageBase::GetImaqImage()
{
return m_imaqImage;
}

111
wpilibc/src/main/native/Vision/IntCameraParameter.cpp
View File

@@ -1,111 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "IntCameraParameter.h"
#include "pcre.h"
#include <stdio.h>
#include <string.h>
/**
* Constructor for an integer camera parameter.
* @param setString The string to set a value in the HTTP request
* @param getString The string to retrieve a value in the HTTP request
*/
IntCameraParameter::IntCameraParameter(const char *setString, const char *getString, bool requiresRestart)
{
m_changed = false;
m_value = 0;
m_setString = setString;
m_getString = getString;
m_requiresRestart = requiresRestart;
}
/**
* Get a value for a camera parameter.
* @returns The camera parameter cached valued.
*/
int IntCameraParameter::GetValue()
{
return m_value;
}
/**
* Set a value for a camera parameter.
* Mark the value for change. The value will be updated in the parameter
* change loop.
*/
void IntCameraParameter::SetValue(int value)
{
m_value = value;
m_changed = true;
}
/**
* Check if a parameter has changed and update.
* Check if a parameter has changed and send the update string if it
* has changed. This is called from the loop in the parameter task loop.
* @returns true if the camera needs to restart
*/
bool IntCameraParameter::CheckChanged(bool &changed, char *param)
{
changed = m_changed;
if (m_changed)
{
sprintf(param, m_setString, m_value);
m_changed = false;
return m_requiresRestart;
}
return false;
}
/**
* Get a parameter value from the string.
* Get a parameter value from the camera status string. If it has been changed
* been changed by the program, then don't update it. Program values have
* precedence over those written in the camera.
*/
void IntCameraParameter::GetParamFromString(const char *string, int stringLength)
{
char resultString[150];
if (SearchForParam(m_getString, string, stringLength, resultString) >= 0)
{
if (!m_changed) m_value = atoi(resultString);
}
}
/**
* @param pattern: the regular expression
* @param searchString the text to search
* @param searchStringLen the length of searchString
* @param result buffer to put resulting text into, must be pre-allocated
*/
int IntCameraParameter::SearchForParam(const char *pattern, const char *searchString, int searchStringLen, char *result)
{
int vectorLen = 10;
int resultVector[vectorLen];
const char *error;
int erroffset;
pcre *compiledPattern = pcre_compile(
pattern, //"root.Image.I0.Appearance.Resolution=(.*)",
PCRE_CASELESS,
&error, // for error message
&erroffset, // for error offset
NULL); // use default character tables
int rc;
rc = pcre_exec(compiledPattern,
NULL,
searchString,
searchStringLen,
0,
0,
resultVector, //locations of submatches
vectorLen); //size of ovector
int length = resultVector[3] - resultVector[2];
memcpy(result, &searchString[resultVector[2]], length);
result[length] = '\0';
return rc;
}

53
wpilibc/src/main/native/Vision/MonoImage.cpp
View File

@@ -1,53 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "MonoImage.h"
#include "nivision.h"
MonoImage::MonoImage() : ImageBase(IMAQ_IMAGE_U8)
{
}
MonoImage::~MonoImage()
{
}
/**
* Look for ellipses in an image.
* Given some input parameters, look for any number of ellipses in an image.
* @param ellipseDescriptor Ellipse descriptor
* @param curveOptions Curve options
* @param shapeDetectionOptions Shape detection options
* @param roi Region of Interest
* @returns a vector of EllipseMatch structures (0 length vector on no match)
*/
vector<EllipseMatch> * MonoImage::DetectEllipses(
EllipseDescriptor *ellipseDescriptor, CurveOptions *curveOptions,
ShapeDetectionOptions *shapeDetectionOptions, ROI *roi)
{
int numberOfMatches;
EllipseMatch *e = imaqDetectEllipses(m_imaqImage, ellipseDescriptor,
curveOptions, shapeDetectionOptions, roi, &numberOfMatches);
vector<EllipseMatch> *ellipses = new vector<EllipseMatch>;
if (e == NULL)
{
return ellipses;
}
for (int i = 0; i < numberOfMatches; i++)
{
ellipses->push_back(e[i]);
}
imaqDispose(e);
return ellipses;
}
vector<EllipseMatch> * MonoImage::DetectEllipses(
EllipseDescriptor *ellipseDescriptor)
{
vector<EllipseMatch> *ellipses = DetectEllipses(ellipseDescriptor, NULL,
NULL, NULL);
return ellipses;
}

283
wpilibc/src/main/native/Vision/PCVideoServer.cpp
View File

@@ -1,283 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "HAL/HAL.h"
#include "PCVideoServer.h"
#include <errno.h>
#include <taskLib.h>
#include <hostLib.h>
#include <inetLib.h>
#include <sockLib.h>
#include <cstring>
#include "NetworkCommunication/UsageReporting.h"
#include "Task.h"
#include "Timer.h"
#include "Vision/AxisCamera.h"
#include "WPIErrors.h"
/**
* @brief Implements an object that automatically does a close on a
* camera socket on destruction.
*/
class ScopedSocket {
public:
ScopedSocket(int camSock)
: m_camSock(camSock)
{
}
~ScopedSocket() {
if (m_camSock != ERROR) {
close(m_camSock);
}
}
// Cast to int allows you to pass this to any function that
// takes the socket as an int.
operator int() const {
return m_camSock;
}
private:
int m_camSock;
};
//============================================================================
// PCVideoServer
//============================================================================
/**
* @brief Constructor.
*/
PCVideoServer::PCVideoServer()
: m_serverTask("PCVideoServer", (FUNCPTR)s_ServerTask)
, m_newImageSem (NULL)
, m_stopServer (false)
{
AxisCamera &cam = AxisCamera::GetInstance();
m_newImageSem = cam.GetNewImageSem();
if (!cam.StatusIsFatal())
{
StartServerTask();
}
else
{
CloneError(&cam);
}
}
/**
* @brief Destructor.
* Stop serving images and destroy this class.
*/
PCVideoServer::~PCVideoServer()
{
// Stop the images to PC server.
Stop();
// Clear the error so that you can use this object to make a connection to
// the VIDEO_TO_PC_PORT to stop the ImageToPCServer if it is waiting to
// accept connections from a PC.
ClearError();
// Open a socket.
int camSock = socket(AF_INET, SOCK_STREAM, 0);
if (camSock == ERROR)
{
wpi_setErrnoError();
return;
}
ScopedSocket scopedCamSock(camSock);
// If successful
if (!StatusIsFatal())
{
// Create a connection to the localhost.
struct sockaddr_in selfAddr;
int sockAddrSize = sizeof(selfAddr);
bzero ((char *) &selfAddr, sockAddrSize);
selfAddr.sin_family = AF_INET;
selfAddr.sin_len = (u_char) sockAddrSize;
selfAddr.sin_port = htons (VIDEO_TO_PC_PORT);
if (( (int)(selfAddr.sin_addr.s_addr = inet_addr (const_cast<char*>("localhost")) ) != ERROR) ||
( (int)(selfAddr.sin_addr.s_addr = hostGetByName (const_cast<char*>("localhost")) ) != ERROR))
{
struct timeval connectTimeout;
connectTimeout.tv_sec = 1;
connectTimeout.tv_usec = 0;
connectWithTimeout(camSock, (struct sockaddr *) &selfAddr, sockAddrSize, &connectTimeout);
}
}
}
/**
* 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;
}
delayTicks(1);
return id;
}
/**
* @brief Start sending images to the PC.
*/
void PCVideoServer::Start()
{
StartServerTask();
}
/**
* @brief Stop sending images to the PC.
*/
void PCVideoServer::Stop()
{
m_stopServer = true;
}
/**
* Static stub for kicking off the server task
*/
int PCVideoServer::s_ServerTask(PCVideoServer *thisPtr)
{
return thisPtr->ServerTask();
}
/**
* @brief Initialize the socket and serve images to the PC.
* This is the task that serves images to the PC in a loop. This runs
* as a separate task.
*/
int PCVideoServer::ServerTask()
{
/* Setup to PC sockets */
struct sockaddr_in serverAddr;
int sockAddrSize = sizeof(serverAddr);
int pcSock = ERROR;
bzero ((char *) &serverAddr, sockAddrSize);
serverAddr.sin_len = (u_char) sockAddrSize;
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons (VIDEO_TO_PC_PORT);
serverAddr.sin_addr.s_addr = htonl (INADDR_ANY);
int success;
while (true)
{
taskSafe();
// Create the socket.
if ((pcSock = socket (AF_INET, SOCK_STREAM, 0)) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to create the PCVideoServer socket");
continue;
}
// Set the TCP socket so that it can be reused if it is in the wait state.
int reuseAddr = 1;
setsockopt(pcSock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast<char*>(&reuseAddr), sizeof(reuseAddr));
// Bind socket to local address.
if (bind (pcSock, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to bind the PCVideoServer port");
close (pcSock);
continue;
}
// Create queue for client connection requests.
if (listen (pcSock, 1) == ERROR)
{
wpi_setErrnoErrorWithContext("Failed to listen on the PCVideoServer port");
close (pcSock);
continue;
}
struct sockaddr_in clientAddr;
int clientAddrSize;
int newPCSock = accept (pcSock, reinterpret_cast<sockaddr*>(&clientAddr), &clientAddrSize);
if (newPCSock == ERROR)
{
close(pcSock);
continue;
}
//TODO: check camera error
// Report usage when there is actually a connection.
nUsageReporting::report(nUsageReporting::kResourceType_PCVideoServer, 0);
int numBytes = 0;
int imageDataSize = 0;
char* imageData = NULL;
while(!m_stopServer)
{
success = takeSemaphore(m_newImageSem, 1000);
if (success == ERROR)
{
// If the semTake timed out, there are no new images from the camera.
continue;
}
success = AxisCamera::GetInstance().CopyJPEG(&imageData, numBytes, imageDataSize);
if (!success)
{
// No point in running too fast -
Wait(1.0);
// If camera is not initialzed you will get failure and
// the timestamp is invalid. Reset this value and try again.
continue;
}
// Write header to PC
static const char header[4]={1,0,0,0};
int headerSend = write(newPCSock, const_cast<char*>(header), 4);
// Write image length to PC
int lengthSend = write(newPCSock, reinterpret_cast<char*>(&numBytes), 4);
// Write image to PC
int sent = write (newPCSock, imageData, numBytes);
// The PC probably closed connection. Get out of here
// and try listening again.
if (headerSend == ERROR || lengthSend == ERROR || sent == ERROR)
{
break;
}
}
// Clean up
delete [] imageData;
close (newPCSock);
newPCSock = ERROR;
close (pcSock);
pcSock = ERROR;
taskUnsafe();
Wait(0.1);
}
return (OK);
}

28
wpilibc/src/main/native/Vision/RGBImage.cpp
View File

@@ -1,28 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "RGBImage.h"
/**
* Create a new image that uses Red, Green, and Blue planes.
*/
RGBImage::RGBImage() : ColorImage(IMAQ_IMAGE_RGB)
{
}
/**
* Create a new image by loading a file.
* @param fileName The path of the file to load.
*/
RGBImage::RGBImage(const char *fileName) : ColorImage(IMAQ_IMAGE_RGB)
{
int success = imaqReadFile(m_imaqImage, fileName, NULL, NULL);
wpi_setImaqErrorWithContext(success, "Imaq ReadFile error");
}
RGBImage::~RGBImage()
{
}

18
wpilibc/src/main/native/Vision/Threshold.cpp
View File

@@ -1,18 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Threshold.h"
Threshold::Threshold(int new_plane1Low, int new_plane1High, int new_plane2Low,
int new_plane2High, int new_plane3Low, int new_plane3High)
{
plane1Low = new_plane1Low;
plane1High = new_plane1High;
plane2Low = new_plane2Low;
plane2High = new_plane2High;
plane3Low = new_plane3Low;
plane3High = new_plane3High;
}