/**
* Print formatted text to the Driver Station LCD text bufer.
*
* Use UpdateLCD() periodically to actually send the test to the Driver Station.
*
* @param line The line on the LCD to print to.
* @param startingColumn The column to start printing to. This is a 1-based number.
* @param writeFmt The printf format string describing how to print.
*/
void DriverStationLCD::Printf(Line line, INT32 startingColumn, const char *writeFmt, ...)
{
va_list args;
UINT32 start = startingColumn - 1;
INT32 maxLength = kLineLength - start;
char lineBuffer[kLineLength + 1];
if (startingColumn < 1 || startingColumn > kLineLength)
{
wpi_fatal(ParameterOutOfRange);
return;
}
if (line < kMain_Line6 || line > kUser_Line6)
{
wpi_fatal(ParameterOutOfRange);
return;
}
va_start (args, writeFmt);
{
Synchronized sync(m_textBufferSemaphore);
// snprintf appends NULL to its output. Therefore we can't write directly to the buffer.
INT32 length = vsnprintf(lineBuffer, kLineLength + 1, writeFmt, args);
if (length < 0) length = kLineLength;
memcpy(m_textBuffer + start + line * kLineLength + sizeof(UINT16), lineBuffer, std::min(maxLength,length));
}
va_end (args);
}
/**
* 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...
* Pack the sequence number, Printf() buffer, the errors messages (not implemented yet), and packed dashboard data buffer.
* @return The total size of the data packed into the userData field of the status packet.
*/
INT32 Dashboard::Finalize(void)
{
if (*m_userStatus == NULL)
{
wpi_fatal(NullParameter);
return 0;
}
if (!m_complexTypeStack.empty())
{
wpi_fatal(MismatchedComplexTypeClose);
return 0;
}
INT32 size = 0;
// Sequence number
memcpy(*m_userStatus + size, &m_sequence, sizeof(m_sequence));
size += sizeof(m_sequence);
m_sequence++;
// User printed strings
INT32 printSize;
{
Synchronized sync(m_printSemaphore);
printSize = strlen(m_localPrintBuffer);
memcpy(*m_userStatus + size, &printSize, sizeof(printSize));
size += sizeof(printSize);
memcpy(*m_userStatus + size, m_localPrintBuffer, printSize);
size += printSize;
m_localPrintBuffer[0] = 0;
}
// Error Strings
INT32 errorSize = 0;
if (printSize + errorSize > kMaxDashboardDataSize)
{
wpi_fatal(DashboardDataOverflow);
return 0;
}
memcpy(*m_userStatus + size, &errorSize, sizeof(errorSize));
size += sizeof(errorSize);
///< TODO: add error reporting strings.
size += errorSize;
// Dashboard Data
INT32 dataSize = m_packPtr - m_localBuffer;
if (printSize + errorSize + dataSize > kMaxDashboardDataSize)
{
wpi_fatal(DashboardDataOverflow);
return 0;
}
memcpy(*m_userStatus + size, &dataSize, sizeof(dataSize));
size += sizeof(dataSize);
memcpy(*m_userStatus + size, m_localBuffer, dataSize);
size += dataSize;
m_packPtr = m_localBuffer;
return size;
}
/**
* 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 index)
{
if (index >= m_size)
{
wpi_fatal(IndexOutOfRange);
return;
}
if ( ! m_isAllocated[index] )
{
wpi_fatal(NotAllocated);
return;
}
m_isAllocated[index] = false;
}
/**
* Allocate a specific resource value.
* The user requests a specific resource value, i.e. channel number and it is verified
* unallocated, then returned.
*/
UINT32 Resource::Allocate(UINT32 index)
{
if (index >= m_size)
{
wpi_fatal(IndexOutOfRange);
return 0;
}
if ( m_isAllocated[index] )
{
wpi_fatal(ResourceAlreadyAllocated);
return 0;
}
m_isAllocated[index] = true;
return index;
}
/**
* Validate that the data being packed will fit in the buffer.
*/
bool Dashboard::ValidateAdd(INT32 size)
{
if ((m_packPtr - m_localBuffer) + size > kMaxDashboardDataSize)
{
wpi_fatal(DashboardDataOverflow);
return false;
}
// Make sure printf is not being used at the same time.
if (m_localPrintBuffer[0] != 0)
{
wpi_fatal(DashboardDataCollision);
return false;
}
return true;
}
/**
* Verify that the solenoid channel number is within limits.
*/
bool SensorBase::CheckSolenoidChannel(UINT32 channel)
{
if (channel > 0 && channel <= kSolenoidChannels)
return true;
wpi_fatal(IndexOutOfRange);
return false;
}
/**
* 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.
*/
DigitalModule::DigitalModule(UINT32 slot)
: Module(slot)
, m_fpgaDIO (NULL)
{
Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalChannels);
m_fpgaDIO = new tDIO(SlotToIndex(m_slot), &status);
// Make sure that the 9403 IONode has had a chance to initialize before continuing.
while(m_fpgaDIO->readLoopTiming(&status) == 0) taskDelay(1);
if (m_fpgaDIO->readLoopTiming(&status) != kExpectedLoopTiming)
{
wpi_fatal(LoopTimingError);
printf("DIO LoopTiming: %d, expecting: %d\n", m_fpgaDIO->readLoopTiming(&status), kExpectedLoopTiming);
}
m_fpgaDIO->writePWMConfig_Period(PWM::kDefaultPwmPeriod, &status);
m_fpgaDIO->writePWMConfig_MinHigh(PWM::kDefaultMinPwmHigh, &status);
// Ensure that PWM output values are set to OFF
for (UINT32 pwm_index = 1; pwm_index <= kPwmChannels; pwm_index++)
{
SetPWM(pwm_index, PWM::kPwmDisabled);
SetPWMPeriodScale(pwm_index, 3); // Set all to 4x by default.
}
// Turn off all relay outputs.
m_fpgaDIO->writeSlowValue_RelayFwd(0, &status);
m_fpgaDIO->writeSlowValue_RelayRev(0, &status);
// Create a semaphore to protect changes to the relay values
m_relaySemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
AddToSingletonList();
}
/**
* Verify that the solenoid module is correct.
* Verify that the solenoid module is slot 8 (for now).
*/
bool SensorBase::CheckSolenoidModule(UINT32 slot)
{
if (slot <= kChassisSlots && modulePopulation[slot] == 9472)
return true;
wpi_fatal(IndexOutOfRange);
return false;
}
/**
* Print formatted text to the Driver Station LCD text bufer. This function
* pads the line with empty spaces.
*
* Use UpdateLCD() periodically to actually send the test to the Driver Station.
*
* @param line The line on the LCD to print to.
* @param writeFmt The printf format string describing how to print.
*/
void DriverStationLCD::PrintfLine(Line line, const char *writeFmt, ...)
{
va_list args;
char lineBuffer[kLineLength + 1];
if (line < kMain_Line6 || line > kUser_Line6)
{
wpi_fatal(ParameterOutOfRange);
return;
}
va_start (args, writeFmt);
{
Synchronized sync(m_textBufferSemaphore);
// snprintf appends NULL to its output. Therefore we can't write directly to the buffer.
INT32 length = std::min(vsnprintf(lineBuffer, kLineLength + 1, writeFmt, args), kLineLength);
if (length < 0) length = kLineLength;
// Fill the rest of the buffer
if (length < kLineLength)
{
memset(lineBuffer + length, ' ', kLineLength - length);
}
memcpy(m_textBuffer + line * kLineLength + sizeof(UINT16), lineBuffer, kLineLength);
}
va_end (args);
}
/**
* Set the sample rate on the module.
*
* This is a global setting for the module and effects all channels.
*
* @param samplesPerSecond The number of samples per channel per second.
*/
void AnalogModule::SetSampleRate(float samplesPerSecond)
{
// TODO: This will change when variable size scan lists are implemented.
// TODO: Need float comparison with epsilon.
//wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
m_sampleRateSet = true;
// Compute the convert rate
UINT32 ticksPerSample = (UINT32)((float)kTimebase / samplesPerSecond);
UINT32 ticksPerConversion = ticksPerSample / GetNumChannelsToActivate();
// ticksPerConversion must be at least 80
if (ticksPerConversion < 80)
{
wpi_fatal(SampleRateTooHigh);
ticksPerConversion = 80;
}
// Atomically set the scan size and the convert rate so that the sample rate is constant
tAI::tConfig config;
config.ScanSize = GetNumChannelsToActivate();
config.ConvertRate = ticksPerConversion;
m_module->writeConfig(config, &status);
// Indicate that the scan size has been commited to hardware.
SetNumChannelsToActivate(0);
wpi_assertCleanStatus(status);
}
/**
* Stop the compressor.
* Stops the polling loop that operates the compressor. At this time the compressor will stop operating.
*/
void StopCompressor()
{
if (compressor == NULL)
{
wpi_fatal(CompressorUndefined);
return;
}
compressor->Stop();
}
/**
* Validate that the data being packed will fit in the buffer.
*/
bool Dashboard::ValidateAdd(INT32 size)
{
if ((m_packPtr - m_localBuffer) + size > kMaxDashboardDataSize)
{
wpi_fatal(DashboardDataOverflow);
return false;
}
return true;
}
/**
* Get the state of the enabled flag.
* Return the state of the enabled flag for the compressor and pressure switch.
*
* @return The state of the compressor task's enable flag.
*/
bool CompressorEnabled()
{
if (compressor == NULL)
{
wpi_fatal(CompressorUndefined);
return false;
}
return compressor->Enabled();
}
/**
* Allocate resources for a compressor/pressure switch pair
* Allocate the underlying object for the compressor.
* @param pressureSwitchChannel The channel on the default digital module for the pressure switch
* @param relayChannel The channel on the default digital module for the relay that controls the compressor
*/
void CreateCompressor(UINT32 pressureSwitchChannel, UINT32 relayChannel)
{
if (compressor == NULL)
{
compressor = new Compressor(pressureSwitchChannel, relayChannel);
return;
}
wpi_fatal(CompressorAlreadyDefined);
}
/*
* 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_fatal(InvalidMotorIndex);
return;
}
m_invertedMotors[motor] = isInverted ? -1 : 1;
}
/**
* 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)
{
if (leftStick == NULL || rightStick == NULL)
{
wpi_fatal(NullParameter);
return;
}
TankDrive(leftStick->GetY(), rightStick->GetY());
}
/**
* Convert a voltage to a raw value for a specified channel.
*
* This process depends on the calibration of each channel, so the channel
* must be specified.
*
* @todo This assumes raw values. Oversampling not supported as is.
*
* @param channel The channel to convert for.
* @param voltage The voltage to convert.
* @return The raw value for the channel.
*/
INT32 AnalogModule::VoltsToValue(INT32 channel, float voltage)
{
if (voltage > 10.0)
{
voltage = 10.0;
wpi_fatal(VoltageOutOfRange);
}
if (voltage < -10.0)
{
voltage = -10.0;
wpi_fatal(VoltageOutOfRange);
}
UINT32 LSBWeight = GetLSBWeight(channel);
INT32 offset = GetOffset(channel);
INT32 value = (INT32) ((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9));
wpi_assertCleanStatus(status);
return value;
}
/**
* DriverStation contructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation()
: m_controlData(NULL)
, m_userControl(NULL)
, m_userStatus(NULL)
, m_digitalOut(0)
, m_batteryChannel(NULL)
, m_task("DriverStation",(FUNCPTR) DriverStation::InitTask)
, m_dashboard(&m_userStatus)
{
m_controlData = new FRCControlData;
m_userControl = new char[USER_CONTROL_DATA_SIZE];
m_userStatus = new char[USER_STATUS_DATA_SIZE];
bzero(m_userStatus, USER_STATUS_DATA_SIZE);
// 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(kBatterySlot, kBatteryChannel);
AddToSingletonList();
if (!m_task.Start((INT32) this))
{
wpi_fatal(DriverStationTaskError);
}
}
/**
* Set the accumulator's deadband.
*/
void AnalogChannel::SetAccumulatorDeadband(INT32 deadband)
{
if (m_accumulator == NULL)
{
wpi_fatal(NullParameter);
return;
}
m_accumulator->writeDeadband(deadband, &status);
wpi_assertCleanStatus(status);
}
/**
* Resets the accumulator to the initial value.
*/
void AnalogChannel::ResetAccumulator()
{
if (m_accumulator == NULL)
{
wpi_fatal(NullParameter);
return;
}
m_accumulator->strobeReset(&status);
wpi_assertCleanStatus(status);
}
/**
* 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 leftAxis,
GenericHID *rightStick, UINT32 rightAxis)
{
if (leftStick == NULL || rightStick == NULL)
{
wpi_fatal(NullParameter);
return;
}
TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis));
}
/**
* 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 *value, UINT32 *count)
{
status = 0;
if (m_accumulator == NULL)
{
wpi_fatal(NullParameter);
return;
}
if (value == NULL || count == NULL)
{
wpi_fatal(NullParameter);
return;
}
tAccumulator::tOutput output = m_accumulator->readOutput(&status);
*value = output.Value + m_accumulatorOffset;
*count = output.Count;
wpi_assertCleanStatus(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 center)
{
if (m_accumulator == NULL)
{
wpi_fatal(NullParameter);
return;
}
m_accumulator->writeCenter(center, &status);
wpi_assertCleanStatus(status);
}
/**
* 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(void)
{
if (m_complexTypeStack.top() != kCluster)
{
wpi_fatal(MismatchedComplexTypeClose);
return;
}
m_complexTypeStack.pop();
AddedElement(kOther);
}
/**
* 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_aSource = aSource;
m_bSource = bSource;
m_allocatedASource = false;
m_allocatedBSource = false;
if (m_aSource == NULL || m_bSource == NULL)
wpi_fatal(NullParameter);
else
InitEncoder(reverseDirection, encodingType);
}
/**
* 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 AnalogChannel::GetAccumulatorValue()
{
if (m_accumulator == NULL)
{
wpi_fatal(NullParameter);
return 0;
}
INT64 value = m_accumulator->readOutput_Value(&status) + m_accumulatorOffset;
wpi_assertCleanStatus(status);
return value;
}
/**
* 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 stick, UINT32 axis)
{
if (axis < 1 || axis > kJoystickAxes)
{
wpi_fatal(BadJoystickAxis);
return 0.0;
}
INT8 value;
GetData();
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_fatal(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;
}
/**
* 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 AnalogChannel::GetAccumulatorCount()
{
status = 0;
if (m_accumulator == NULL)
{
wpi_fatal(NullParameter);
return 0;
}
UINT32 count = m_accumulator->readOutput_Count(&status);
wpi_assertCleanStatus(status);
return count;
}
/**
* Handles errors generated by task related code.
*/
bool Task::HandleError(STATUS results)
{
if (results != ERROR) return true;
switch(errnoGet())
{
case S_objLib_OBJ_ID_ERROR:
wpi_fatal(TaskIDError);
break;
case S_objLib_OBJ_DELETED:
wpi_fatal(TaskDeletedError);
break;
case S_taskLib_ILLEGAL_OPTIONS:
wpi_fatal(TaskOptionsError);
break;
case S_memLib_NOT_ENOUGH_MEMORY:
wpi_fatal(TaskMemoryError);
break;
case S_taskLib_ILLEGAL_PRIORITY:
wpi_fatal(TaskPriorityError);
break;
default:
printErrno(errnoGet());
wpi_fatal(TaskError);
}
return false;
}
/**
* 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_fatal(NullParameter);
}
else
{
InitGyro();
}
}