Ejemplo n.º 1
0
///  setEndPose
void machineStatus::setEndPose( Point p) {
#ifdef MULTI_AXIS
	endPose = Pose( p, Point(0,0,0) );
#else
    endPose = Pose( p, Point(0,0,1) );
#endif
}
Ejemplo n.º 2
0
//Slot Unwrapper for internet datagram (vision)
void SimulatorCore::UnWrapVisionPacket(SSL_WrapperPacket iPacketTeam){
    SSL_DetectionRobot lPacketRobotBlue;
    SSL_DetectionRobot lPacketRobotYellow;
    SSL_DetectionBall lPacketBall = iPacketTeam.detection().balls(0);
    int lBlueSize = iPacketTeam.detection().robots_blue_size();
    int lYellowSize = iPacketTeam.detection().robots_blue_size();
    mBall->setPosition(lPacketBall.x(),lPacketBall.y());

    for(int i = 0; i < lBlueSize; ++i){
        lPacketRobotBlue = iPacketTeam.detection().robots_blue(i);
        int RobotId = lPacketRobotBlue.robot_id();
        if(RobotId > mNbPlayerPerTeam - 1){
            break;  //TODO clean this with dynamic player allocation
        }
        RobotModel* lRobotBlue = mBlueTeam->GetRobot(RobotId);
        lRobotBlue->setPose(Pose(lPacketRobotBlue.x(),lPacketRobotBlue.y(),
                                 lPacketRobotBlue.orientation()));
        // TODO should be in another function
        lRobotBlue->UpdateSimulationData();
    }

    for(int i = 0; i < lYellowSize; ++i){
        lPacketRobotYellow = iPacketTeam.detection().robots_yellow(i);
        int RobotId = lPacketRobotYellow.robot_id();
        if(RobotId > mNbPlayerPerTeam - 1){
            break;  //TODO clean this with dynamic player allocation
        }
        RobotModel* lRobotYellow = mYellowTeam->GetRobot(RobotId);
        lRobotYellow->setPose(Pose(lPacketRobotYellow.x(),lPacketRobotYellow.y(),
                                 lPacketRobotYellow.orientation()));
    }

    this->sendCommandToGrSim(); //TODO not clear, send back the new command to grsim
}
Ejemplo n.º 3
0
void Sprite::move(double xshift, double yshift, bool repaintP){
  Position oldorigin = pose.getOrigin();
  pose = Pose(Pose(xshift,yshift),pose);  // move in parent's frame

  if(penIsDown){
    Line l(oldorigin.x, oldorigin.y, 
	 oldorigin.x+xshift, oldorigin.y+yshift);
    //l.setColor(COLOR("green"));
    l.imprint(false);
  }
  if(repaintP) repaint();
}
Ejemplo n.º 4
0
void MotionControl::drawMap(SDL_Surface* screen, int sx, int sy) const {
#if 0
	//pixelRGBA(screen, sx, sy, 255, 255, 255, 255);
	lineRGBA(screen, sx, sy, sx, sy-100, 255, 255, 0, 255);
	lineRGBA(screen, sx, sy, sx+100, sy, 255, 255, 0, 255);
	float pxl_per_m = 100;
	filledCircleRGBA(screen, sx + lastPose.x * pxl_per_m, sy - lastPose.y * pxl_per_m, 5, 0, 255, 255, 255);
	lineRGBA(screen, sx + lastPose.x * pxl_per_m, sy - lastPose.y * pxl_per_m, sx + (lastPose.x + cos(lastPose.theta)) * pxl_per_m, sy - (lastPose.y + sin(lastPose.theta)) * pxl_per_m, 255, 0, 0, 255);
	PoseList poles{
		Pose(3, 3.1, 0),
		Pose(3, 2.3, 0),

		Pose(3.2, 1.2, 0),
		Pose(4, 1.2, 0),

		Pose(2.2, 1.9, 0),
		Pose(2.2, 1.1, 0),

		Pose(1, 0.3, 0),
		Pose(1, 1.1, 0),

		Pose(1.5, 2.8, 0),
		Pose(1.5, 2, 0)

	};
	for (auto it = poles.begin(); it != poles.end(); ++it) {
		filledCircleRGBA(screen, sx + it->x * pxl_per_m, sy - it->y * pxl_per_m, 5, 255, 0, 0, 255);
	}
	for (auto it = waypoints.begin(); it != waypoints.end(); ++it) {
		filledCircleRGBA(screen, sx + it->x * pxl_per_m, sy - it->y * pxl_per_m, 5, 0, 0, 255, 255);
	}
	auto last = midpoints.begin();
	//cout << endl << "Starting le draw" << endl;
	if (last != midpoints.end()) {
		int i=1;
		auto next = last;
		lineRGBA(screen, sx + lastPose.x * pxl_per_m, sy - lastPose.y * pxl_per_m, sx + last->x * pxl_per_m, sy - last->y * pxl_per_m, 255, 255, 0, 255);
		for (++next; ; ++next) {
			float arrowx = cos(last->theta), arrowy = sin(last->theta);
			lineRGBA(screen, sx + last->x * pxl_per_m, sy - last->y * pxl_per_m, sx + (last->x+arrowx) * pxl_per_m, sy - (last->y+arrowy) * pxl_per_m, 255-255*i/midpoints.size(), 255*i/midpoints.size(), 0, 255);

			if (next == midpoints.end())
				break;

			lineRGBA(screen, sx + last->x * pxl_per_m, sy - last->y * pxl_per_m, sx + next->x * pxl_per_m, sy - next->y * pxl_per_m, 255, 255, 255, 255);
			last = next;
			i++;
		}

	}
#endif
}
Ejemplo n.º 5
0
/// reset machineStatus to reasonable defaults
void machineStatus::clearAll() {
    F=S=0.0;
    plane = CANON_PLANE_XY;
    origin = Pose( Point(0,0,0), Point(0,0,0));
    coolant.flood = false;
    coolant.mist = false;
    coolant.spindle = false;
    endPose = startPose = Pose( Point(0,0,0), Point(0,0,1));
    endDir = prevEndDir = Point(0,0,-1);
    spindleStat = OFF;
    myTool = -1;
    motionType = NOT_DEFINED;
    lastMotionWasTraverse = false;
}
Ejemplo n.º 6
0
  bool Simulator::
  Initialize()
  {
    for (size_t ic(0); ic < RobotClient::registry.size(); ++ic) {
      RobotClient *client(RobotClient::registry.at(ic));
      RobotServer *server(new RobotServer(client, *m_world));
      if ( !client->Initialize(*server)) {
	delete server;
	delete client;
	return false;
      }
      m_world->AddRobot(server);
      m_robot.push_back(robot_s(server, client));
      Frame const pose(client->m_initial_pose.x, client->m_initial_pose.y, client->m_initial_pose.theta);
      server->InitializePose(pose);
      client->SetPose(Pose(client->m_initial_pose.x, client->m_initial_pose.y, client->m_initial_pose.theta));
      if (client->m_goals.size() > 0) {
	client->SetGoal(m_timestep, client->m_goals[0]);
      }
      
      // This was a nice feature that might be worth resurrecting using fpplib
      // if ( ! layout_file.empty())
      // 	m_appwin.push_back(shared_ptr<AppWindow>(new AppWindow(rdesc[ii]->name, layout_file,
      // 							       640, 480, this)));
    }
    
    UpdateAllSensors();
    
    return true;
  }
Ejemplo n.º 7
0
void CameraModel::insertBallWithNoise(SSL_WrapperPacket *iPacket,
                                      BallData *iBall){
    Pose lBallPose = Pose(iBall->getPosition().x,iBall->getPosition().y,0);

    lBallPose = this->addNoise(lBallPose);
    addBallToVisionPacket(iPacket,lBallPose);
}
Ejemplo n.º 8
0
void Joint::rotate(){
	//not sure if this is how it works...
	//local_pose.orientation = local_pose.orientation + rotate;
	//PoseEuler temp = PoseEuler(vertex3(), vertex3(0.0, 0.0, angle));
//	local_transformation = local_transformation * ( temp.getRotation()* angularVelocity);
	
	Pose currentPose = path->update();

	float x, y;
	x = currentPose.position.getx();
	y = currentPose.position.gety();
	angle = -PI/2.0 + (float) (std::atan2(y, x) - std::atan2(1.0, 0.0));


	//REALLY MEANS: angle = angularVelocity * dt + angle0;
	//angle += angularVelocity;
	//std::cout << "x:" << x << ", y:" << y << ", theta:" << angle << std::endl;
	Pose temp = Pose(vertex3(), quaternion(angle, 0.0, 0.0));
	//Pose temp = Pose(vertex3(), quaternion(0.0, angle, 0.0));
	local_transformation = temp.getRotation();

	//local_transformation = ( temp.getRotation() * angularVelocity);

	//if(!isNull()){
	//	for(unsigned int i=0; i<children.size(); i++){
	//		this->children.at(i)->child->rotate(rotate);
	//	}
	//}
}
Ejemplo n.º 9
0
Robot::Robot(){
	id = 0;
	is_our_team = true;
	actPose = setPose = finalPose = Pose(0, 0, 0);
	command = Command(0, 0, 0);
	path_is_valid_yet = false;
}
Ejemplo n.º 10
0
	//---------------------------------------------------------------------
	Pose* Pose::clone(void) const
	{
		Pose* newPose = OGRE_NEW Pose(mTarget, mName);
		newPose->mVertexOffsetMap = mVertexOffsetMap;
		// Allow buffer to recreate itself, contents may change anyway
		return newPose;
	}
Ejemplo n.º 11
0
    ExperimentalApp() : GLFWApp(1280, 800, "Glass Material App")
    {
        igm.reset(new gui::ImGuiManager(window));
        gui::make_dark_theme();
        
        //cubeTex = load_cubemap();

        int width, height;
        glfwGetWindowSize(window, &width, &height);
        glViewport(0, 0, width, height);

         // Debugging views for offscreen surfaces
        uiSurface.bounds = {0, 0, (float) width, (float) height};
        uiSurface.add_child( {{0.0000f, +10},{0, +10},{0.1667f, -10},{0.133f, +10}});
        uiSurface.add_child( {{0.1667f, +10},{0, +10},{0.3334f, -10},{0.133f, +10}});
        uiSurface.layout();

        grid = RenderableGrid(1, 100, 100);
        cameraController.set_camera(&camera);
        camera.look_at({0, 2.5, -2.5}, {0, 2.0, 0});

		lights[0] = {{0, 10, -10}, {0, 0, 1}};
		lights[1] = {{0, 10, 10}, {0, 1, 0}};

        Renderable reflectiveSphere = Renderable(make_sphere(2.f));
        reflectiveSphere.pose = Pose(float4(0, 0, 0, 1), float3(0, 2, 0));
        glassModels.push_back(std::move(reflectiveSphere));

        Renderable debugAxis = Renderable(make_axis(), false, GL_LINES);
        debugAxis.pose = Pose(float4(0, 0, 0, 1), float3(0, 1, 0));
        regularModels.push_back(std::move(debugAxis));

        Renderable cubeA = Renderable(make_cube());
        cubeA.pose = Pose(make_rotation_quat_axis_angle({1, 0, 1}, ANVIL_PI / 3), float3(5, 0, 0));
        regularModels.push_back(std::move(cubeA));

        Renderable cubeB = Renderable(make_cube());
		cubeB.pose = Pose(make_rotation_quat_axis_angle({1, 0, 1}, ANVIL_PI / 4), float3(-5, 0, 0));
        regularModels.push_back(std::move(cubeB));

        glassMaterialShader = make_watched_shader(shaderMonitor, "assets/shaders/glass_vert.glsl", "assets/shaders/glass_frag.glsl");
        simpleShader = make_watched_shader(shaderMonitor, "assets/shaders/simple_vert.glsl", "assets/shaders/simple_frag.glsl");

        cubeCamera.reset(new CubemapCamera({1024, 1024}));

        gl_check_error(__FILE__, __LINE__);
    }
Ejemplo n.º 12
0
 Pose InputDevice::getPose(int channel) const {
     auto pose = _poseStateMap.find(channel);
     if (pose != _poseStateMap.end()) {
         return (*pose).second;
     } else {
         return Pose();
     }
 }
Ejemplo n.º 13
0
void Joint::translate(const vertex3& movement){
	//how much to move, or where to move to?
	//simply add how much it moves to it's pose

	Pose temp = Pose(movement, vertex3());
	local_transformation = local_transformation * temp.translate_object();
	//local_pose.position = local_pose.position + movement;
}
Ejemplo n.º 14
0
Pose Perception::getGlobalPose() const
{
    if(master)
        return Device::getGlobalPose();
    else if(unit)
        return unit->getPose();
    return Pose(0,0);
}
Ejemplo n.º 15
0
    void Map::loadPositions(std::istream &input) {
        positions.clear();

        std::string lineBuffer;
        while (std::getline(input, lineBuffer)) {
            positions.push_back(Pose(lineBuffer));
        }
    }
Ejemplo n.º 16
0
//--------------------------------------------------------------
void ofApp::setup(){

    sender.setup("127.0.0.1", 8888);

    kinect.open();
    kinect.initBodySource();
    // kinect.initColorSource();
    kinect.initDepthSource();
    // kinect.initBodyIndexSource();

    kinect.getSensor()->get_CoordinateMapper(&m_pCoordinateMapper);

    camera.setDistance(10);

    ofSetWindowShape(1920, 1080);

    CalcParams elbowR       = { ShoulderRight, ElbowRight, WristRight };
    CalcParams elbowL       = { ShoulderLeft, ElbowLeft, WristLeft };
    CalcParams zShoulderR   = { HipRight, ShoulderRight, ElbowRight };
    CalcParams zShoulderL   = { HipLeft, ShoulderLeft, ElbowLeft };

    jointCalcParams.insert( make_pair("elbowRight", elbowR) );
    jointCalcParams.insert( make_pair("elbowLeft", elbowL) );
    jointCalcParams.insert( make_pair("zShoulderRight", zShoulderR) );
    jointCalcParams.insert( make_pair("zShoulderLeft", zShoulderL) );

    // pose(ER, ER, zSR, zSL)
    poses.insert( make_pair(2, Pose(180., 180., 100., 100.)) );
    poses.insert( make_pair(1, Pose(180., 180., 15., 100.)) ); //f
    poses.insert( make_pair(3, Pose(180., 165., 15., 180.)) ); // d
    poses.insert( make_pair(4, Pose(180., 180., 15., 140.)) ); // e
    poses.insert( make_pair(5, Pose(180., 180., 100., 15.)) ); // b
    poses.insert( make_pair(6, Pose(180., 180., 140., 15.)) ); // c
    poses.insert( make_pair(7, Pose(165., 180., 180., 100.)) ); // j
    poses.insert( make_pair(8, Pose(180., 180., 60., 140.)) ); // l
    poses.insert( make_pair(9, Pose(180., 165., 100., 180.)) );// p
    poses.insert( make_pair(10, Pose(180., 180., 140., 140.)) );

    showGUI = false;
    gui.setup("Parameters");
    gui.add(maxDistance.set("Max Distance", 5., 0., 10.));
    gui.add(tol.set("Pose tolerance", 15., 0., 50.));

}
Ejemplo n.º 17
0
void Line::reset(double x1, double y1, double x2, double y2){
  pose = Pose((x1+x2)/2,(y1+y2)/2);
	      
  double dx = (x1-x2)/2;
  double dy = (y1-y2)/2;
  vertex[0] = Position(dx,dy);
  vertex[1] = Position(-dx,-dy);
  thickness = 2;
  show();
}
Ejemplo n.º 18
0
Pose WorldDesc::getObjPos(std::string name)
{
	std::vector< std::pair< Pose, std::string > >::iterator ii=this->objectsPositions.begin();
	for(;ii!=this->objectsPositions.end();ii++){
		if((*ii).second==name){
			return ii->first;
		}
	}
	return Pose(0,0,0);
}
Ejemplo n.º 19
0
bool EvaluationModule::checkAngleToPass(Vector2D targetPosition, Pose currRobotPosition, double & angleToTarget) const{

	angleToTarget = calculateAngleToTarget( currRobotPosition, Pose(targetPosition.x,targetPosition.y,0) );
	double threshold = 0.017 * currRobotPosition.getPosition().distance(targetPosition);
	//LOG_INFO(this->log,"set threshold to "<<threshold);

	if( fabs( angleToTarget ) < threshold ){//1stopien
		return true;
	}
	return false;
}
Ejemplo n.º 20
0
void TreeOptimizer2::computePreconditioner(){
  gamma[0] = gamma[1] =  gamma[2] = numeric_limits<double>::max();

  for (uint i=0; i<M.size(); i++)
    M[i]=Pose(0.,0.,0.);

  int edgeCount=0;
  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
    edgeCount++;
    if (! (edgeCount%10000))
      DEBUG(1) << "m";

    Edge* e=*it;
    Transformation t=e->transformation;
    InformationMatrix S=e->informationMatrix;
    
    InformationMatrix R;
    R.values[0][0]=t.rotationMatrix[0][0];
    R.values[0][1]=t.rotationMatrix[0][1];
    R.values[0][2]=0;
    
    R.values[1][0]=t.rotationMatrix[1][0];
    R.values[1][1]=t.rotationMatrix[1][1];
    R.values[1][2]=0;
    
    R.values[2][0]=0;
    R.values[2][1]=0;
    R.values[2][2]=1;

    InformationMatrix W =R*S*R.transpose();
    
    Vertex* top=e->top;
    for (int dir=0; dir<2; dir++){
      Vertex* n = (dir==0)? e->v1 : e->v2;
      while (n!=top){
	uint i=n->id;
	M[i].values[0]+=W.values[0][0];
	M[i].values[1]+=W.values[1][1];
	M[i].values[2]+=W.values[2][2];
	gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
	gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
	gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
	n=n->parent;
      }
    }
  }
  
  if (verboseLevel>1){
    for (uint i=0; i<M.size(); i++){
      cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
    }
  }
}
Ejemplo n.º 21
0
Task::status Rotate::run(void * arg, int steps){

	GameStatePtr currGameState( new GameState );
	double lastSimTime=0;
	double currSimTime=0;
	currSimTime=video.updateGameState(currGameState);
	//biezaca pozycja robota
	Pose currRobotPose = (*currGameState).getRobotPos( robot->getRobotID() );
	if(this->targetRobotId != Robot::unknown)
		targetPosition = (*currGameState).getRobotPos(this->targetRobotId).getPosition();

	while( !this->stopTask && (steps--)!=0 ){

		if( lastSimTime < ( currSimTime=video.updateGameState(currGameState) ) ){
			lastSimTime = currSimTime;

			currRobotPose=currGameState->getRobotPos( robot->getRobotID() );
			bool haveBall = false;
			double angleToTarget = 0;
			double threshold = 0.017;

			if(this->predicates & Task::pass){
				bool canPass=this->evaluationModule.checkAngleToPass(targetPosition, currRobotPose, angleToTarget);
				threshold = 0.017 * currRobotPose.getPosition().distance(this->targetPosition);
				LOG_INFO(this->log,"set threshold to "<<threshold);

				if( canPass ){
					this->robot->stop();
					LOG_INFO(log,"Rotation to pass OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget);
					return Task::ok;
				}
			}
			else{
				angleToTarget = calculateAngleToTarget( currRobotPose, Pose(targetPosition.x,targetPosition.y,0) );
				double threshold = 0.017; //1 stopien
				if( fabs( angleToTarget ) < threshold ){
					this->robot->stop();
					LOG_INFO(log,"Rotation OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget);
					return Task::ok;
				}
			}
			double w = robot->calculateAngularVel( currRobotPose, targetPosition, currGameState->getSimTime(), haveBall );
			LOG_INFO(log,"move robot from"<<currRobotPose<<" to "<<targetPosition<<" setVel w "<<w<<" angle to target "<<angleToTarget);
			robot->setRelativeSpeed( Vector2D(0,0), w );
		}
		usleep(100);
	}

	if(this->stopTask)
		return Task::ok;

	return Task::not_completed;
}
Ejemplo n.º 22
0
Pose Goodrich::calcResult(int id, Pose goal, bool is_last){
	this->id = id;
	this->goal = goal;
	this->is_last = is_last;
	result = Pose(0, 0, 0);
	
	attractiveForce();
	repulsiveForceRobotObjects();
	repulsiveForceRobotRobot();

	return result;
}
Ejemplo n.º 23
0
void Line::paint(Pose *p){
  if(visible){
    XPoint xpt[2];
    if(p){
      Pose r = Pose(*p,pose);
      r.compute_res_vertex(xpt, vertex, 2);
    }
    else pose.compute_res_vertex(xpt, vertex, 2);

    drawLine(xpt[0], xpt[1], color, (int) thickness);    
  } 
}
Ejemplo n.º 24
0
PID::PID(){
	proportionalL = 0.40; //p1 = 0.0004
	derivativeL = 0.0;
	integrativeL = 0.0;

	proportionalA = 0.80; //p2 = 0.8000
	derivativeA = 0.0;
	integrativeA = 0.0;

	command = Command(0, 0, 0);
	actPose = setPose = Pose(0, 0, 0);
}
Ejemplo n.º 25
0
void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
  edgeCompareMode=mode;
  // compute the size of the preconditioning matrix
  clear();
  Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
  root=v0;
  v0->parameters=Transformation(v0->pose);
  v0->parentEdge=0;
  v0->parent=0;
  v0->level=0;
  v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
}
Ejemplo n.º 26
0
// warning mixed line endings in pose files will screw you up
// windows seems to handle ok but in linux, the  mixed line endings
// will have the affect of appearing to take angles from subsequent
// poses and mashing them into one  pose ... eg multiple seperate lines
// are condensed into one with the most recent angles overwriting earlier angles
void Kitten::readPoseFile(char const *path) {
    
    // Open the file.
    std::ifstream fh;
    fh.open(path);    
    std::vector<double> angles;
    
    while (fh.good()) {
        
        double x, y, z;
        
        angles.clear();
	
        // Read a line from the file.
        std::string line;
        getline(fh, line);
        
        // Skip empty lines, or comments.
        if (!line.size() || line[0] == '#') {
            continue;
        }
        
        // Push the line into a stringstream so we can easily pull out doubles.
        std::stringstream ss;
        ss << line;        
        
        // Read translation coordinates.
        ss >> x;
        ss >> y;
        ss >> z;
        
        // Read angles while there is still stuff to read.
        while (ss.good()) {
            double a;
            ss >> a;
            angles.push_back(a);
        }

	int missing  = NUM_ANGLES- angles.size();
	for (int i = 0; i< missing;i++){
		angles.push_back(0);
	}
	for (int i =0;i<angles.size();i++){
		if (angles[i]!=0){
			std::cout << "pose: " << poses_.size() << " angleno: " << i 
			  << " " << anglearray[i]  <<" angle: " << angles[i] << std::endl;
		}
	}
        
        poses_.push_back(Pose(x, y, z, angles));
    }

}
Ejemplo n.º 27
0
Pose Robot::getPose() const {
    
    double x=0, y=0, z=0;
    std::vector<double> angles(NUM_ANGLES);
    
    // If we don't have any poses then return an empty pose.
    if (!poses_.size()) {
        return Pose(x, y, z, angles);
    }
    
    // If we only have one pose, then return it.
    if (poses_.size() == 1) {
        return poses_[0];
    }

    // Return the current pose.
    // NOTE: poseIndex is double, NOT an int!!! Fractional values represent
    // interpolation between neighbouring poses.  Eg: 2.5 would be halfway 
    // between the 3rd and 4th pose.
    int wholeIndex = int(floor(poseIndex_));
    double indexDiff = poseIndex_ - wholeIndex;
    Pose tempPose(poses_[wholeIndex].x_, 
                  poses_[wholeIndex].y_,
                  poses_[wholeIndex].z_,
                  angles);
    double angleDiff = 0;
    int nextIndex = 0;
    if (poseIndex_ < poses_.size() -1)
    {
        nextIndex = int(floor(poseIndex_)) + 1;
    }
    //Translation diff
    double diff = (poses_[nextIndex].x_ - poses_[int(floor(poseIndex_))].x_) * indexDiff;
    tempPose.x_ = tempPose.x_ + diff;
    diff = (poses_[nextIndex].y_ - poses_[int(floor(poseIndex_))].y_ ) * indexDiff;
    tempPose.y_ = tempPose.y_ + diff;
    diff = (poses_[nextIndex].z_ - poses_[int(floor(poseIndex_))].z_ ) * indexDiff;
    tempPose.z_ = tempPose.z_ + diff;

    //Angles diff
    for (int i = NUM_ANGLES; i >= 0; i--) 
    {
       angleDiff = (poses_[nextIndex].angles_[i] - poses_[int(floor(poseIndex_))].angles_[i]) * indexDiff;
       tempPose.angles_[i] = poses_[int(floor(poseIndex_))].angles_[i] + angleDiff;
    }

    return tempPose;

    // @@@@@ Replace the above return statement with your own code to implement
    // @@@@@ linear interpolation between poses.

}
Drone::Drone(int txPin, int rxPin) {
  _serialIO = SerialIO(txPin, rxPin);
  _callback = Callback();
  _incomingPacketReader = IncomingPacketReader(&_serialIO);
  _vitals = Vitals(&_serialIO, &_incomingPacketReader, &_callback);
  _responseHandler = ResponseHandler(&_serialIO, &_incomingPacketReader, &_callback, &_vitals);
  _rc = RC(&_serialIO, &_callback, &_incomingPacketReader);
  _gpio = GPIO(&_serialIO, &_callback, &_incomingPacketReader);
  _i2c = I2C(&_serialIO, &_callback, &_incomingPacketReader);
  _pose = Pose(&_serialIO, &_callback, &_incomingPacketReader);
  _autopilot = Autopilot(&_serialIO, &_callback, &_incomingPacketReader);
  _transmitterSupport = TransmitterSupport(&_serialIO, &_callback, &_incomingPacketReader);
}
Ejemplo n.º 29
0
void Turret::update(float dt)
{
	effects.setPose(master->getMountPose(mount)*Pose(vec2f(0,0),getAngle()*3.1415/180.0));
	effects.update(dt);

	angle+=(_direction*velocity*dt);
	if(angle<-180)
		angle+=360;
	if(angle> 180)
		angle-=360;
	if(angle>dimensions)
		angle=dimensions;
	if(angle<-dimensions)
		angle=-dimensions;
}
Ejemplo n.º 30
0
	Pose interpolate(quint64 time)
	{
		const quint64 sz=mPoses.size();
		if(sz<=0) {
			return Pose(0);
		}
		const quint64 b=before(time);
		const quint64 a=after(time);
		if(a==b) {
			return mPoses[a];
		}
		const qreal alpha=(qreal)(time-b)/(qreal)(a-b);
		Pose out=mPoses[b];
		out.mix(mPoses[a], alpha);
	}