C++ wpi_assert函数代码示例

/**
 * 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);
}

/**
 * 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 (pos < 0.0)
    {
        pos = 0.0;
    }
    else if (pos > 1.0)
    {
        pos = 1.0;
    }

    INT32 rawValue;
    // note, need to perform the multiplication below as floating point
    // before converting to int
    rawValue =
        (INT32) ((pos * (float)GetFullRangeScaleFactor()) +
                 GetMinNegativePwm());

    wpi_assert((rawValue >= GetMinNegativePwm())
               && (rawValue <= GetMaxPositivePwm()));
    wpi_assert(rawValue != kPwmDisabled);

    // send the computed pwm value to the FPGA
    SetRaw((UINT8) rawValue);
}

/**
 * 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 only be one of the three reasonable
 *    values, 0v-0v, 0v-12v, or 12v-0v.
 * 
 * When set to kForwardOnly or kReverseOnly, you can specify the constant for the
 *    direction or you can simply specify kOff and kOn.  Using only kOff and kOn is
 *    recommended.
 * 
 * @param value The state to set the relay.
 */
void Relay::Set(Relay::Value value)
{
	switch (value)
	{
	case kOff:
		if (m_direction == kBothDirections || m_direction == kForwardOnly)
		{
			m_module->SetRelayForward(m_channel, false);
		}
		if (m_direction == kBothDirections || m_direction == kReverseOnly)
		{
			m_module->SetRelayReverse(m_channel, false);
		}
		break;
	case kOn:
		wpi_assert(m_direction != kBothDirections);
		if (m_direction == kForwardOnly)
		{
			m_module->SetRelayForward(m_channel, true);
		}
		else if (m_direction == kReverseOnly)
		{
			m_module->SetRelayReverse(m_channel, true);
		}
		break;
	case kForward:
		wpi_assert(m_direction != kReverseOnly);
		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:
		wpi_assert(m_direction != kForwardOnly);
		if (m_direction == kBothDirections)
		{
			m_module->SetRelayForward(m_channel, false);
		}
		if (m_direction == kBothDirections || m_direction == kReverseOnly)
		{
			m_module->SetRelayReverse(m_channel, true);
		}
		break;
	default:
		wpi_assert(false);
	}
}

/**
 * @brief Unregister a button to track the number of times it was pressed.
 * @param joystick_id Which joystick to track a button on
 * @param button_idd Which button on the joystick to track
 */
void Proxy166::UnregisterCounter(int joystick_id, int button_id) {
	wpi_assert(joystick_id < NUMBER_OF_JOYSTICKS && joystick_id >= 0);
	wpi_assert(button_id < NUMBER_OF_JOY_BUTTONS && button_id >= 0);
	
	if(tracker.size() == 0)
		return;
	vector<int>::iterator it = tracker.begin();
	while((it+=3) != tracker.end())
	{
		if(*it == joystick_id && *(it+1) == button_id) {
			tracker.erase(it, it+2);
		}
	}
}

/**
 * Set the source object that causes the counter to count down.
 * Set the down counting DigitalSource.
 */
void Counter::SetDownSource(DigitalSource *source)
{
	wpi_assert(m_downSource == NULL);
	unsigned char mode = m_counter->readConfig_Mode(&status);
	wpi_assert(mode == kTwoPulse || mode == kExternalDirection);
	m_downSource = source;
	m_counter->writeConfig_DownSource_Module(source->GetModuleForRouting(), &status);
	m_counter->writeConfig_DownSource_Channel(source->GetChannelForRouting(), &status);
	m_counter->writeConfig_DownSource_AnalogTrigger(source->GetAnalogTriggerForRouting(), &status);

	SetDownSourceEdge(true, false);
	m_counter->strobeReset(&status);
	wpi_assertCleanStatus(status);
}

/**
 * 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
                }
            }
        }
    }
}

/**
 * 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();
	}
}

void InterruptableSensorBase::AllocateInterrupts(bool watcher) {
  wpi_assert(m_interrupt == nullptr);
  // 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));
}

/**
 * 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 channel)
{
    wpi_assert((channel >= 1) && (channel <= 8));
    GetData();
    return ((m_controlData->
             dsDigitalIn >> (channel - 1)) & 0x1) ? true : false;
}

/**
 * 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)
{
    tRioStatusCode status = 0;
    wpi_assert(handler != NULL);
    m_handler = handler;
    m_param = param;
    m_periodic = false;
    m_expirationTime = 0;
    m_period = 0;
    m_nextEvent = NULL;
    m_queued = false;
    CRITICAL_REGION(m_semaphore)
    {
        // do the first time intialization of static variables
        if (talarm == NULL)
        {
            manager =
                new tInterruptManager(1 << kTimerInterruptNumber, false,
                                      &status);
            manager->registerHandler(ProcessQueue, NULL, &status);
            manager->enable(&status);
            talarm = new tAlarm(&status);
        }
    }
    END_REGION;
    wpi_assertCleanStatus(status);
}

/**
 * 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
}

/**
 * Helper function to determine the size of a jpeg. The general structure of
 * how to parse a jpeg for length can be found in this stackoverflow article:
 * http://stackoverflow.com/a/1602428. Be sure to also read the comments for
 * the SOS flag explanation.
 */
unsigned int USBCamera::GetJpegSize(void* buffer, unsigned int buffSize) {
    uint8_t* data = (uint8_t*)buffer;
    if (!wpi_assert(data[0] == 0xff && data[1] == 0xd8)) return 0;
    unsigned int pos = 2;
    while (pos < buffSize) {
        // All control markers start with 0xff, so if this isn't present,
        // the JPEG is not valid
        if (!wpi_assert(data[pos] == 0xff)) return 0;
        unsigned char t = data[pos + 1];
        // These are RST markers. We just skip them and move onto the next marker
        if (t == 0x01 || (t >= 0xd0 && t <= 0xd7)) {
            pos += 2;
        } else if (t == 0xd9) {
            // End of Image, add 2 for this and 0-indexed
            return pos + 2;
        } else if (!wpi_assert(t != 0xd8)) {
            // Another start of image, invalid image
            return 0;
        } else if (t == 0xda) {
            // SOS marker. The next two bytes are a 16-bit big-endian int that is
            // the length of the SOS header, skip that
            unsigned int len = (((unsigned int)(data[pos + 2] & 0xff)) << 8 |
                                ((unsigned int)data[pos + 3] & 0xff));
            pos += len + 2;
            // The next marker is the first marker that is 0xff followed by a non-RST
            // element. 0xff followed by 0x00 is an escaped 0xff. 0xd0-0xd7 are RST
            // markers
            while (data[pos] != 0xff || data[pos + 1] == 0x00 ||
                    (data[pos + 1] >= 0xd0 && data[pos + 1] <= 0xd7)) {
                pos += 1;
                if (pos >= buffSize) return 0;
            }
        } else {
            // This is one of several possible markers. The next two bytes are a
            // 16-bit
            // big-endian int with the length of the marker header, skip that then
            // continue searching
            unsigned int len = (((unsigned int)(data[pos + 2] & 0xff)) << 8 |
                                ((unsigned int)data[pos + 3] & 0xff));
            pos += len + 2;
        }
    }

    return 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:
		wpi_assert(false);
		return false;
	}
}

DigitalGlitchFilter::DigitalGlitchFilter() {
  std::lock_guard<priority_mutex> sync(m_mutex);
  auto index =
      std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false);
  wpi_assert(index != m_filterAllocated.end());

  m_channelIndex = std::distance(m_filterAllocated.begin(), index);
  *index = true;

  HALReport(HALUsageReporting::kResourceType_DigitalFilter, m_channelIndex);
}

/**
 * In synchronous mode, wait for the defined interrupt to occur. You should <b>NOT</b> attempt to read the
 * sensor from another thread while waiting for an interrupt. This is not threadsafe, and can cause 
 * memory corruption
 * @param timeout Timeout in seconds
 * @param ignorePrevious If true, ignore interrupts that happened before
 * WaitForInterrupt was called.
 * @return What interrupts fired
 */
InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(float timeout, bool ignorePrevious)
{
	if (StatusIsFatal()) return InterruptableSensorBase::kTimeout;
	wpi_assert(m_interrupt != NULL);
	int32_t status = 0;
	uint32_t result;

	result = waitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
	wpi_setErrorWithContext(status, getHALErrorMessage(status));

	return static_cast<WaitResult>(result);
}