/**
* 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);
}
/**
* @brief Whether a joystick is registered for tracking
* @param joystick_id What joystick to check
* @param button_id What button on the joystick to check.
* @return Whether it is registered.
*/
bool Proxy166::IsRegistered(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 false;
vector<int>::iterator it = tracker.begin();
while((it+=3) != tracker.end())
{
if(*it == joystick_id && *(it+1) == button_id) {
return true;
}
}
return false;
}
/**
* 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
}
}
}
}
}
/**
* @brief Sets the cache value of a button on a joystick.
* @param joy_id Which joystick to set the button status for.
* @param button_id Which button on the joystick to set the status for.
* @param newval What to set the button's value to.
*/
void Proxy166::SetButton(int joy_id, int button_id, bool newval)
{
wpi_assert(joy_id < NUMBER_OF_JOY_BUTTONS && joy_id >= 0);
//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
Joysticks[joy_id].button[button_id] = newval;
//semGive(JoystickLocks[joy_id]);
}
/**
* 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);
}
/**
* Disable Interrupts without without deallocating structures.
*/
void InterruptableSensorBase::DisableInterrupts() {
if (StatusIsFatal()) return;
wpi_assert(m_interrupt != nullptr);
int32_t status = 0;
disableInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* @brief Register 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::RegisterCounter(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) {
vector<int>::iterator it = tracker.begin();
while((it+=3) != tracker.end())
{
if(*it == joystick_id && *(it+1) == button_id) {
return;
}
}
}
tracker.push_back(joystick_id);
tracker.push_back(button_id);
tracker.push_back(0);
}
/**
* Return the timestamp for the falling interrupt that occurred most recently.
* This is in the same time domain as GetClock().
* The falling-edge interrupt should be enabled with
* {@link #DigitalInput.SetUpSourceEdge}
* @return Timestamp in seconds since boot.
*/
double InterruptableSensorBase::ReadFallingTimestamp() {
if (StatusIsFatal()) return 0.0;
wpi_assert(m_interrupt != nullptr);
int32_t status = 0;
double timestamp = readFallingTimestamp(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
return timestamp;
}
/**
* @brief Gets the cached Z axis value of a joystick.
* @param joy_id Which joystick to get the cached Z axis value for.
* @return Float equal to the cached Z axis value.
*/
float Proxy166::GetJoystickZ(int joy_id) {
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
float value = 0;
//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
value = Joysticks[joy_id].Z;
//semGive(JoystickLocks[joy_id]);
return value;
}
/**
* @brief Gets a cached switch value.
* @param switch_id Which switch to retrieve the cached value of.
* @return The int value of the cached switch value.
*/
int Proxy166::GetSwitch(int switch_id) {
wpi_assert(switch_id < NUMBER_OF_SWITCHES && switch_id >= 0);
int value = 0;
//semTake(SwitchLocks[switch_id], WAIT_FOREVER);
value = Switches[switch_id];
//semGive(SwitchLocks[switch_id]);
return value;
}
/**
* @brief Gets the pending amount of times a button was pressed and released since last call.
* @param joystick_id Which joystick to check
* @param button_id Which button on the joystick to check
* @return How many times the button was pressed and released since last call.
*/
int Proxy166::GetPendingCount(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)
wpi_assertWithMessage(false, "Tried to fetch pending count for a non-registered button.");
vector<int>::iterator it = tracker.begin();
while(it != tracker.end())
{
if(*it == joystick_id && *(it+1) == button_id) {
return *(it+2);
}
it += 3;
}
wpi_assertWithMessage(false, "Tried to fetch pending count for a non-registered button.");
return 0;
}
/**
* 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;
}
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;
}
/**
* @brief Gets the cache value of the trigger (button 1) of a joystick.
* @param joy_id Which joystick to retrieve the trigger status for.
* @param reset Whether to reset the button's value after being called.
* @return The last read current value
*/
bool Proxy166::GetTrigger(int joy_id, bool reset) {
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
bool bid = GetButton(joy_id,1);
// reset the button so actions are triggered only once
if (reset) {
SetButton(joy_id, 1, 0);
}
return bid;
}
/**
* 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()
{
wpi_assert(m_batteryChannel != NULL);
// 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);
}
/**
* Cancel interrupts on this device.
* This deallocates all the chipobject structures and disables any interrupts.
*/
void InterruptableSensorBase::CancelInterrupts() {
if (StatusIsFatal()) return;
wpi_assert(m_interrupt != nullptr);
int32_t status = 0;
cleanInterrupts(m_interrupt, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_interrupt = nullptr;
m_interrupts->Free(m_interruptIndex);
}
/**
* 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);
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy166::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
Joysticks[joy_id].X = stick.GetX();
Joysticks[joy_id].Y = stick.GetY();
Joysticks[joy_id].Z = stick.GetZ();
Joysticks[joy_id].throttle = stick.GetThrottle();
for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) {
Joysticks[joy_id].button[i] = stick.GetRawButton(i);
}
//semGive(JoystickLocks[joy_id]);
}
/**
* 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 != nullptr && m_rearRightMotor != nullptr);
if (m_frontLeftMotor != nullptr)
m_frontLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
m_rearLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
if (m_frontRightMotor != nullptr)
m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
m_safetyHelper->Feed();
}
/**
* Get dynamic data from the driver station buffer
*/
void KinectStick::GetData()
{
uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
if (_recentPacketNumber != packetNumber)
{
_recentPacketNumber = packetNumber;
int retVal = getDynamicControlData(kJoystickBundleID, _sticks.data, sizeof(_sticks.data), 5);
if (retVal == 0)
{
wpi_assert(_sticks.formatted.size == sizeof(_sticks.data) - 1);
}
}
}
