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

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

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

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 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 open(){
  /*The arduino must be setup to use the same baud rate*/ 
  ardu.Open(PORT);    
  ardu.SetBaudRate(SerialStreamBuf::BAUD_9600);  
}

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;
//.........这里部分代码省略.........

void Serial_Close_Port(void)
{
        if(serial_port.IsOpen())
	{
                serial_port.Close();
	}
}

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