void
Wander::action()
{
// get parameters
const WanderParameters * params =
dynamic_cast<const WanderParameters *>(params_);
// make sure nothing went wrong
MIRO_ASSERT(params != NULL);
// set the arbiter message to random values
double r = rand01();
if (r < params->thresholdChange) {
r = rand01();
message_.velocity.translation =
params->minTranslation +
(long)(r * (params->maxTranslation - params->minTranslation));
r = rand01();
if (r > params->thresholdStraight) {
r = rand01();
message_.velocity.rotation =
- params->maxRotation + r * 2 * params->maxRotation;
}
else
message_.velocity.rotation = 0.;
}
arbitrate(message_);
}
void TemperatureThresholdArbitrator::clearPolicyCachedData(UIntN policyIndex)
{
auto policyRequest = m_requestedTemperatureThresholds.find(policyIndex);
if (policyRequest != m_requestedTemperatureThresholds.end())
{
policyRequest->second = TemperatureThresholds::createInvalid();
arbitrate(policyIndex, TemperatureThresholds::createInvalid(), Temperature::createInvalid());
}
}
//------------------------------- activate ------------------------------------
//-----------------------------------------------------------------------------
void GoalThink::activate()
{
if (!_owner->isPossessed())
{
arbitrate();
}
_status = ACTIVE;
}
int main()
{
int wait;
// Set up port for start switch
lcdInit();
// Set up motors and encoders
motorInit();
encodersInit();
servoInit();
// Set up IRs
gp2d12Init(PIN_HEAD_IR);
digitalSetDirection(PIN_IR,AVRRA_INPUT);
digitalSetData(PIN_IR,AVRRA_LOW);
// Set up OP599A - analog already initialized
digitalSetDirection(PIN_PHOTO,AVRRA_INPUT);
digitalSetData(PIN_PHOTO,AVRRA_LOW);
// Set up fan & test
digitalSetData(PIN_FAN, AVRRA_LOW);
digitalSetDirection(PIN_FAN, AVRRA_OUTPUT);
digitalSetData(PIN_FAN, AVRRA_HIGH);
delayms(25);
digitalSetData(PIN_FAN, AVRRA_LOW);
lcdPrint("Wait... ");
wait = digitalGetData(PIN_START);
while(wait) {
// standard delay ..
delayms(10);
wait = digitalGetData(PIN_START);
}
// Start our map
mapInit();
lcdClear();
PrintHeading(lheading,nStart);
sei();
// Run Behaviors
while(1) {
// update odometer
while(rCount > COUNTS_CM(1) ) {
// odometer is in CM
odometer = odometer - 1;
rCount = rCount - COUNTS_CM(1);
}
// now run behaviors
if(arbitrate()>0) {
// let motors wind down
delayms(500);
clearCounters;
plan();
clearCounters;
}
}
}
Bool PowerControlArbitrator::arbitrate(UIntN policyIndex, const PowerControlStatusSet& powerControlStatusSet)
{
increaseVectorSizeIfNeeded(m_requestedPowerControlStatusSet, policyIndex);
savePolicyRequest(policyIndex, powerControlStatusSet);
std::vector<PowerControlStatus> arbitratedPowerControlStatusVector = createInitialArbitratedPowerControlStatusVector();
arbitrate(arbitratedPowerControlStatusVector);
PowerControlStatusSet arbitratedPowerControlStatusSet = getArbitratedPowerControlStatusSet(arbitratedPowerControlStatusVector);
// see if the arbitrated PowerControlStatusSet is changing
Bool arbitratedValueChanged = false;
if (arbitratedPowerControlStatusSet != *m_arbitratedPowerControlStatusSet)
{
arbitratedValueChanged = true;
delete m_arbitratedPowerControlStatusSet;
m_arbitratedPowerControlStatusSet = new PowerControlStatusSet(arbitratedPowerControlStatusSet);
}
return arbitratedValueChanged;
}
void
Straight::action()
{
// get the parameters
const StraightParameters * params =
dynamic_cast<const StraightParameters *>(params_);
// make sure nothing went wrong
MIRO_ASSERT(params != NULL);
// create arbiter message
Miro::BAP::MotionArbiterMessage message;
// initialize arbiter message
message.id = this;
message.active = true;
message.velocity.translation = params->translation;
message.velocity.rotation = params->rotation;
// send arbiter message
arbitrate(message);
}
/**
* Performs consumption of a single phit.
*
* For "single" consumption, the consumption port (just one) is arbitrated just like any other one.
* @param i The node in which the consumption is performed.
*/
void arbitrate_cons_single(long i) {
arbitrate(i, p_con);
return;
}
