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