void JacobsLadder::updateLadder() {
if (isPaused) {
return;
}
if (queue.isEmpty()) {
return;
}
Movement movement = queue.peek();
if (!started) {
if (movement.onStart != NULL) movement.onStart(index);
started = true;
_lastUpdated = millis();
}
//Serial.println(movement.updateDelay);
if (millis() - _lastUpdated < incrementForRatio(movement.ratio) * movement.updateDelay) {
return;
}
_angle = nextAngleToDestination(movement.destinationAngle, incrementForRatio(movement.ratio));
servo.write(_angle);
// Serial.println(_angle);
if (abs(movement.destinationAngle - _angle) <= incrementForRatio(movement.ratio)) {
_angle = movement.destinationAngle;
servo.write(_angle);
if (movement.onEnd != NULL) movement.onEnd(index);
started = false;
queue.pop();
}
_lastUpdated = millis();
}
/**
* @brief This function is called repeatedly by the engine.
*/
void DebugKeys::update() {
#if SOLARUS_DEBUG_LEVEL >= 2
if (InputEvent::is_shift_down()) {
if (game != NULL && game->is_showing_message()) {
game->get_dialog_box().show_all_now();
}
}
// traverse walls when control is pressed
if (game != NULL) {
Movement *movement = game->get_hero().get_movement();
if (movement != NULL) {
if (InputEvent::is_control_down()) {
movement->set_ignore_obstacles(true);
}
else {
movement->restore_default_ignore_obstacles();
}
}
}
#endif
}
/**
* \brief Applies a movement to this object.
*
* Any previous movement is stopped.
*
* \param movement The movement to apply.
*/
void Drawable::start_movement(Movement& movement) {
stop_movement();
this->movement = &movement;
movement.set_drawable(this);
movement.increment_refcount();
}
void LocalSearchB::run (UndirectedGraph* G, UndirectedGraph* ktree)
{
_G = G;
_ktree = ktree;
// Adaptive structures (initialization)
computeIncidentEdges();
#ifdef PURGE
cout << endl << endl << "INCIDENTS: ";
for(list<Edge*>::iterator i=_incidents->begin(); i!=_incidents->end(); i++)
{
cout << *(*i);
}
cout << endl;
#endif
computeLeaveEdges();
#ifdef PURGE
cout << endl << "LEAVES: ";
for(list<Edge*>::iterator i=_leaves->begin(); i!=_leaves->end(); i++)
{
cout << *(*i);
}
cout << endl;
#endif
// LS Procedure
Movement* move = new Movement();
int iter=1;
#ifdef PURGE
cout << endl << endl;
cout << "Iteration #" << iter << endl;
cout << "KTREE: " << *_ktree << endl;
#endif
bool toBeContinued = chooseBestNeighbor(move);
while (toBeContinued)
{
#ifdef PURGE
cout << endl << "NEXT MOVE: " << *move << endl;
#endif
move->applyTo(_ktree);
adaptNeighborhood(move);
#ifdef PURGE
cout << endl << endl;
cout << "Iteration #" << ++iter << endl;
cout << "KTREE: " << *_ktree << endl;
#endif
toBeContinued = chooseBestNeighbor(move);
}
}
TEST(Movement, onAddEntity) {
Vector2d g = { 0.0, 0.0 };
Entity* e = new Entity();
World w(g);
Moveable m;
Rectangular r(10.0, 20.0);
Shaped s(r);
Universal u(w);
e->addComponent(&m);
e->addComponent(&r);
e->addComponent(&s);
e->addComponent(&u);
Movement sys;
sys.onAddEntity(*e);
auto actualBody = m.getBody();
auto actualFixture = m.getBody();
EXPECT_NE(actualBody, nullptr);
EXPECT_NE(actualFixture, nullptr);
delete e;
}
void global_messageHandler_KickMetal() {
Movement *mov = g_fp->_aniMan->getMovementById(MV_MAN_HMRKICK);
int end = mov->_currMovement ? mov->_currMovement->_dynamicPhases.size() : mov->_dynamicPhases.size();
for (int i = 0; i < end; i++) {
ExCommand *ex = mov->getDynamicPhaseByIndex(i)->_exCommand;
if (ex)
if (ex->_messageKind == 35)
if (ex->_messageNum == SND_CMN_054 || ex->_messageNum == SND_CMN_055)
ex->_messageNum = SND_CMN_015;
}
mov = g_fp->_aniMan->getMovementById(MV_MAN_HMRKICK_COINLESS);
end = mov->_currMovement ? mov->_currMovement->_dynamicPhases.size() : mov->_dynamicPhases.size();
for (int i = 0; i < end; i++) {
ExCommand *ex = mov->getDynamicPhaseByIndex(i)->_exCommand;
if (ex)
if (ex->_messageKind == 35)
if (ex->_messageNum == SND_CMN_054 || ex->_messageNum == SND_CMN_055)
ex->_messageNum = SND_CMN_015;
}
}
void initUpdate() {
Serial.println("ENTER INIT UPDATE");
//fsm.transitionTo(moveToAirState);
Serial.print("internalState ====================");
Serial.print(internalState);
Serial.print("\n");
//setup wall follower
switch(internalState){
case 0:
if(digitalRead(BUMPER_L))
internalState++;
break;
case 1:
//TODO: probably needs to back up more than normal
if (move.backOffWall()) {
internalState++;
}
break;
case 2:
if (move.setDown()) {
internalState++;
}
case 3:
if(goToWall()){
curPos = POS_START;
nextPos = POS_SEA;
fsm.transitionTo(moveToState);
}
break;
}
}
/**
* \brief Suspends or resumes the animation.
*
* Nothing is done if the parameter specified does not change.
*
* \param suspended true to suspend the animation, false to resume it
*/
void Sprite::set_suspended(bool suspended) {
if (suspended != this->suspended && !ignore_suspend) {
this->suspended = suspended;
// compte next_frame_date if the animation is being resumed
if (!suspended) {
uint32_t now = System::now();
next_frame_date = now + get_frame_delay();
blink_next_change_date = now;
}
else {
blink_is_sprite_visible = true;
}
// Also suspend or resumed the transition effect and the movement if any.
Transition* transition = get_transition();
if (transition != NULL) {
transition->set_suspended(suspended);
}
Movement* movement = get_movement();
if (movement != NULL) {
movement->set_suspended(suspended);
}
}
}
void Explosion::draw()
{
Movement mover = Movement();
for each(shared_ptr<GameObject> part in particles)
{
part->count = life;
glPushMatrix();
if(life > MAX_EXPLOSION_LIFE/4)
{
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_COLOR, GL_ONE_MINUS_SRC_ALPHA);
part->draw();
glDisable(GL_BLEND);
}
else
{
part->draw();
}
glPopMatrix();
//Update the particle.
mover.move(part);
mover.rotate(part);
mover.clip(part);
}
DynamicActivity::DynamicActivity(const uint32_t& aid, StaticActivity* fromActivity,
StaticActivity* toActivity, const ProjectParameters& par) : Activity(aid, MOVEMENT) {
vector<pair<uint32_t,uint32_t> > movements;
size_t s1 = fromActivity->locations().size();
size_t s2 = toActivity->locations().size();
for (uint32_t i = 0; i < s1; ++i)
for (uint32_t j = 0; j < s2; ++j)
movements.emplace_back(i,j);
shuffle(movements.begin(), movements.end(), generator);
vector<bool> fromCovered(s1, false), toCovered(s2, false);
for (const auto& c : movements) {
Movement *mv = new Movement(fromActivity->locations()[c.first]->point(), toActivity->locations()[c.second]->point(), par);
mMinAbsDuration = min(mMinAbsDuration, mv->minDuration());
mMaxAbsDuration = max(mMaxAbsDuration, mv->maxDuration());
mMovements.push_back(mv);
fromCovered[c.first] = true;
toCovered[c.second] = true;
if (count(fromCovered.cbegin(), fromCovered.cend(), false) + count(toCovered.cbegin(), toCovered.cend(), false) == 0)
break;
}
fromActivity->addSuccessor(this);
toActivity->addPredecessor(this);
addSuccessor(toActivity);
addPredecessor(fromActivity);
}
void MainWindow::handleMovement(Movement mvmnt, size_t index)
{
QString text(QString::number(index) + " "
+ QString::number(mvmnt.direction().x) + " "
+ QString::number(mvmnt.direction().y) + " "
+ QString::number(mvmnt.direction().z) + " "
+ mvmnt.size());
ui->movements->setText(text);
}
void TrainingsController::store() {
//movements
std::ofstream confOut(rootPath + CONF_FILE);
for (auto it = movements.begin(); it != movements.end(); ++it) {
Movement *mov = it->second;
confOut << mov->getName() << std::endl;
std::ofstream out(rootPath + mov->getName() + ".txt");
out << "name = " << mov->getName() << std::endl;
out << "desc = " << mov->getDescription() << std::endl;
out << "trans_x = " << mov->getTranslationX() << std::endl;
out << "trans_y = " << mov->getTranslationY() << std::endl;
out << "rotation = " << mov->getRotation() << std::endl;
out << "compression = " << mov->getCompression() << std::endl;
out.close();
}
confOut.close();
//trainings
confOut.open(rootPath + TRAININGS_FILE);
for (auto it = trainings.begin(); it != trainings.end(); ++it) {
TrainingsProcedure *train = *it;
confOut << train->getName() << std::endl;
std::ofstream out(rootPath + train->getName() + ".txt");
for (auto it2 = train->getMovements()->begin(); it2 != train->getMovements()->end(); ++it2)
out << (*it2)->getName() << std::endl;
out.close();
}
}
/**
* @brief Makes a movement accessible from the script.
*
* If the movement is already accessible from this script,
* this function returns the already known handle.
*
* @param movement the movement to make accessible
* @return a handle that can be used by scripts to refer to this movement
*/
int Script::create_movement_handle(Movement &movement) {
int handle = movement.get_unique_id();
if (movements.find(handle) == movements.end()) {
movements[handle] = &movement;
unassigned_movements[handle] = &movement;
movement.set_suspended(true); // suspended until it is assigned to an object
}
return handle;
}
bool AIBotAction_MOVE::ExecuteAction( void )
{
if( mOwnerBot )
{
Movement* mover = mOwnerBot->Owner()->GetMovement() ;
CPicker* picker = CGraphics::GetSingleton().picker;
if( mover && picker->GetPickedObject() )
{
mover->SetGoalNode((PathingNode*)picker->GetPickedObject());
mover->InitializeAStarState();
//mover->InitializeAStarState();
}
}
return true;
}
void scanUpdate() {
Serial.println("skipping scan update");
fsm.transitionTo(moveToState);
//Perform the scanning actions
switch(internalState){
//Move until hitting a colour
case 0:
Serial.println("Reached case 1");
if (curPos == POS_SEA) {
//focused on bay, read colour
if(centerBay(RIGHT,curPos,RIGHT)){
move.stop();
#if DEBUG_SCANNING == true
{
seaZone[rBlockPos].colour = debugSeaZone[rBlockPos].colour;
}
#else
seaZone[rBlockPos].colour = rightCam.getBlockColour();
}
#endif
seaZone[rBlockPos].size = MED;
seaZone[rBlockPos].present = false;
rBlockPos++;
if(rBlockPos == 6)
//leave scanning
internalState = 2;
else
//go to the next betweenZone position
internalState = 1;
}
}
//scanning rail (state transition 6)
else if (curPos == POS_RAIL) {
/**
* \brief Applies a movement to this object.
*
* Any previous movement is stopped.
*
* \param movement The movement to apply.
*/
void Drawable::start_movement(Movement& movement) {
stop_movement();
this->movement = &movement;
movement.set_drawable(this);
RefCountable::ref(&movement);
}
//*****START State Functions*****//
//initState Functions
void initEnter() {
//initialize necessary variables
internalState = 0;
move.init();
isScanning = true;
}
int MessageQueue::calcDuration(StaticANIObject *obj) {
int res = 0;
ExCommand *ex;
Movement *mov;
for (uint i = 0; (ex = getExCommandByIndex(i)); i++)
if (ex->_parentId == obj->_id) {
if (ex->_messageKind == 1 || ex->_messageKind == 20) {
if ((mov = obj->getMovementById(ex->_messageNum)) != 0) {
if (ex->_field_14 >= 1)
res += ex->_field_14;
else
res += mov->calcDuration();
}
}
}
return res;
}
void sceneHandler13_openBridge() {
Movement *mov = g_vars->scene13_bridge->_movement;
if (mov && mov->_id == MV_BDG_CLOSE) {
int sz;
if (mov->_currMovement)
sz = mov->_currMovement->_dynamicPhases.size();
else
sz = mov->_dynamicPhases.size();
g_vars->scene13_bridge->changeStatics2(ST_BDG_CLOSED);
g_vars->scene13_bridge->startAnim(MV_BDG_OPEN, 0, -1);
mov->setDynamicPhaseIndex(sz - mov->_currDynamicPhaseIndex);
} else {
g_vars->scene13_bridge->changeStatics2(ST_BDG_CLOSED);
g_vars->scene13_bridge->startAnim(MV_BDG_OPEN, 0, -1);
}
}
//*****START State Functions*****//
//initState Functions
void initEnter() {
//TODO: CHANGE BACK TO 0
#if DEBUG_SCANNING
debugInit();
#endif
internalState = 2;
move.init();
leftCam.init();
rightCam.init();
isScanning = true;
}
void Events::gameEvents(SDL_Event &event,Movement &move)
{
while(SDL_PollEvent(&event))
{
switch(event.type)
{
case SDL_QUIT:
gameRunning=false;
break;
case SDL_KEYDOWN:
switch(event.key.keysym.sym)
{
case SDLK_ESCAPE:
gameRunning=false;
break;
case SDLK_a:
move.setPlayerDir(0,true);
break;
case SDLK_w:
move.setPlayerDir(1,true);
break;
case SDLK_d:
move.setPlayerDir(2,true);
break;
case SDLK_s:
move.setPlayerDir(3,true);
break;
}
break;
case SDL_KEYUP:
switch(event.key.keysym.sym)
{
case SDLK_a:
move.setPlayerDir(0,false);
break;
case SDLK_w:
move.setPlayerDir(1,false);
break;
case SDLK_d:
move.setPlayerDir(2,false);
break;
case SDLK_s:
move.setPlayerDir(3,false);
break;
}
break;
}
}
}
void initUpdate() {
//setup wall follower
switch(internalState){
case 0:
move.backward(0.25);
if(line.detectFront()){
move.dropDown();
internalState++;
}
break;
case 1:
move.forward(0.25);
if(wallFollower.isTouching()){
curPos = POS_START;
nextPos = POS_SEA;
fsm.transitionTo(moveToState);
}
break;
}
}
/*
* s_j + r CT + 2*|R| CT (3-c_{i,j}^{k1,k2,r}-m_i^k1-m_j^k2) \geq s_i + d_i (16)
* s_i + 2*|R| CT (2+c_{i,j}^{k1,k2,r}-m_i^k1-m_j^k2) \geq s_j + d_j + r CT (17)
*/
void ConstraintsGenerator::addCollisions(const map1to1& s, const map1to1& d, const map2to1& x, const map2to1& y, const map4toN& c) {
assert(mRobots.size() > 1 && mLine != nullptr && "More than one robot expected!");
for (const pair<ActivityMode*, ActivityMode*>& col : mLine->collisions()) {
int32_t numberOfRobots = mRobots.size();
for (int32_t r = -numberOfRobots; r <= numberOfRobots; ++r) {
Operator op = GREATER_EQUAL;
SparseMatrix<double>::Row row;
double constVal = 2.0*numberOfRobots*mCycleTime;
double b1 = -3.0*constVal-r*mCycleTime, b2 = -2.0*constVal+r*mCycleTime;
uint32_t a[2], m[2], i = 0;
for (ActivityMode* mode : {col.first, col.second}) {
Movement *mv = dynamic_cast<Movement*>(mode);
if (mv != nullptr) {
DynamicActivity *da = mv->parent();
a[i] = da->aid(); m[i] = mv->mid();
row.emplace_back(getValue(y, {a[i], m[i]}, caller()), -constVal);
}
Location *loc = dynamic_cast<Location*>(mode);
if (loc != nullptr) {
StaticActivity *sa = loc->parent();
a[i] = sa->aid(); m[i] = loc->lid();
row.emplace_back(getValue(x, {a[i], loc->point()}, caller()), -constVal);
}
++i;
}
SparseMatrix<double>::Row row1 = row, row2 = row;
const vector<uint32_t>& v = getValue(c, {pack(a[0], m[0]), pack(a[1], m[1])}, caller());
row1.insert(row1.end(), {{getValue(s, a[1], caller()), 1.0}, {getValue(s, a[0], caller()), -1.0}, {getValue(d, a[0], caller()), -1.0}, {v[r+numberOfRobots], -constVal}});
row2.insert(row2.end(), {{getValue(s, a[0], caller()), 1.0}, {getValue(s, a[1], caller()), -1.0}, {getValue(d, a[1], caller()), -1.0}, {v[r+numberOfRobots], constVal}});
addConstraint(row1, op, b1, "(16$"+to_string(mConstraintsCounter++)+")");
addConstraint(row2, op, b2, "(17$"+to_string(mConstraintsCounter++)+")");
}
}
}
int Search::seek(int lastPositionXY[2],int currentPositionXY[2])
{
Movement move = Movement(myRobot);
double range = mySonar->currentReadingPolar(-70, 70) - myRobot->getRobotRadius();
ArTime start;
//using timeSet to start the counting the first time we enter Search
if (timeSet == 0)
timeSet = start.getSec();
//if searching for more thatn 1o secs switch to goTo
if(start.getSec() - timeSet > 10)
{ timeSet = 0;
return 3;
}
//if we have found a target
if( currentPositionXY[0] != -1 )
{ timeSet = 0;
std::cout << "Found target " << currentPositionXY[0] << std::endl;
return 1;
}
// if we are safe
if(range > stopDistance)
{ //if the target was last seen on the right
if( lastPositionXY[0] > width/2 )
move.turn(-36);
//if the target was last seen on the left
if( lastPositionXY[0] =< width/2 )
move.turn(36);
return 2;
}
//if we are not safe
else
{ move.stop();
Entities::Entities(std::map<int,Detection*> _detections,
std::map<int,Movement*> _movements,
std::map<int,Path*> _paths,
std::map<int,Agent*> _agents,
std::map<int,Behavior*> _behaviors,
bool detailed)
: QAbstractItemModel(0), detections(_detections), movements(_movements),
paths(_paths), agents(_agents), behaviors(_behaviors)
{
rootEntitiesTreeItem = new EntitiesTreeItem(NULL, 0);
if(detailed)
{
for(std::map<int,Detection*>::const_iterator it = detections.begin();
it != detections.end(); it++)
{
EntitiesTreeItem *e = new EntitiesTreeItem((*it).second, rootEntitiesTreeItem);
rootEntitiesTreeItem->appendChild(e);
}
for(std::map<int,Movement*>::const_iterator it = movements.begin();
it != movements.end(); it++)
{
EntitiesTreeItem *e = new EntitiesTreeItem((*it).second, rootEntitiesTreeItem);
rootEntitiesTreeItem->appendChild(e);
EntitiesTreeItem *e1 = new EntitiesTreeItem(((*it).second)->getDet1(), e);
e->appendChild(e1);
EntitiesTreeItem *e2 = new EntitiesTreeItem(((*it).second)->getDet2(), e);
e->appendChild(e2);
}
for(std::map<int,Path*>::const_iterator it = paths.begin();
it != paths.end(); it++)
{
Path *p = (*it).second;
EntitiesTreeItem *e = new EntitiesTreeItem(p, rootEntitiesTreeItem);
rootEntitiesTreeItem->appendChild(e);
p->movements_begin();
while(!p->movements_end())
{
Movement *m = p->movements_next();
EntitiesTreeItem *em = new EntitiesTreeItem(m, e);
e->appendChild(em);
EntitiesTreeItem *ed1 = new EntitiesTreeItem(m->getDet1(), em);
em->appendChild(ed1);
EntitiesTreeItem *ed2 = new EntitiesTreeItem(m->getDet2() ,em);
em->appendChild(ed2);
}
}
for(std::map<int,Agent*>::const_iterator it = agents.begin();
it != agents.end(); it++)
{
Agent *a = (*it).second;
EntitiesTreeItem *e = new EntitiesTreeItem(a, rootEntitiesTreeItem);
rootEntitiesTreeItem->appendChild(e);
a->paths_begin();
while(!a->paths_end())
{
Path *p = a->paths_next();
EntitiesTreeItem *ep = new EntitiesTreeItem(p, e);
e->appendChild(ep);
p->movements_begin();
while(!p->movements_end())
{
Movement *m = p->movements_next();
EntitiesTreeItem *em = new EntitiesTreeItem(m, ep);
ep->appendChild(em);
EntitiesTreeItem *ed1 = new EntitiesTreeItem(m->getDet1(), em);
em->appendChild(ed1);
EntitiesTreeItem *ed2 = new EntitiesTreeItem(m->getDet2() ,em);
em->appendChild(ed2);
}
}
}
} // if(detailed)
for(std::map<int,Behavior*>::const_iterator it = behaviors.begin();
it != behaviors.end(); it++)
{
Behavior *b = (*it).second;
EntitiesTreeItem *e = new EntitiesTreeItem(b, rootEntitiesTreeItem);
rootEntitiesTreeItem->appendChild(e);
b->agents_begin();
while(!b->agents_end())
{
Agent *a = b->agents_next();
EntitiesTreeItem *ea = new EntitiesTreeItem(a, e);
e->appendChild(ea);
a->paths_begin();
while(!a->paths_end())
{
Path *p = a->paths_next();
EntitiesTreeItem *ep = new EntitiesTreeItem(p, ea);
ea->appendChild(ep);
if(detailed)
{
p->movements_begin();
while(!p->movements_end())
{
Movement *m = p->movements_next();
EntitiesTreeItem *em = new EntitiesTreeItem(m, ep);
ep->appendChild(em);
EntitiesTreeItem *ed1 = new EntitiesTreeItem(m->getDet1(), em);
em->appendChild(ed1);
EntitiesTreeItem *ed2 = new EntitiesTreeItem(m->getDet2() ,em);
em->appendChild(ed2);
}
}
}
}
}
}
int verify_Checkmate_Stalemate() {
int i;
int Range=0;
int check;
Movement dummy;
/*If the moving coin is white.*/
if(currentMove.coinId>=16 && currentMove.coinId<32) {
/*Checking range for all black coins.*/
for(i=LBROOK;i<=BPAWN8;i++) {
Range=allCoins[i].getNRange();
/*If range of any of the coin is not 0 , then neither checkmate nor stalemate .*/
if(Range!=0 && allCoins[i].returnMode()!=0) {
return -1;
}
}
dummy.setInitPos(allCoins[BKING].returnCurPos());
dummy.coinId=BKING;
dummy.captureCoinId=BKING;
dummy.setFinalPos(allCoins[BKING].returnCurPos());
/*Verifying whether check or not.*/
check=allCoins[BKING].verifyCheck(dummy);
cout<<"verifyCheck returned :"<<check<<endl;
/*If Checkmate return 1 or return 0 for Stalemate.*/
if(check==0) {
return 1;
} else {
return 0;
}
}
/*If the moving coin is black.*/
if(currentMove.coinId>=0 && currentMove.coinId<=15) {
/*Checking range for all white coins.*/
for(i=WPAWN1;i<=RWROOK;i++) {
Range=allCoins[i].getNRange();
/*If range of any of the coin is not 0 , then neither checkmate nor stalemate .*/
if(Range!=0 && allCoins[i].returnMode()!=0) {
return -1;
}
}
dummy.setInitPos(allCoins[WKING].returnCurPos());
dummy.coinId=WKING;
dummy.captureCoinId=WKING;
dummy.setFinalPos(allCoins[WKING].returnCurPos());
/*Verifying whether check or not.*/
check=allCoins[WKING].verifyCheck(dummy);
cout<<"verifyCheck returned :"<<check<<endl;
/*If Checkmate return 1 or return 0 for Stalemate.*/
if(check==0) {
return 1;
} else {
return 0;
}
}
}
void scanUpdate() {
//Perform the scanning actions
switch(internalState){
//Move until hitting a color
case 0:
//scanning sea (state transition 2 in state diagram)
if (curPos == POS_SEA) {
move.slideRight(0.25);
//focused on bay, read color
if(rightCam.inZone()){
//put color read code here!
//--------------------------
//--------------------------
blockPos++;
if(blockPos > 5){
//we are done lets moveTo the next place
internalState = 2;
}
else{
internalState = 1;
}
}
}
//scanning rail (state transition 6)
else if (curPos == POS_RAIL) {
move.slideLeft(0.25);
//if we're focused on a bay, read color
if (leftCam.inZone()) {
//put color read code here!
//--------------------------
//--------------------------
blockPos++;
if(blockPos > 5){
//we are done lets moveTo the next place
internalState = 2;
}
else{
internalState = 1;
}
}
}
//scanning pickup (state transition 4)
else {
move.slideRight(0.25);
//TODO: change to "onBlock" or something?
if (rightCam.inZone()) {
//put color read code here!
//--------------------------
//--------------------------
blockPos++;
if(blockPos > 13){
//we are done lets moveTo the next place
internalState = 2;
}
else{
internalState = 1;
}
}
}
break;
//We already read this color, so just keep moving until white
case 1:
//move right
if (curPos == POS_PICK_SEA || curPos == POS_PICK_UP) {
move.slideRight(0.25);
//TODO: CREATE inbetweenZones
if(rightCam.inbetweenZones()){
internalState = 0:
}
}
//move left
else {
int main(int argc,char* argv[])
{
int portNb=19997;
int error;
vector <double> xyz;
int clientID = simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5);
cerr << clientID << endl;
//===================================================================
Motion_structure * estructura = new Motion_structure(argv[1], 0.001);
//===================================================================
xyz.push_back(0);
xyz.push_back(0);
xyz.push_back(0);
simxStartSimulation(clientID, simx_opmode_oneshot_wait);
if (clientID!=-1)
{
int handle[6];
error = simxGetObjectHandle(clientID, "redundantRob_joint1", &handle[0], simx_opmode_oneshot_wait);
if (error != 0) cerr << error << endl;
error = simxGetObjectHandle(clientID, "redundantRob_joint2", &handle[1], simx_opmode_oneshot_wait);
if (error != 0) cerr << error << endl;
error = simxGetObjectHandle(clientID, "redundantRob_joint3", &handle[2], simx_opmode_oneshot_wait);
if (error != 0) cerr << error << endl;
error = simxGetObjectHandle(clientID, "redundantRob_joint4", &handle[3], simx_opmode_oneshot_wait);
if (error != 0) cerr << error << endl;
error = simxGetObjectHandle(clientID, "redundantRob_joint5", &handle[4], simx_opmode_oneshot_wait);
if (error != 0) cerr << error << endl;
error = simxGetObjectHandle(clientID, "redundantRob_joint6", &handle[5], simx_opmode_oneshot_wait);
if (error != 0) cerr << error << endl;
int tiempo_entre_puntos;
double velocidad;
while (simxGetConnectionId(clientID)!=-1)
{
char quit;
cout << "Si desea mover el brazo aprete enter, si desea salir aprete q" << endl;
cin >> quit;
if(quit == 'q')break;
cout << "\nIngrese la variacion de la position Xq" << endl;
//fscanf(stdin, "%f %f %f %f", &xyz.at(0),&xyz.at(1),&xyz.at(2), &velocidad);
cin >> xyz.at(0);
cout << "\nusted ingreso " << xyz.at(0) << endl;
cout << "\nIngrese la variacion de la position Y" << endl;
cin >> xyz.at(1);
cout << "\nusted ingreso " << xyz.at(1) << endl;
cout << "\nIngrese la variacion de la position Z" << endl;
cin >> xyz.at(2);
cout << "\nusted ingreso " << xyz.at(2) << endl;
cout << "\ningrese la velocidad del movimiento " << endl;
cin >> velocidad;
//========================================================================
Movement movimiento;
movimiento = estructura->move_delta_xyz_position(xyz,velocidad);
tiempo_entre_puntos = movimiento.get_time_between_tw0_points_millisecons();
//=========================================================================
for (int i = 0; i < movimiento.get_length_position(); ++i)
{
//=========================================================================
vector <double> posicion = movimiento.get_from_list_motor_angle_position(i);
vector <double> velocidad = movimiento.get_from_list_motor_velocities(i);
//=========================================================================
simxPauseCommunication(clientID, 1);
for (int j = 0; j < (int)posicion.size(); ++j)
{
error = simxSetJointTargetPosition(clientID, handle[j], posicion.at(j), simx_opmode_oneshot);
error = simxSetJointTargetVelocity(clientID, handle[j], velocidad.at(j), simx_opmode_oneshot);
}
simxPauseCommunication(clientID, 0);
extApi_sleepMs( tiempo_entre_puntos );
}
}
}
DynamicActivity* XmlReader::processDynamicActivityNode(const Node *node) {
const Element *activityElement = castToElement(node);
DynamicActivity *dynamicActivity = new DynamicActivity(stoul(getAttribute(activityElement, "aid")));
dynamicActivity->name(getTextFromElement(activityElement, "name", false));
dynamicActivity->description(getTextFromElement(activityElement, "desc", false));
double minDuration = F64_MAX, maxDuration = F64_MIN;
const Element *movements = castToElement(activityElement->get_first_child("movements"));
string a1 = getAttribute(movements, "from_aid", false), a2 = getAttribute(movements, "to_aid", false);
int64_t fromActivity = (a1.empty() ? -1 : stol(a1)), toActivity = (a2.empty() ? -1 : stol(a2));
const NodeSet movementSet = movements->find("movement");
for (const Node *movementNode : movementSet) {
const Element *movementElement = castToElement(movementNode);
Movement *mv = new Movement(stoul(getAttribute(movementElement, "mid")));
mv->minDuration(stod(getTextFromElement(movementElement, "min-duration")));
mv->maxDuration(stod(getTextFromElement(movementElement, "max-duration")));
if (mv->minDuration() < TIME_ERR) {
string mid = to_string(mv->mid());
delete dynamicActivity;
delete mv;
throw InvalidDatasetFile(caller(), "Dynamic activity's movement "+mid+" must have positive duration!", movementNode->get_line());
}
// Workaround the incapability of ILP solvers to solve the problem with the fixed variables.
if (abs(mv->maxDuration()-mv->minDuration()) < TIME_ERR)
mv->maxDuration(mv->minDuration()+TIME_ERR);
const NodeSet monomials = movementElement->find("energy-function/monomial");
for (const Node *monomialNode : monomials) {
const Element *monomialElement = castToElement(monomialNode);
int32_t degree = stoi(getAttribute(monomialElement, "degree"));
double coeff = stod(getAttribute(monomialElement, "coeff"));
mv->addMonomial({degree, coeff});
}
uint32_t fromPoint = stoul(getTextFromElement(movementElement, "from-point"));
uint32_t toPoint = stoul(getTextFromElement(movementElement, "to-point"));
Location *locFrom = nullptr, *locTo = nullptr;
StaticActivity *actFrom = nullptr, *actTo = nullptr;
auto fSit = mPointToLocation.find(fromPoint), tSit = mPointToLocation.find(toPoint);
if (fSit == mPointToLocation.cend() || tSit == mPointToLocation.cend()) {
delete dynamicActivity;
delete mv;
throw InvalidDatasetFile(caller(), "Dynamic activity's point is not linked with any static activity!", movementNode->get_line());
} else {
locFrom = fSit->second;
locTo = tSit->second;
assert(locFrom != nullptr && locTo != nullptr && "Unexpected null pointers!");
actFrom = locFrom->parent();
actTo = locTo->parent();
assert(actFrom != nullptr && actTo != nullptr && "Should not be nullptr as it has been set in processRobotNode method!");
}
if ((actFrom->aid() != fromActivity && fromActivity != -1) || (actTo->aid() != toActivity && toActivity != -1)) {
delete dynamicActivity;
delete mv;
throw InvalidDatasetFile(caller(), "Optional attributes 'from_aid' and/or 'to_aid' are invalid!", movementNode->get_line());
}
minDuration = min(minDuration, mv->minDuration());
maxDuration = max(maxDuration, mv->maxDuration());
setValue(mMovementToPoints, mv, {fromPoint, toPoint}, caller());
dynamicActivity->addMovement(mv);
}
dynamicActivity->minAbsDuration(minDuration);
dynamicActivity->maxAbsDuration(maxDuration);
return dynamicActivity;
}
