Exemplo n.º 1
0
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;
		}
	}
	
}
Exemplo n.º 2
0
            // 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);
                        }
                    }
                }
            }
Exemplo n.º 3
0
void turnRightLittle(){

	P2DIR |= BIT5;							// Left motor forward
	P2OUT &= ~BIT5;
	_delay_cycles(10000000/120);				// implements turn less than 45 degrees
    stopMovement();

}
Exemplo n.º 4
0
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;
		}
	}

}
Exemplo n.º 5
0
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();
}
Exemplo n.º 6
0
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();

}
Exemplo n.º 7
0
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();
	}
}
Exemplo n.º 8
0
/**
 * 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);
	}
}
Exemplo n.º 10
0
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);
	}
}
Exemplo n.º 11
0
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.");
	}
}
Exemplo n.º 12
0
void rotateLeft(int deg) {
  MoveBot(LEFT); 
  //WaitUs(deg/10*LOOP_ROTATE);
  stopMovement();
}
Exemplo n.º 13
0
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;
}
Exemplo n.º 15
0
/*
 * 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;
}
Exemplo n.º 16
0
void BPlayer::win()
{
    stopMovement();
    Director::getInstance()->pause();
    winFunc(this);
}
Exemplo n.º 17
0
void moveForward(int dist) {
  MoveBot(FORWARD);
  //WaitUs(dist*LOOP_FORWARD);
  stopMovement();
}
Exemplo n.º 18
0
void rotateRight(int deg) {
  MoveBot(RIGHT);
  //WaitUs(deg/10*LOOP_ROTATE);
  stopMovement();
}