Example #1
0
void measureGravitation()
{
  double mean[3] = {0,0,0};
  int nbIter = 100;
  
  for(int k=0;k<nbIter;++k)
  {
    std::cerr << k << "/" << nbIter << "\r";
    // read data from IMU
    char c; // character
    std::string s("");
    ardu.get( c ) ;
    while(c != '#'){ardu.get( c );} // do nothing until we read the end of a block
    
    ardu.get( c );
    while(c != '#')
    {
      s += c;
      ardu.get( c );
    }
    splitData(s);
    
    mean[0] += data[4];
    mean[1] += data[5];
    mean[2] += data[6];
    
  }
  for(int j=0;j<3;++j)
    grav[j] = mean[j]/nbIter;
  
  std::cerr << std::endl;
}
Example #2
0
void DisplayCallback(void)
{
  std::cerr << "sidpp" << std::endl;
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(zoom, aspect, zNear, zFar);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glTranslatef(0.0,0.0,-sdepth);
  glRotatef(-stheta, 1.0, 0.0, 0.0);
  glRotatef(sphi, 0.0, 0.0, 1.0);
  
  // read data from IMU
  
  char c; // character
  std::string s("");
  ardu.get( c ) ;
  while(c != '#'){ardu.get( c );} // do nothing until we read the end of a block
  
  ardu.get( c );
  while(c != '#')
  {
    s += c;
    ardu.get( c );
  }

  
  
  // Convert string to std::vector<double>
//  std::vector<double> data = splitData(s);
  splitData(s);

// for(int k=0;k<data.size();++k)
// std::cerr << data[k] << std::endl;
  
  
  // Move the cube
  UpdatePosition();
  
  // Orient the cube
  DrawFromQuaternion(data);
  
  


  glutSwapBuffers();
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}
Example #3
0
// This function returns queued data on port, returns empty string if there is no data
// does not block
string read(SerialStream& serial_port)
{
    string result;
    while( serial_port.rdbuf()->in_avail() )
    {
        char next_byte;
        serial_port.get(next_byte);
        result += next_byte;
    }
    return result;
}
void Server::startRead()
{
      char buffer[1024] = {0};
      client->read(buffer, client->bytesAvailable());
      cout << buffer << endl;
      if(buffer[0] == 'l'){
           emit moving();
//           rot = rot + 5;
//           if(rot > 230)
//                   rot=0;
           serial_port <<"05"<<endl;
           //serial_port << ROT << "#" << rot << endl;
      }

      if(buffer[0] == 'r'){
          emit moving();
//          rot = rot - 5;
//                  if(rot < -30)
//                  rot = 210;
          serial_port <<"07"<<endl;
          //serial_port << ROT <<"#" << rot << endl;
      }

      if(buffer[0] == 'u'){
          emit moving();
//          tilt = tilt - 5;
          //TODO: overflow
          serial_port <<"04"<<endl;
          //serial_port << TILT <<"#" << tilt << endl;
      }

      if(buffer[0] == 'd'){
          emit moving();
//          tilt = tilt + 5;
          //TODO: Overflow
          serial_port <<"06"<<endl;
          //serial_port << TILT <<"#" << tilt << endl;
      }

      if(buffer[0] == 's'){
        std::string strBuffer(buffer);
        unsigned pos = strBuffer.find("#");
        std::string name = strBuffer.substr(pos+1);
        std::cout << "Name of surface: " << name << std::endl;
        //Send position request
        serial_port << "00" << endl;

        // Read Position response
        char temp=0;
        int i=0;
        char cstr[64];
        while(temp!='\n')
        {
            try
            {
                serial_port.get(temp);
               // temp=serial_port.ReadByte(100);
            }
            catch(SerialPort::ReadTimeout &e)
            {
                cout<<"Read Timeout"<<endl;
            }
            if((temp!='\n')&&(temp!=0)&&(temp!=' '))
            {
                cstr[i]=temp;
                ++i;
                //cout<<i++<<temp<<'x'<<endl;
            }
        }
        cstr[i]='\0';

        std::string dataBuffer(cstr);
        unsigned pos1 = dataBuffer.find("#");
        unsigned pos2 = dataBuffer.find("#",pos1+1);

        std::string rotString = dataBuffer.substr(pos1+1, pos2-pos1-1);
        std::string tiltString = dataBuffer.substr(pos2+1);

        std::cout <<"i got from " << dataBuffer << "\nrot: " << rotString << " and tilt: " << tiltString << std::endl;


        int rot =  QString::fromStdString(rotString).toFloat(NULL);
        int tilt =  QString::fromStdString(tiltString).toFloat(NULL);

        std::cout << "Rotation: " << rot << " Tilt: "<< tilt << std::endl;

        emit save(QString::fromStdString(name),rot,tilt);
      }

      if(buffer[0] == 'p'){
          //ToDo Distance
          char * lf ="\n" ;
          QStringList top = parser->surfaceNames();
          for ( QStringList::Iterator it = top.begin(); it != top.end(); ++it ) {
                 printf( "%s \n", (*it).toUtf8().constData() );
                 client->write((*it).toUtf8());
                 client->write(lf);
           }
          client->write(lf); // EoS
          client->flush(); //is, like always, important
      }

      if(buffer[0] == 'm'){
          //TODO: Distance
          std::string strBuffer(buffer);
          unsigned pos = strBuffer.find("#");
          std::string surface = strBuffer.substr(pos+1);
          std::cout << "set Projector to: " << surface << std::endl;
          int tilt,pan;
          parser->getTiltPan(QString::fromStdString(surface),&tilt,&pan);

          qDebug() << "set Projector to: "<< QString::fromStdString(surface) <<" Tilt: " << tilt << " Pan: " << pan;
          serial_port << ROT <<"#" << pan << endl;
          serial_port << TILT <<"#" << tilt << endl;
          emit moving();
      }

      if(buffer[0] == 'x'){
          std::string strBuffer(buffer);
          unsigned pos = strBuffer.find("#");
          std::string surface = strBuffer.substr(pos+1);
          std::cout << "delete " << surface << std::endl;
          parser->delteSurface(QString::fromStdString(surface));
      }

      if(buffer[0] == 'c'){
          std::cout << "recalibrate "<< std::endl;
          emit calibrate();
      }

      float distance =  getDistance(320,240);
      std::cout << "after emit distance : " << distance << std::endl;
      setFocus(distance);
}