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(); }