void
MotionArbiterPower::arbitrate(const ArbiterMessage& _message)
{
const MotionArbiterMessage& message =
dynamic_cast<const MotionArbiterMessage &>(_message);
if (active_) {
Guard guard(mutex_);
MotionArbiterParameters::PriorityMap::const_iterator i
= params_->priorities.find(message.id);
if (i != params_->priorities.end()) {
if (message_[i->second].active != message.active)
cout << "MotionArbiterPower: " << message.id->getBehaviourName()
<< " - " << message.active << endl;
message_[i->second] = message;
setMotion();
}
else {
cerr << "MotionArbiterPower: got message from unregistered behaviour: "
<< message.id->getBehaviourName() << endl;
}
}
else {
cerr << "MotionArbiterPower: got arbitrate call while not active from behaviour: "
<< message.id->getBehaviourName() << endl;
}
}
PhysXD6Joint::PhysXD6Joint(PxPhysics* physx, const D6_JOINT_DESC& desc)
:D6Joint(desc)
{
PxRigidActor* actor0 = nullptr;
if (desc.bodies[0].body != nullptr)
actor0 = static_cast<PhysXRigidbody*>(desc.bodies[0].body)->_getInternal();
PxRigidActor* actor1 = nullptr;
if (desc.bodies[1].body != nullptr)
actor1 = static_cast<PhysXRigidbody*>(desc.bodies[1].body)->_getInternal();
PxTransform tfrm0 = toPxTransform(desc.bodies[0].position, desc.bodies[0].rotation);
PxTransform tfrm1 = toPxTransform(desc.bodies[1].position, desc.bodies[1].rotation);
PxD6Joint* joint = PxD6JointCreate(*physx, actor0, tfrm0, actor1, tfrm1);
joint->userData = this;
mInternal = bs_new<FPhysXJoint>(joint, desc);
// Calls to virtual methods are okay here
for (UINT32 i = 0; i < (UINT32)D6JointAxis::Count; i++)
setMotion((D6JointAxis)i, desc.motion[i]);
for (UINT32 i = 0; i < (UINT32)D6JointDriveType::Count; i++)
setDrive((D6JointDriveType)i, desc.drive[i]);
setLimitLinear(desc.limitLinear);
setLimitTwist(desc.limitTwist);
setLimitSwing(desc.limitSwing);
setDriveTransform(desc.drivePosition, desc.driveRotation);
setDriveVelocity(desc.driveLinearVelocity, desc.driveAngularVelocity);
}
// construct with motion
RtMotion::RtMotion(double tx, double ty, double tz,
double rx, double ry, double rz) {
ACE_TRACE(("RtMRIImage::RtMotion()"));
dataID.setModuleID("motion");
setMotion(tx, ty, tz, rx, ry, rz);
}
void
MotionArbiterPower::init(const ArbiterParameters * _params)
{
Guard guard(mutex_);
const MotionArbiterParameters * params
= dynamic_cast<const MotionArbiterParameters *>(_params);
// convert current arbitration to new configuration
MessageVector tmp(message_);
message_.resize(params->priorities.size()); // new message array size
// if new configuration has no behaviour, that is allowed to arbitrate,
// we better should stop
if (message_.size() == 0) {
Miro::VelocityIDL velocity;
velocity.translation = 0;
velocity.rotation = 0.;
double leftPower;
double rightPower;
leftPower = velocity.translation + velocity.rotation;
leftPower *= scale_;
if( leftPower < 0 )
leftPower -= offset_;
else if( leftPower > 0 )
leftPower += offset_;
rightPower = velocity.translation - velocity.rotation;
rightPower *= scale_;
if( rightPower < 0 )
rightPower -= offset_;
else if( rightPower > 0 )
rightPower += offset_;
pMotion_->setLRPower((int)leftPower, (int)rightPower);
currentVelocity.translation = 0;
currentVelocity.rotation = 0;
}
if (params_) { // allways except first time
MotionArbiterParameters::PriorityMap::const_iterator i, j;
// for each behaviour
for (i = params->priorities.begin(); i != params->priorities.end(); ++i) {
// if it existed before
j = params_->priorities.find(i->first);
if (j != params_->priorities.end()) {
// copy its current arbitration message to the new index
message_[i->second] = tmp[j->second];
}
else {
// else reset entry
message_[i->second] = MotionArbiterMessage();
}
}
}
params_ = params;
setMotion();
}
void scene::runNPC(float movSpd, float rotSpd)
{
mesh *pMesh;
mesh *wayPoint;
for (mMeshList.moveFirst(); !mMeshList.eof(); mMeshList.moveNext())
{
pMesh = mMeshList.getData();
if (pMesh->mName != NULL)
if (strcmp(pMesh->mName, "npc") == 0)
{
if (pMesh->mTarget == NULL)
{
pMesh->mTarget = new float[3];
setMotion((long) pMesh, 1, false);
moveMesh(
(long) pMesh,
MOV_VEL,
0,
0,
randEx(movSpd * 0.5, movSpd)
);
moveMesh(
(long) pMesh,
MOV_ANGVEL,
randEx(rotSpd * 0.5, rotSpd),
randEx(rotSpd * 0.5, rotSpd),
randEx(rotSpd * 0.5, rotSpd)
);
wayPoint = (mesh *) findWaypoint((long) pMesh);
if (wayPoint != NULL)
{
vectorCopy(pMesh->mTarget, wayPoint->mOrigin);
pMesh->mNext = wayPoint;
}
}
else
{
wayPoint = pMesh->mNext;
if
(
vectorDist(pMesh->mOrigin, wayPoint->mOrigin) <
pMesh->mRadius + wayPoint->mRadius
)
{
wayPoint = wayPoint->mNext;
if (wayPoint != NULL)
{
vectorCopy(pMesh->mTarget, wayPoint->mOrigin);
pMesh->mNext = wayPoint;
}
}
}
}
}
}
void TLFrame::setUseProperties( TLFrame *in_frame )
{
Q_CHECK_PTR( in_frame );
setUsed( in_frame -> isUsed() );
setKey( in_frame -> isKey() );
setLast( in_frame -> isLast() );
setUnknownMotion( in_frame -> isUnknownMotion() );
setMotion( in_frame -> isMotion() );
setHasDrawing( in_frame -> hasDrawing() );
}
void TLFrame::setUnknownMotion( bool in_is_unknown_motion )
{
is_unknown_motion = in_is_unknown_motion;
if ( is_unknown_motion )
{
setUsed( true );
setMotion( false );
}
update();
}
void startLineFollowing(int spd) {
oldError = 0;
setMotion(spd, 0);
followLine(spd);
TMRArd_InitTimer(LINE_FOLLOW_TIMER, LINE_FOLLOW_UPDATE_PERIOD);
}
task main()
{
disableDiagnosticsDisplay();
while(true) {
getJoystickSettings(joystick); // Update Buttons and Joysticks
wait1Msec(10);
/*--------------------------
controller one
-------------------------*/
/*-------------------------
maping
---------------------------
up =
down =
left = strafe left
right = strafe right
joystick left = left motors
joystick right = right motors
A = catcher down
B = catcher up
X =
Y =
L1 =
L2 =
R1 =
R2 =
---------------------------*/
// If lift is too high slower the driver motors
if(nMotorEncoder[liftRight] < GOVLIMIT) {
GOVERNOR = 1;
} else {
GOVERNOR = 2;
}
// Drive the robot from joystick 1
if((abs(joystick.joy1_y1) >= deadZone) || (abs(joystick.joy1_y2) >= deadZone)) {
setMotion(joystick.joy1_y1 / GOVERNOR, joystick.joy1_y2 / GOVERNOR);
}
/*else if(joystick.joy1_TopHat == 6) {
strafe(50);
}
else if(joystick.joy1_TopHat == 2){
strafe(-50);
}*/
else if(joy1Btn(JOY_BUTTON_LT)) {
strafe(-50);
}
else if(joy1Btn(JOY_BUTTON_RT)) {
strafe(50);
}
else {
stopMotors();
}
if(joy1Btn(JOY_BUTTON_RB))
{
int iCRate = servoChangeRate[goalCapture]; // Save change rate
servoChangeRate[goalCapture] = 0; // Max Speed
servo[goalCapture] = CATCHDOWN; // Set servo position
wait1Msec(20);
servoChangeRate[goalCapture] = iCRate; // Reset the servo
}
else if(joy1Btn(JOY_BUTTON_LB))
{
int iCRate = servoChangeRate[goalCapture]; // Save change rate
servoChangeRate[goalCapture] = 0; // Max Speed
servo[goalCapture] = CATCHUP; // Set servo position
wait1Msec(20);
servoChangeRate[goalCapture] = iCRate; // Reset the servo
}
/*--------------------------
controller two
-------------------------*/
/*-------------------------
maping
---------------------------
up =
down =
left =
right =
joystick left =
joystick right =
A =
B =
X =
Y =
LB = big backward
LT = big forward
RB = small backward
RT = small forward
---------------------------*/
// Raise/lower the lift
if(abs(joystick.joy2_y2) > deadZone) {
stopLiftTask(); //first, ensure that robot is not already moving the lift
if((joystick.joy2_y2 <= 0) && TSreadState(LTOUCH)) {
lift(0);// if touch sensor is active and driver says go down, stop the lift (don't burn out motor)
nMotorEncoder[liftMotor] = 0; //The lift is down, so set lift encoder to 0
} else {//If touch is NOT active or driver says go up
lift(rescale(joystick.joy2_y2)); //Raise lift at a rescaled value of the joystick
}
} else { //No controls?
lift(0); //Stop lift motors.
}
// Set the lift to preset heights
/*if(joy2Btn(JOY_BUTTON_A)) {
liftHeight(35);
} else if(joy2Btn(JOY_BUTTON_B)) {
liftHeight(65);
} else if(joy2Btn(JOY_BUTTON_Y)) {
liftHeight(95);
} else if(joy2Btn(JOY_BUTTON_X)) {
liftHeight(115);
} else if(joy2Btn(JOY_BUTTON_RT)) {
liftHeight(0);
nMotorEncoder[liftRight] = 0;
}*/
// Run the spinner to pick up balls
if((abs(joystick.joy2_y1)) >= deadZone) {
spin(rescale(joystick.joy2_y1));
} else {
spin(0);
}
}
}
int QDeclarativeParticles::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QDeclarativeItem::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
if (_id < 17)
qt_static_metacall(this, _c, _id, _a);
_id -= 17;
}
#ifndef QT_NO_PROPERTIES
else if (_c == QMetaObject::ReadProperty) {
void *_v = _a[0];
switch (_id) {
case 0:
*reinterpret_cast< QUrl*>(_v) = source();
break;
case 1:
*reinterpret_cast< int*>(_v) = count();
break;
case 2:
*reinterpret_cast< int*>(_v) = emissionRate();
break;
case 3:
*reinterpret_cast< qreal*>(_v) = emissionVariance();
break;
case 4:
*reinterpret_cast< int*>(_v) = lifeSpan();
break;
case 5:
*reinterpret_cast< int*>(_v) = lifeSpanDeviation();
break;
case 6:
*reinterpret_cast< int*>(_v) = fadeInDuration();
break;
case 7:
*reinterpret_cast< int*>(_v) = fadeOutDuration();
break;
case 8:
*reinterpret_cast< qreal*>(_v) = angle();
break;
case 9:
*reinterpret_cast< qreal*>(_v) = angleDeviation();
break;
case 10:
*reinterpret_cast< qreal*>(_v) = velocity();
break;
case 11:
*reinterpret_cast< qreal*>(_v) = velocityDeviation();
break;
case 12:
*reinterpret_cast< QDeclarativeParticleMotion**>(_v) = motion();
break;
}
_id -= 13;
} else if (_c == QMetaObject::WriteProperty) {
void *_v = _a[0];
switch (_id) {
case 0:
setSource(*reinterpret_cast< QUrl*>(_v));
break;
case 1:
setCount(*reinterpret_cast< int*>(_v));
break;
case 2:
setEmissionRate(*reinterpret_cast< int*>(_v));
break;
case 3:
setEmissionVariance(*reinterpret_cast< qreal*>(_v));
break;
case 4:
setLifeSpan(*reinterpret_cast< int*>(_v));
break;
case 5:
setLifeSpanDeviation(*reinterpret_cast< int*>(_v));
break;
case 6:
setFadeInDuration(*reinterpret_cast< int*>(_v));
break;
case 7:
setFadeOutDuration(*reinterpret_cast< int*>(_v));
break;
case 8:
setAngle(*reinterpret_cast< qreal*>(_v));
break;
case 9:
setAngleDeviation(*reinterpret_cast< qreal*>(_v));
break;
case 10:
setVelocity(*reinterpret_cast< qreal*>(_v));
break;
case 11:
setVelocityDeviation(*reinterpret_cast< qreal*>(_v));
break;
case 12:
setMotion(*reinterpret_cast< QDeclarativeParticleMotion**>(_v));
break;
}
_id -= 13;
} else if (_c == QMetaObject::ResetProperty) {
_id -= 13;
} else if (_c == QMetaObject::QueryPropertyDesignable) {
_id -= 13;
} else if (_c == QMetaObject::QueryPropertyScriptable) {
_id -= 13;
} else if (_c == QMetaObject::QueryPropertyStored) {
_id -= 13;
} else if (_c == QMetaObject::QueryPropertyEditable) {
_id -= 13;
} else if (_c == QMetaObject::QueryPropertyUser) {
_id -= 13;
}
#endif // QT_NO_PROPERTIES
return _id;
}
