Example #1
0
void ExtendoHand::getHeading(Vector3D &m) {
    if (nineAxis) {
#ifdef ENABLE_MAGNETOMETER
        int16_t mx, my, mz;
        magnet.getHeading(&mx, &my, &mz);
        m.set(mx, my, mz);
#else
        m.set(0, 0, 0);
#endif // ENABLE_MAGNETOMETER
    } else {
        m.set(0, 0, 0);
    }
}
Example #2
0
void ExtendoHand::getRotation(Vector3D &g) {
    if (nineAxis) {
#ifdef ENABLE_GYRO
        int16_t gx, gy, gz;
        gyro.getRotation(&gx, &gy, &gz);
        g.set(gx, gy, gz);
#else
        g.set(0, 0, 0);
#endif // ENABLE_GYRO
    } else {
        g.set(0, 0, 0);
    }
}
Example #3
0
void ExtendoHand::getAcceleration(Vector3D &a) {
    if (nineAxis) {
        int16_t ax, ay, az;
        accel.getAcceleration(&ax, &ay, &az);

        // approximate g values, per calibration with a specific sensor
        a.set(
            ax / 230.0 - 0.05,
            ay / 230.0,
            az / 230.0);
    } else {
#ifdef THREE_AXIS
        a.set(
            motionSensor.accelX(),
            motionSensor.accelY(),
            motionSensor.accelZ());
#endif // THREE_AXIS
    }
}
Example #4
0
void PointDomain::closest_point( Mesh::VertexHandle ,
                                 const Vector3D& ,
                                 Vector3D& closest,
                                 Vector3D& normal,
                                 MsqError& err ) const
{
  closest = geom();
  normal.set(0,0,0);
  MSQ_SETERR(err)( "Cannot get normal for PointDomain", MsqError::INTERNAL_ERROR );
}
Example #5
0
/**
 * Creates a new ODE_HingeJoint.
 *
 * @param body1 the first body to connect the joint to.
 * @param body2 the second body to connect the joint to.
 * @return the new ODE_HingeJoint.
 */
dJointID ODE_HingeJoint::createJoint(dBodyID body1, dBodyID body2) {
	if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get())) {
		Core::log("Invalid axes for ODE_HingeJoint.");
		return 0;
	}
	//if one of the bodyIDs is null, the joint is connected to a static object.
	dJointID newJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
	dJointAttach(newJoint, body1, body2);
	
	Vector3D anchor = mJointAxisPoint1->get() ;
	Vector3D axis = mJointAxisPoint2->get() ;
	
	axis.set(mJointAxisPoint2->getX() - mJointAxisPoint1->getX(), mJointAxisPoint2->getY() - mJointAxisPoint1->getY(), mJointAxisPoint2->getZ() - mJointAxisPoint1->getZ());
	
	dJointSetHingeAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
	dJointSetHingeAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
	return newJoint; 
}
Example #6
0
void PointDomain::element_normal_at( Mesh::ElementHandle ,
                                     Vector3D &coordinate) const
  { coordinate.set(0,0,0); }
Example #7
0
void PointDomain::vertex_normal_at( Mesh::VertexHandle ,
                                    Vector3D &coordinate) const
  { coordinate.set(0,0,0); }
void Robot01BlockCode::processLocalEvent(EventPtr pev) {
	stringstream info;
	MessagePtr message;

	switch (pev->eventType) {
    case EVENT_TRANSLATION_END:
		robotBlock->setColor(LIGHTBLUE);
		info.str("");
		info << robotBlock->blockId << " rec.: EVENT_TRANSLATION_END";
		scheduler->trace(info.str(),hostBlock->blockId);
		// prepare for next motion
		nbreOfWaitedAnswers=0;
		block2Answer=NULL;
		if (blockToUnlock!=0) {
			sendUnlockMessage(blockToUnlock);
		} else { // dernier élément du train
			if (trainNext==NULL) {
				info.str("");
				info << "rerun " ;//<< trainNextId << "," << trainPreviousId;
				scheduler->trace(info.str(),hostBlock->blockId);
				robotBlock->setColor(DARKORANGE);
				trainPrevious=NULL;
				PointRel3D pt;
				calcPossibleMotions(pt);
				sendReLinkTrainMessage();
			} else {
				info.str("");
				info << "ready " ;//<< trainNextId << "," << trainPreviousId;
				scheduler->trace(info.str(),hostBlock->blockId);
				robotBlock->setPrevNext(trainPrevious,trainNext);
				robotBlock->setColor(BLUE);
			}
		}
		break;

	case EVENT_NI_RECEIVE:
		message = (std::static_pointer_cast<NetworkInterfaceReceiveEvent>(pev))->message;
		P2PNetworkInterface * recvInterface = message->destinationInterface;
		switch(message->id) {
		case MAP_MSG_ID : {
			MapMessage_ptr recvMessage = std::static_pointer_cast<MapMessage>(message);

			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;

			// cout << "INTERFACE" << robotBlock->getDirection(message->destinationInterface) << endl;
					
			info.str("");
			info << "rec. MapMessage : MAP_MSG from " << sourceId;
			scheduler->trace(info.str(),hostBlock->blockId);

			if (targetGrid) {
				sendAckMap(recvInterface);
			} else {
				// first message
				memcpy(gridSize,recvMessage->gridSize,3*sizeof(short));
				targetGrid = initGrid(gridSize,recvMessage->targetGrid);

				block2Answer=recvInterface;
				nbreOfWaitedAnswers=0;
				sendMapToNeighbors(block2Answer);

				if (nbreOfWaitedAnswers==0) {
					sendAckMap(block2Answer);
					block2Answer=NULL;
					info.str("");
					info << " the end";
					scheduler->trace(info.str(),hostBlock->blockId,GOLD);
					currentTrainGain=0;

					PointRel3D pt;
					calcPossibleMotions(pt);
				}
			}
		}
			break;

		case ACKMAP_MSG_ID : {
			AckMapMessage_ptr recvMessage = std::static_pointer_cast<AckMapMessage>(message);
			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
			info.str("");
			info << "rec. AckMapMessage(" << nbreOfWaitedAnswers << ") from " << sourceId;
			scheduler->trace(info.str(),hostBlock->blockId,LIGHTBLUE);

			nbreOfWaitedAnswers--;
			if (nbreOfWaitedAnswers==0) {
				if (block2Answer!=NULL) {
					sendAckMap(block2Answer);
					block2Answer=NULL;
					info.str("");
					info << "waits for train message";
				} else {
// you are master block because NULL father
					info.str("");
					info << " next step";
				}
				scheduler->trace(info.str(),hostBlock->blockId,GOLD);
				currentTrainGain=0;

				PointRel3D pos;
				pos.x = robotBlock->position[0];
				pos.y = robotBlock->position[1];
				pos.z = robotBlock->position[2];
				goodPlace = targetGrid[(pos.z*gridSize[1]+pos.y)*gridSize[0]+pos.x]==fullCell;
				PointRel3D pt;
				calcPossibleMotions(pt);
			}
		}
			break;

		case TRAIN_MSG_ID : {
			TrainMessage_ptr recvMessage = std::static_pointer_cast<TrainMessage>(message);
			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
			info.str("");
			info << "rec. TrainMessage (" << recvMessage->newPos << "," << recvMessage->gain <<") from " << sourceId;
			currentTrainGain+=recvMessage->gain;
			currentTrainGoal= recvMessage->newPos;
			info << "\ncurrentGain = " << currentTrainGain;
			scheduler->trace(info.str(),hostBlock->blockId);

			trainPrevious = recvMessage->sourceInterface;
			robotBlock->setPrevNext(trainPrevious,trainNext);

			if (block2Answer==NULL) {
				calcPossibleMotions(recvMessage->newPos);
				sendLinkTrainMessages(recvMessage->destinationInterface);
			} else {
				stringstream info;
				info.str("");
				info << "block2answer!=NULL";
				scheduler->trace(info.str(),hostBlock->blockId,RED);
			}
		}
			break;

        case ACKTRAIN_MSG_ID : {
			AckTrainMessage_ptr recvMessage = std::static_pointer_cast<AckTrainMessage>(message);
			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
			info.str("");
			info << "rec. AckTrainMessage("<< recvMessage->answer << ") from " << sourceId;
			scheduler->trace(info.str(),hostBlock->blockId);
			// 2 situations à gérer :
			// - le voisin ne peut pas se déplacer
			// - le bloc est en tête
			if (recvMessage->answer && trainPrevious!=NULL) {
				AckTrainMessage *message = new AckTrainMessage(true);
				scheduler->schedule(new NetworkInterfaceEnqueueOutgoingEvent(scheduler->now() + COM_DELAY, message, trainPrevious->connectedInterface));
				info.str("");
				info << "send AckTrainMessage(true) to " << trainPrevious->connectedInterface->connectedInterface->hostBlock->blockId;
				scheduler->trace(info.str(),hostBlock->blockId,GREEN);
			} else {
				// gestion de l'échec
				if (!recvMessage->answer) {
					trainPrevious=NULL;

					if (possibleMotions && !possibleMotions->empty()) {
						// il y a eu échec sur la premiere règle
						// on essaye la suivante :
						Validation *firstCondition = possibleMotions->back();
						possibleMotions->pop_back();
						delete firstCondition;

						info.str("");
						info << possibleMotions->size();
						std::reverse_iterator<vector<Validation*>::iterator> rev_until (possibleMotions->begin());
						std::reverse_iterator<vector<Validation*>::iterator> rev_from (possibleMotions->end());
						while (rev_from!=rev_until) {
							info  << "/" << (*rev_from)->capa->isHead << ":" << (*rev_from)->capa->isEnd << " " << (*rev_from)->capa->name << " gain=" << (*rev_from)->gain;
							rev_from++;
						}
						scheduler->trace(info.str(),hostBlock->blockId,GOLD);

						/***********************************/
						/* IL FAUT TESTER SI C'EST UNE FIN */

						if (!possibleMotions->empty()) {
							sendLinkTrainMessages(trainNext);
						} /*else {
							sendLinkTrainMessages(trainNext,NULL);
							trainNext=NULL;
							robotBlock->setPrevNext(trainPrevious,trainNext);
							}*/
					} else {
//								sendLinkTrainMessages(block2Answer);
					}
				} else {
					info.str("");
					info << "Head of train :" << robotBlock->blockId << ", next=" << trainNext->hostBlock->blockId;
					//info << "\n" << robotBlock->blockId << " mv(" << motionVector.x << "," << motionVector.y << "," << motionVector.z << ")"
					//		 << " nmv(" << nextMotionVector.x << "," << nextMotionVector.y << "," << nextMotionVector.z << ")";
					scheduler->trace(info.str(),hostBlock->blockId,GREEN);
					block2Answer=NULL;
					sendAnswerDelayOrMotionDelayMessage(scheduler->now()-10*COM_DELAY);
				}
			}
        }
			break;

        case MOTIONDELAY_MSG_ID : {
			MotionDelayMessage_ptr recvMessage = std::static_pointer_cast<MotionDelayMessage>(message);
			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
			info.str("");
			info << robotBlock->blockId << " rec. MotionDelayMsg(" << recvMessage->unlockMode << ") from " << sourceId;
			scheduler->trace(info.str(),hostBlock->blockId,GREEN);
			info.str("");
			info << robotBlock->blockId << " mv(" << motionVector.x << "," << motionVector.y << "," << motionVector.z << ")"
				 << " nmv(" << nextMotionVector.x << "," << nextMotionVector.y << "," << nextMotionVector.z << ")";
			scheduler->trace(info.str(),hostBlock->blockId,GREEN);

			if (recvMessage->unlockMode) {
				trainPrevious=NULL;
				robotBlock->setPrevNext(trainPrevious,trainNext);
			}
			sendAnswerDelayOrMotionDelayMessage(recvMessage->globalTime);
		}
			break;
        case ANSWERDELAY_MSG_ID : {
			AnswerDelayMessage_ptr recvMessage = std::static_pointer_cast<AnswerDelayMessage>(message);
			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
			info.str("");
			info << robotBlock->blockId << " rec. AnswerDelayMsg(" << recvMessage->globalRDVTime << ") from " << sourceId;
			scheduler->trace(info.str(),hostBlock->blockId);
			Vector3D finalPosition;
			finalPosition.set(robotBlock->position.pt[0]+motionVector.x,
							  robotBlock->position.pt[1]+motionVector.y,robotBlock->position.pt[2]+motionVector.z);
			blockToUnlock=0;
			scheduler->schedule(new TranslationStartEvent(recvMessage->globalRDVTime,robotBlock,finalPosition));
			stringstream info;
			info.str("");
			info << robotBlock->blockId << " TranslationStartEvent(" << recvMessage->globalRDVTime << ")";
			scheduler->trace(info.str(),hostBlock->blockId,LIGHTGREY);
			if (trainPrevious) {
				AnswerDelayMessage *adm_message = new AnswerDelayMessage(recvMessage->globalRDVTime,false);
				scheduler->schedule(new NetworkInterfaceEnqueueOutgoingEvent(scheduler->now() + COM_DELAY, adm_message, trainPrevious->connectedInterface));
				stringstream info;
				info.str("");
				info << robotBlock->blockId << " send AnswerDelayMsg("<< adm_message->globalRDVTime << ") to " << trainPrevious->hostBlock->blockId;
				scheduler->trace(info.str(),hostBlock->blockId,GREEN);
			}
		}
			break;

        case UNLOCK_MSG_ID : {
			UnlockMessage_ptr recvMessage = std::static_pointer_cast<UnlockMessage>(message);
			unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
			info.str("");
			info << robotBlock->blockId << " rec. UnlockMessage(" << recvMessage->target << ") from " << sourceId;
			scheduler->trace(info.str(),hostBlock->blockId);

			// search if target is directly connected to the block
			int i=0;
			P2PNetworkInterface *p2p = robotBlock->getInterface(SCLattice::Direction(i));
			bool found=(p2p->connectedInterface && p2p->connectedInterface->hostBlock->blockId==recvMessage->target);
			//cout <<(p2p->connectedInterface?p2p->connectedInterface->hostBlock->blockId:-1) << endl;
			while (i<6 && !found) {
				i++;
				if (i<6) {
					p2p = robotBlock->getInterface(SCLattice::Direction(i));
					found=(p2p->connectedInterface && p2p->connectedInterface->hostBlock->blockId==recvMessage->target);
					//cout <<(p2p->connectedInterface?p2p->connectedInterface->hostBlock->blockId:-1) << endl;
				}
			}
			if (found) {
				Time time = scheduler->now();
				MotionDelayMessage *message = new MotionDelayMessage(time,true);
				scheduler->schedule(new NetworkInterfaceEnqueueOutgoingEvent(time + COM_DELAY, message, p2p));
				stringstream info;
				info.str("");
				info << robotBlock->blockId << " send MotionDelayMsg(unlock) to " << p2p->connectedInterface->hostBlock->blockId;
				scheduler->trace(info.str(),hostBlock->blockId,GREEN);
			}
        }
			break;
/**************************
				case RELINKTRAIN_MSG_ID : {
					ReLinkTrainMessage_ptr recvMessage = std::static_pointer_cast<ReLinkTrainMessage>(message);
					unsigned int sourceId = recvMessage->sourceInterface->hostBlock->blockId;
          info.str("");
					info << " rec. ReLinkTrainMessage() from " << sourceId;
					scheduler->trace(info.str(),hostBlock->blockId,GREEN);
					trainNext=message->destinationInterface;
					trainPrevious=NULL;
					currentTrainGain=0;
					PointRel3D pt;
					calcPossibleMotions(pt);
					sendReLinkTrainMessage();
				}
        break;*/

        default :
			cerr << "Block " << hostBlock->blockId << " received an unrecognized message from " << message->sourceInterface->hostBlock->blockId << endl;
			break;
		}
		break;
	}
	robotBlock->setColor((trainPrevious?(trainNext?MAGENTA:PINK):(trainNext?RED:goodPlace?GREEN:YELLOW)));
}