void Serial_Close_Port(void)
{
if(serial_port.IsOpen())
{
serial_port.Close();
}
}
void Broadcast::Transmit()
{
SerialStream mySerial;
mySerial.Open("/dev/ttyUSB0");
mySerial.SetBaudRate(SerialStreamBuf::BAUD_9600);
mySerial << Data;
}
DWORD WINAPI ThreadFunc(LPVOID lpParam)
{
SerialStream stream;
if (!stream.open(4))
{
std::cout << "Couldn't open COM" << std::endl;
return -1;
}
string output;
while (true)
{
stream >> output;
int ind1, ind2, ind3;
stringstream ss;
ind1 = output.find("#");
ind2 = output.find(",", ind1 + 1);
ind3 = output.find("#", ind2+1);
float desired, actual;
ss << output.substr(ind1+1, ind2-ind1-1);
ss >> desired;
ss << output.substr(ind2 + 1, ind3 - ind2 - 1);
ss >> actual;
desiredData[desiredStart] = desired;
actualData[actualStart] = actual;
desiredStart = (desiredStart + 1) % 1000;
actualStart = (actualStart + 1) % 1000;
}
}
int main(int argc, char** argv)
{
/*The mySerialino must be setup to use the same baud rate*/
cout << "opening port" << endl;
mySerial.Open(PORT);
mySerial.SetBaudRate(SerialStreamBuf::BAUD_9600);
mySerial.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
cout << "port open" << endl;
do
{
cout << "Press a command key, use q to quit: ";
char val = cin.get();
cout << "Key pressed was " << val << endl;
cin.ignore();
if (val == 'q') break;
stringstream ss;
string cmd;
ss << val;
ss >> cmd;
cmd.append("#");
sendSerial(cmd);
if (val == 'b')
{
cout << "value received = " << getSerial() << endl;
}
} while(1);
}
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;
}
// 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;
}
int main(int argc, char** argv)
{
cout << "opening port" << endl;
mySerial.Open(PORT);
mySerial.SetBaudRate(SerialStreamBuf::BAUD_9600);
mySerial.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
cout << "port open" << endl;
ros::init(argc, argv, "serialCommands");
ros::NodeHandle nh_;
ros::Subscriber arduino_sub = nh_.subscribe("arduino_commands", 60, arduinoCallback);
arduino_pub = nh_.advertise<std_msgs::String>("arduino_sensors", 5);
ros::spin();
mySerial.Close();
}
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);
}
int getSerial(){
int res;
char str[16];
mySerial.read(str,2);
sscanf(str,"%d",&res);
return res;
}
void Send(uint8_t * out_buf) {
#ifndef NDEBUG
// Check Header
fprintf(stdout, "Message header:\n");
for (int i=0; i<HEADER_SIZE; i++) {
fprintf(stdout, " %02x",out_buf[i]);
}
fprintf(stdout, "\n\n");
#endif
int N = out_buf[N_INDEX];
int L = out_buf[L_INDEX]+1;
#ifndef NDEBUG
fprintf(stdout, "Message body:\n");
for (int i=0; i<N; i++) {
for (int j=0; j<L; j++) {
fprintf(stdout, " %02x",out_buf[HEADER_SIZE+i*L+j]);
}
fprintf(stdout, "\n");
}
fprintf(stdout, "END\n");
#endif
int message_size = HEADER_SIZE + N*L;
memcpy(output_buffer,out_buf,sizeof(uint8_t)*message_size);
ardu.write( output_buffer, message_size ) ;
}
// This function blocks for upto timeout usec and waits for data
// to be available on the port.
std::string readBlocking(SerialStream& serial_port, int timeout)
{
while( serial_port.rdbuf()->in_avail() == 0 && timeout > 0 )
{
timeout -= 100;
usleep(100);
}
if(timeout < 0)
return std::string();
return read(serial_port);
}
void Serial_Send_Message(string message)
{
char output_buffer[64];
int i,message_length;
message_length=message.length();
for(i=0;i<message_length;i++)
{
output_buffer[i]=message[i];
}
serial_port.write(output_buffer,message_length);
}
int main(int argc, char **argv)
{
// initialize global variables
prevtime = std::chrono::system_clock::now();
std::cerr << "koko1" << std::endl;
for(int k=0;k<7;++k)
data.push_back(0);
ardu.Open(PORT);
std::cerr << "koko2" << std::endl;
ardu.SetBaudRate(SerialStreamBuf::BAUD_38400);
ardu.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
std::cerr << "koko3" << std::endl;
glutInit(&argc, argv);
std::cerr << "koko4" << std::endl;
InitGL();
std::cerr << "koko5" << std::endl;
glutMainLoop();
return 0;
}
void Recieve(uint8_t* in_buf) {
// Read Header
ardu.read( input_buffer, HEADER_SIZE ) ;
for (int i=0; i<HEADER_SIZE; i++)
memcpy(&in_buf[i],&input_buffer[i],sizeof(uint8_t));
// in_buf[i] = input_buffer[i];
#ifndef NDEBUG
// Check Header
fprintf(stdout, "Message header:\n");
for (int i=0; i<HEADER_SIZE; i++) {
fprintf(stdout, " %02x",in_buf[i]);
}
fprintf(stdout, "\n\n");
#endif
int N = buf[N_INDEX];
int L = buf[L_INDEX]+1;
int nbytes_body = N * L;
ardu.read( input_buffer, nbytes_body ) ;
for (int i=0; i<nbytes_body; i++)
memcpy(&in_buf[i+HEADER_SIZE],&input_buffer[i],sizeof(uint8_t));
// in_buf[i+HEADER_SIZE] = input_buffer[i];
#ifndef NDEBUG
fprintf(stdout, "Message body:\n");
for (int i=0; i<N; i++) {
for (int j=0; j<L; j++) {
fprintf(stdout, " %02x",in_buf[HEADER_SIZE+i*L+j]);
}
fprintf(stdout, "\n");
}
fprintf(stdout, "END\n");
#endif
}
void Serial_Open_Port(string comPort)
{
serial_port.Open( comPort ) ;
if(! serial_port.good())
{
std::cerr<<"Error: could not open serial port"<<std::endl;
exit(1);
}
serial_port.SetBaudRate( SerialStreamBuf::BAUD_9600) ;
serial_port.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
serial_port.SetNumOfStopBits(1) ;
serial_port.SetParity( SerialStreamBuf::PARITY_EVEN ) ;
serial_port.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ) ;
}
void serialport_open(const char* sp) {
/*The arduino must be setup to use the same baud rate*/
fprintf(stdout, "Setting Params:\n");
ardu.SetBaudRate( SerialStreamBuf::BAUD_115200 ) ;
ardu.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
ardu.SetNumOfStopBits(1) ;
ardu.SetParity( SerialStreamBuf::PARITY_ODD ) ;
ardu.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_HARD ) ;
ardu.SetVTime(1);
fprintf(stdout, "Opening:\n");
ardu.Open( sp ) ;
fprintf(stdout, "Opened:\n");
}
void init_connection(SerialStream& stream)
{
stream.Open("/dev/ttyUSB0");
if (stream.IsOpen()){
cout << "\tStream open" << endl;
} else {
cout << "\tCould not open stream" << endl;
exit(1);
}
//Connection Characteristics
stream.SetBaudRate(SerialStreamBuf::BAUD_9600);
stream.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
stream.SetNumOfStopBits(1);
stream.SetParity(SerialStreamBuf::PARITY_NONE);
stream.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_HARD); //??
stream.SetVMin(1000);
}
int main(int argc,char** argv)
{
char response[3];
serial_stream.Open( argv[1] ) ;
serial_stream.SetBaudRate( SerialStreamBuf::BAUD_115200 ) ;
serial_stream.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
serial_stream.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ) ;
serial_stream.SetParity( SerialStreamBuf::PARITY_NONE ) ;
serial_stream.SetNumOfStopBits(0) ;
while (1)
{
serial_stream<<"ABCD";
cout<<"waiting response"<<endl;
serial_stream.read(response,3);
cout<<response<<endl;
}
return 0;
}
void open()
{
ardu.Open("/dev/ttyACM0");
ardu.SetBaudRate(SerialStreamBuf::BAUD_9600);
ardu.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
}
void write(SerialStream& serial_port, const std::string& data)
{
serial_port.write(data.c_str(), data.size());
}
SerialComm()
{
node.SetBaudRate(SerialStreamBuf::BAUD_9600);
node.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
node.Open(PORT);
}
void serial_setup( void )
{
ardu.Open(PORT);
ardu.SetBaudRate(BAUDRATE);
ardu.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
}
void read(char *in, size_t numbytes){
ardu.read(in,numbytes);
}
void open(){
/*The arduino must be setup to use the same baud rate*/
ardu.Open(PORT);
ardu.SetBaudRate(SerialStreamBuf::BAUD_9600);
}
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);
}
Server::Server(DomParser* _parser): QObject(0)
{
parser = _parser;
connect(&server, SIGNAL(newConnection()), this, SLOT(acceptConnection()));
serial_port.Open(ARDUINO) ;
if ( ! serial_port.good() )
{
std::cerr << "[" << __FILE__ << ":" << __LINE__ << "] "
<< "Error: Could not open serial port."
<< std::endl ;
exit(1) ;
}
//
// Set the baud rate of the serial port.
//
serial_port.SetBaudRate( SerialStreamBuf::BAUD_9600 ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not set the baud rate." <<
std::endl ;
exit(1) ;
}
//
// Set the number of data bits.
//
serial_port.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not set the character size." <<
std::endl ;
exit(1) ;
}
//
// Disable parity.
//
serial_port.SetParity( SerialStreamBuf::PARITY_NONE ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not disable the parity." <<
std::endl ;
exit(1) ;
}
//
// Set the number of stop bits.
//
serial_port.SetNumOfStopBits( 1 ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not set the number of stop bits."
<< std::endl ;
exit(1) ;
}
//
// Turn off hardware flow control.
//
serial_port.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE ) ;
if ( ! serial_port.good() )
{
std::cerr << "Error: Could not use hardware flow control."
<< std::endl ;
exit(1) ;
}
server.listen(QHostAddress::Any, 8888);
}
int main()
{
// Open the serial port for reading:
ardu.Open(PORT);
ardu.SetBaudRate(SerialStreamBuf::BAUD_115200);
ardu.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
char str[8];
// ardu << out;
ardu >> str;
std::cout << str;
//sscanf(str,"%d",&res);
//sleep(1);
//serial_send(fd, 'A');
sleep(1);
//while(result != 'A')
return 1;
while(true)
{
ardu >> str;
std::cout << str;
//int test = read(fd, &byte, 1);
//serial_send(fd, 'A');
//printf("%i %i %c\n", test, byte, byte);
//sleep(1);
}
// Set the last signalled state of the system as sent by the opto signal over USB.
// "t" is top-dead-center, and "b" is bottom-dead-center.
char system_state = 'B';
int image_counter = image_counter_min;
// Create the main rendering window
sf::RenderWindow App(sf::VideoMode(800, 600, 32), "SFML Graphics");
// Use v-sync (make it so that we sync with the monitor, and don't draw more than 60fps.
App.UseVerticalSync(false);
// App.SetFramerateLimit(60); // Limit to 60 frames per second
// App.SetFramerateLimit(0); // No limit
// Start game loop
while (App.IsOpened())
{
// Process events
sf::Event Event;
while (App.GetEvent(Event))
{
// Close window : exit
if (Event.Type == sf::Event::Closed)
App.Close();
// A key has been pressed
if (Event.Type == sf::Event::KeyPressed)
{
// Escape key : exit
if (Event.Key.Code == sf::Key::Escape)
App.Close();
}
}
// Read from the USB port.
uint8_t input_state; //= serial_recv(fd);
printf("%i ", input_state);
if (input_state != system_state && (input_state == 'B' || input_state == 'T'))
{ // Start the appropriate image sequence.
// Change the system state.
system_state = input_state;
//printf("%c\n", system_state);
// Reset the image counter.
image_counter = 0;
}
// Get the elapsed time.
float elapsed = Clock.GetElapsedTime();
if (elapsed > refresh_rate)
{
// Update the image.
printf("%i %f\n", image_counter, elapsed);
Clock.Reset();
// Increment or decrement the image counter.
if (system_state == 'B') image_counter++;
else if (system_state == 'T') image_counter++;
// Make sure the counter doesn't go out of bounds.
if (image_counter > image_counter_max) image_counter = image_counter_max;
if (image_counter < image_counter_min) image_counter = image_counter_min;
// Draw the appropriate colour:
switch( image_counter )
{
case 0:
App.Clear(sf::Color(0, 0, 0));
break;
case 1:
App.Clear(sf::Color(85, 85, 85));
break;
case 2:
App.Clear(sf::Color(170, 170, 170));
break;
case 3:
App.Clear(sf::Color(255, 255, 255));
break;
}
}
// Clear the screen with red color
// App.Clear(sf::Color(200, 0, 0));
// Display window contents on screen
App.Display();
}
return EXIT_SUCCESS;
}
void serialport_close(const char* sp) {
/*The arduino must be setup to use the same baud rate*/
ardu.Close() ;
}
