void KinemCNDEngine::action()
{
KinemSimpleShearBox::getBoxes_Dt();
if( ((shearSpeed > 0) && (gamma<=gammalim)) || ((shearSpeed < 0) /*&& (gamma>=gammalim)*/ ) )
{
if(temoinfin!=0)
temoinfin=0;
letMove(shearSpeed * dt,0);
gamma+=shearSpeed * dt;
}
else
{
stopMovement();
if(temoinfin==0)
{
Omega::instance().saveSimulation(Key + "endShear.xml");
temoinfin=1;
}
}
for(unsigned int j=0;j<gamma_save.size();j++)
{
if ( ( ( (shearSpeed>0)&&(gamma > gamma_save[j]) ) || ((shearSpeed<0)&&(gamma < gamma_save[j])) ) && (temoin_save[j]==0) )
{
stopMovement(); // reset of all the speeds before the save
Omega::instance().saveSimulation(Key+"_"+lexical_cast<string> (floor(gamma*1000)) + "mmsheared.xml");
temoin_save[j]=1;
}
}
}
// TODO: handle ANIMATE_INTERRUPT
void Opcode80CE::_run()
{
Logger::debug("SCRIPT") << "[80CE] [=] void animate_move_obj_to_tile(void* who, int tile, int speed)" << std::endl;
int speed = _script->dataStack()->popInteger();
int tile = _script->dataStack()->popInteger();
auto object = _script->dataStack()->popObject();
// ANIMATE_WALK (0)
// ANIMATE_RUN (1)
// ANIMATE_INTERRUPT (16) - flag to interrupt current animation
auto critter = dynamic_cast<Game::CritterObject*>(object);
auto state = Game::Game::getInstance()->locationState();
if (state)
{
auto tileObj = state->hexagonGrid()->at(tile);
auto path = state->hexagonGrid()->findPath(object->hexagon(), tileObj);
if (path.size())
{
critter->stopMovement();
critter->setRunning((speed & 1) != 0);
auto queue = critter->movementQueue();
for (auto pathHexagon : path)
{
queue->push_back(pathHexagon);
}
}
}
}
void turnRightLittle(){
P2DIR |= BIT5; // Left motor forward
P2OUT &= ~BIT5;
_delay_cycles(10000000/120); // implements turn less than 45 degrees
stopMovement();
}
void KinemCNLEngine::action()
{
if(LOG) cout << "debut applyCondi du CNCEngine !!" << endl;
KinemSimpleShearBox::getBoxes_Dt();
if(LOG) cout << "gamma = " << lexical_cast<string>(gamma) << " et gammalim = " << lexical_cast<string>(gammalim) << endl;
if(gamma<=gammalim)
{
if(LOG) cout << "Je suis bien dans la partie gamma < gammalim" << endl;
if(temoin==0)
{
if(LOG) cout << "Je veux maintenir la Force a f0 = : " << f0 << endl;
temoin=1;
}
computeDY(0.0);
letMove(shearSpeed*dt,deltaH);
gamma+=shearSpeed * dt;
}
else if (temoin<2)
{
stopMovement(); // INDISPENSABLE !
it_stop=scene->iter;
cout << "Shear stopped : gammaLim reached at it "<< it_stop << endl;
temoin=2;
}
else if (temoin==2 && (scene->iter==(it_stop+5000)) )
{
Omega::instance().saveSimulation(Key + "endShear" +lexical_cast<string> ( scene->iter ) + ".xml");
Omega::instance().pause();
}
for(unsigned int j=0;j<gamma_save.size();j++)
{
if ((gamma > gamma_save[j]) && (temoin_save[j]==0))
{
stopMovement(); // reset of all the speeds before the save
Omega::instance().saveSimulation(Key+"_"+lexical_cast<string> (floor(gamma*1000)) +"_" +lexical_cast<string> (floor(gamma*10000)-10*floor(gamma*1000))+ "mmsheared.xml");
temoin_save[j]=1;
}
}
}
void turnRightBig(){
P2DIR |= BIT5; // Left motor forward
P2OUT &= ~BIT5;
P2DIR |= BIT0; // Right motor reverse
P2OUT &= ~BIT0;
_delay_cycles(10000000/10); // implements turn greater than 45 degrees
stopMovement();
}
void turnLeftLittle(){
P2DIR |= BIT1; // Right motor forward
P2OUT &= ~BIT1;
P2DIR |= BIT3; // Left motor reverse
P2OUT &= ~BIT3;
_delay_cycles(10000000/120); // implements turn less than 45 degrees
stopMovement();
}
void Disp2DPropLoadEngine::action()
{
if(LOG) cerr << "debut applyCondi !!" << endl;
leftbox = Body::byId(id_boxleft);
rightbox = Body::byId(id_boxright);
frontbox = Body::byId(id_boxfront);
backbox = Body::byId(id_boxback);
topbox = Body::byId(id_topbox);
boxbas = Body::byId(id_boxbas);
if(firstIt)
{
it_begin=scene->iter;
H0=topbox->state->pos.y();
X0=topbox->state->pos.x();
Vector3r F_sup=scene->forces.getForce(id_topbox);
Fn0=F_sup.y();
Ft0=F_sup.x();
Real OnlySsInt=0 // the half number of real sphere-sphere (only) interactions, at the beginning of the perturbation
,TotInt=0 // the half number of all the real interactions, at the beginning of the perturbation
;
InteractionContainer::iterator ii = scene->interactions->begin();
InteractionContainer::iterator iiEnd = scene->interactions->end();
for( ; ii!=iiEnd ; ++ii )
{
if ((*ii)->isReal())
{
TotInt++;
const shared_ptr<Body>& b1 = Body::byId( (*ii)->getId1() );
const shared_ptr<Body>& b2 = Body::byId( (*ii)->getId2() );
if ( (b1->isDynamic()) && (b2->isDynamic()) )
OnlySsInt++;
}
}
coordSs0 = OnlySsInt/8590; // 8590 is the number of spheres in the CURRENT case
coordTot0 = TotInt / 8596; // 8596 is the number of bodies in the CURRENT case
firstIt=false;
}
if ( (scene->iter-it_begin) < nbre_iter)
{ letDisturb();
}
else if ( (scene->iter-it_begin) == nbre_iter)
{
stopMovement();
string fileName=Key + "DR"+boost::lexical_cast<string> (nbre_iter)+"ItAtV_"+boost::lexical_cast<string> (v)+"done.xml";
// Omega::instance().saveSimulation ( fileName );
saveData();
}
}
/**
* Processes the properties of the body, shape, mass friction, etc...
*/
void RigidBody::processBody_() {
// Remove the current body from the physics engine
selfRemoveBody_();
if(!shape_) {
return;
}
shape_->calculateLocalInertia(mass_, inertia_);
float x = getX();
float y = getY();
float z = getZ();
if(body_) {
delete(body_);
}// else {
//btTransform xform;
//xform = motionState_.getWorldTransform();
//xform.setRotation(btQuaternion (btVector3(getRotX(), getRotY(), getRotZ()), getRotAngle()*(PI/180)));
//xform.setOrigin(
//((btPairCachingGhostObject*)body_)->setWorldTransform (xform);
//motionState_->setWorldTransform (xform);
btTransform xform;
motionState_->getWorldTransform(xform);
xform.setOrigin(btVector3(getX(), getY(), getZ()));
motionState_->setWorldTransform(xform);
//}
btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
mass_,
motionState_,
shape_,
inertia_
);
rigidBodyCI.m_friction = friction_;
rigidBodyCI.m_mass = mass_;
body_ = new btRigidBody(rigidBodyCI);
setXYZ(x, y, z);
selfAddBody_();
/*btRigidBody* rb = dynamic_cast<btRigidBody*>(body_);
if(rb) {
rb->clearForces();
}*/
stopMovement();
}
void CPlayerBase::setControllable(bool control)
{
controlEnabled = control;
stopMovement();
ChangeState("idle");
ChangeCommonState("idle");
if (!control) {
if (!h_sense_vision.isValid()) h_sense_vision = tags_manager.getFirstHavingTag("game_controller");
sense_vision = GETH_COMP(h_sense_vision, TCompSenseVision);
assert(sense_vision);
if (!sense_vision) return;
sense_vision->unregisterHandle(myHandle);
}
}
task main()
{
waitForStart();
servo[sweeper] = 169;
servo[clawservo1] = 240;
servo[clawservo2] = 240;
wait1Msec(1000);
forward(.6);
wait1Msec(600);
stopMovement();
servo[sweeper] = 0;
wait1Msec(1000);
rotateLeft();
wait1Msec(400);
forward();
wait1Msec(1900);
rotateRight();
wait1Msec(700);
forward();
wait1Msec(950);
stopMovement();
forward(.5);
wait1Msec(500);
while (true)
{
forward(-0.5);
wait1Msec(1000);
forward(0.65);
wait1Msec(1000);
}
}
void AuthoringHandler::view_EntityDeleted(Eris::Entity* entity)
{
VisualizationStore::iterator I = mVisualizations.find(static_cast<EmberEntity*> (entity));
if (I != mVisualizations.end()) {
//see if there's an ongoing movement for the deleted entity, and if we therefore should stop that
if (mMoveInstance) {
if (I->second == &mMoveInstance->getVisualization()) {
stopMovement();
}
}
mVisualizations.erase(I);
delete I->second;
} else {
S_LOG_WARNING("Got delete signal for entity which doesn't has an authoring visualization. This should not happen.");
}
}
void rotateLeft(int deg) {
MoveBot(LEFT);
//WaitUs(deg/10*LOOP_ROTATE);
stopMovement();
}
void Referee::slotRead()
{
QDataStream in(messengerOfTheGods);
in.setVersion(QDataStream::Qt_4_0);
if (messageSize == 0)
{
if ((int) messengerOfTheGods->bytesAvailable() < (int) sizeof(quint16))
return;
quint16 size;
in >> size;
messageSize = (int) size;
}
if ((int) messengerOfTheGods->bytesAvailable() < (int) messageSize)
return;
// Here we have received a complete message
bool lastPacket = false;
do
{
quint16 id;
in >> id;
int code = (int) id;
qreal a, b;
quint16 color;
switch (code)
{
case HERMES_GAME_START:
Q_EMIT gameStart();
break;
case HERMES_DETECTION_START:
Q_EMIT detectionStart();
break;
case HERMES_GAME_OVER:
Q_EMIT gameOver();
break;
case HERMES_A_B:
in >> a;
in >> b;
Q_EMIT abValues((double) a, (double) b);
break;
case HERMES_TEAMCOLOR:
in >> color;
Q_EMIT trueColorOfTeam((TeamColor) color);
break;
case HERMES_STOP_MOVEMENT:
Q_EMIT stopMovement();
break;
default:
messengerOfTheGods->readAll();
break;
}
if (!in.atEnd())
{
lastPacket = false;
if ((int) messengerOfTheGods->bytesAvailable() < (int) sizeof(quint16))
{
messengerOfTheGods->readAll();
lastPacket = true;
}
else
{
quint16 size;
in >> size;
messageSize = (int) size;
}
}
else
{
lastPacket = true;
}
} while (!lastPacket);
// sets corrected velocity of joystick command
void CollisionVelocityFilter::performControllerStep() {
double dt;
double vx_max, vy_max;
geometry_msgs::Twist cmd_vel;
cmd_vel.linear = robot_twist_linear_;
cmd_vel.angular = robot_twist_angular_;
dt = ros::Time::now().toSec() - last_time_;
last_time_ = ros::Time::now().toSec();
// if closest obstacle is within stop_threshold, then do not move
if( closest_obstacle_dist_ < stop_threshold_ ) {
stopMovement();
return;
}
double vel_angle = atan2(cmd_vel.linear.y,cmd_vel.linear.x);
vx_max = v_max_ * fabs(cos(vel_angle));
if (vx_max > fabs(cmd_vel.linear.x)) vx_max = fabs(cmd_vel.linear.x);
vy_max = v_max_ * fabs(sin(vel_angle));
if (vy_max > fabs(cmd_vel.linear.y)) vy_max = fabs(cmd_vel.linear.y);
//Slow down in any way while approximating an obstacle:
if(closest_obstacle_dist_ < influence_radius_) {
double F_x, F_y;
double vx_d, vy_d, vx_factor, vy_factor;
double kv_obst=kv_, vx_max_obst=vx_max, vy_max_obst=vy_max;
//implementation for linear decrease of v_max:
double obstacle_linear_slope_x = vx_max / (obstacle_damping_dist_ - stop_threshold_);
vx_max_obst = (closest_obstacle_dist_- stop_threshold_ + stop_threshold_/10.0f) * obstacle_linear_slope_x;
if(vx_max_obst > vx_max) vx_max_obst = vx_max;
else if(vx_max_obst < 0.0f) vx_max_obst = 0.0f;
double obstacle_linear_slope_y = vy_max / (obstacle_damping_dist_ - stop_threshold_);
vy_max_obst = (closest_obstacle_dist_- stop_threshold_ + stop_threshold_/10.0f) * obstacle_linear_slope_y;
if(vy_max_obst > vy_max) vy_max_obst = vy_max;
else if(vy_max_obst < 0.0f) vy_max_obst = 0.0f;
//Translational movement
//calculation of v factor to limit maxspeed
double closest_obstacle_dist_x = closest_obstacle_dist_ * cos(closest_obstacle_angle_);
double closest_obstacle_dist_y = closest_obstacle_dist_ * sin(closest_obstacle_angle_);
vx_d = kp_/kv_obst * closest_obstacle_dist_x;
vy_d = kp_/kv_obst * closest_obstacle_dist_y;
vx_factor = vx_max_obst / sqrt(vy_d*vy_d + vx_d*vx_d);
vy_factor = vy_max_obst / sqrt(vy_d*vy_d + vx_d*vx_d);
if(vx_factor > 1.0) vx_factor = 1.0;
if(vy_factor > 1.0) vy_factor = 1.0;
F_x = - kv_obst * vx_last_ + vx_factor * kp_ * closest_obstacle_dist_x;
F_y = - kv_obst * vy_last_ + vy_factor * kp_ * closest_obstacle_dist_y;
cmd_vel.linear.x = vx_last_ + F_x / virt_mass_ * dt;
cmd_vel.linear.y = vy_last_ + F_y / virt_mass_ * dt;
}
// make sure, the computed and commanded velocities are not greater than the specified max velocities
if (fabs(cmd_vel.linear.x) > vx_max) cmd_vel.linear.x = sign(cmd_vel.linear.x) * vx_max;
if (fabs(cmd_vel.linear.y) > vy_max) cmd_vel.linear.y = sign(cmd_vel.linear.y) * vy_max;
if (fabs(cmd_vel.angular.z) > vtheta_max_) cmd_vel.angular.z = sign(cmd_vel.angular.z) * vtheta_max_;
// limit acceleration:
// only acceleration (in terms of speeding up in any direction) is limited,
// deceleration (in terms of slowing down) is handeled either by cob_teleop or the potential field
// like slow-down behaviour above
if (fabs(cmd_vel.linear.x) > fabs(vx_last_))
{
if ((cmd_vel.linear.x - vx_last_)/dt > ax_max_)
cmd_vel.linear.x = vx_last_ + ax_max_ * dt;
else if((cmd_vel.linear.x - vx_last_)/dt < -ax_max_)
cmd_vel.linear.x = vx_last_ - ax_max_ * dt;
}
if (fabs(cmd_vel.linear.y) > fabs(vy_last_))
{
if ((cmd_vel.linear.y - vy_last_)/dt > ay_max_)
cmd_vel.linear.y = vy_last_ + ay_max_ * dt;
else if ((cmd_vel.linear.y - vy_last_)/dt < -ay_max_)
cmd_vel.linear.y = vy_last_ - ay_max_ * dt;
}
if (fabs(cmd_vel.angular.z) > fabs(vtheta_last_))
{
if ((cmd_vel.angular.z - vtheta_last_)/dt > atheta_max_)
cmd_vel.angular.z = vtheta_last_ + atheta_max_ * dt;
else if ((cmd_vel.angular.z - vtheta_last_)/dt < -atheta_max_)
cmd_vel.angular.z = vtheta_last_ - atheta_max_ * dt;
}
pthread_mutex_lock(&m_mutex);
vx_last_ = cmd_vel.linear.x;
vy_last_ = cmd_vel.linear.y;
vtheta_last_ = cmd_vel.angular.z;
pthread_mutex_unlock(&m_mutex);
// publish adjusted velocity
topic_pub_command_.publish(cmd_vel);
return;
}
/*
* main.c
*/
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
BCSCTL1 = CALBC1_8MHZ; // 8MHz clock
DCOCTL = CALDCO_8MHZ;
P2DIR |= BIT2; // P2.2 is associated with TACCTL1
P2SEL |= BIT2;
P2DIR |= BIT4; // P2.4 is associated with TACCTL2
P2SEL |= BIT4;
TA1CTL = ID_3 | TASSEL_2 | MC_1; //set duty cycle and MCLK
TA1CCR0 = 100;
TA1CCR1 = 50;
TA1CCTL1 = OUTMOD_7; // set TACCTL1 to Reset / Set mode
TA1CCR2 = 50; // set TACCTL2 to Set / Reset mode
TA1CCTL2 = OUTMOD_3;
ADC10CTL0 = ADC10SHT_3 + ADC10ON + ADC10IE; // ADC10ON, interrupt enabled
ADC10CTL1 = ADC10DIV_7;
ADC10CTL1 |= ADC10SSEL1|ADC10SSEL0; // Select SMCLK
for (;;){
stopMovement();
_delay_cycles(110000);
if(leftSensor() < 0x190) // two if statements to stay close to left wall
{
turnLeftLittle();
_delay_cycles(9000);
stopMovement();
_delay_cycles(100000);
}
if(leftSensor() >= 0x170)
{
turnRightLittle();
_delay_cycles(9000);
stopMovement();
_delay_cycles(100000);
}
if (centerSensor() >=0x170 && leftSensor() >= 0x165) // statement to turn right if both left and center sensors triggered
{
turnRightBig();
_delay_cycles(7500);
stopMovement();
_delay_cycles(100000);
}
if (centerSensor() < 0x170 && leftSensor() >= 0x170) // has robot move forward when neither sensors are triggered
{
moveForward();
_delay_cycles(100000);
}
else if (centerSensor() < 0x190)
{
moveForward();
_delay_cycles(100000);
}
}
return 0;
}
void BPlayer::win()
{
stopMovement();
Director::getInstance()->pause();
winFunc(this);
}
void moveForward(int dist) {
MoveBot(FORWARD);
//WaitUs(dist*LOOP_FORWARD);
stopMovement();
}
void rotateRight(int deg) {
MoveBot(RIGHT);
//WaitUs(deg/10*LOOP_ROTATE);
stopMovement();
}