C++ Serial::write方法代码示例

void send_query(byte query)
{
  myPort.write(MSG_START);
  myPort.write(query);
  myPort.write(query);  //the checksum on queries is just the query again
  myPort.write(MSG_END); 
}

void send_command(byte command, byte highByte, byte lowByte)
{
  if(command!=STOP)
  {
    byte checksum=(byte)(((int)command+(int)highByte+(int)lowByte)%255);
    myPort.write(MSG_START);
    delay(del);
    myPort.write(command);
    delay(del);
    myPort.write(highByte);
    delay(del);
    myPort.write(lowByte);
    delay(del);
    myPort.write(checksum);
    delay(del);
    myPort.write(MSG_END);
  }
  else
  {
    myPort.write(MSG_START);
    delay(del);
    myPort.write(command);
    delay(del);
    myPort.write(command); //checksum
    delay(del);
    myPort.write(MSG_END);
  }
    
}

void pen_up()
{
  myPort.write(MSG_START);
  myPort.write(PEN_UP);
  myPort.write(PEN_UP);  //the checksum on queries is just the query again
  myPort.write(MSG_END); 
}

void pen_down()
{
  myPort.write(MSG_START);
    delay(del);
  myPort.write(PEN_DOWN);
    delay(del);
  myPort.write(PEN_DOWN);  //the checksum on queries is just the query again
    delay(del);
  myPort.write(MSG_END); 
}

void setup() 
{
  size(1300, 1000);
  img = (loadImage("pic4.jpg"));

  /*//Use this to create your own pic of a specific colour to test individual RGB vals
   img = createImage(cols,rows,RGB); // 
   for(int i = 0; i < img.pixels.length; i++) {
   img.pixels[i] = color(0, 0, 255); 
   }
   */

  loadPixels(); 
  System.out.println(img.pixels.length);
  String portName = "COM4";
  myPort = new Serial(this, portName, 9600);
  background(0);
  dataIn[0][0] = 0;

  //send ack every 2 seconds, wait for ack back (see serial event
  //this begins negotiation between machines
  //reception of the ack is handled by serial interrupt method below
  while (!ack) {  
    myPort.write(88); 
    delay(2000);
  }
    ack = true;
}//end setup

	bool waa::PC2Arduino(char *command){
		int i = 0;
		char buffer = '\0';
		bool status = false;
		//tstring commPortName(Comport);
		//Serial serial(commPortName);
		serial.flush();
	
		do {
			i++;
		} while (strcmp(command, PCCommand[i]) != 0 || i >= DEF_ARDUINO_COMMAND_NUMBER);

		if (i<DEF_ARDUINO_COMMAND_NUMBER && i >= 0)
		{
			//serial.write(ArduinoCommand+i);
			serial.write(ArduinoCommand[i]);
			Sleep(5);
			serial.read(&buffer, 1);
			Sleep(5);
			if (buffer == '0')	status = true;
		}
		else {
			printf("command error,please insert again\n");
		}
	

		return status;
}

void send_empty_packet()
{
  memset(packet, 0, PACKET_MAX_SIZE); 
  packet[0] = PACKET_SYNC;
  packet[1] = PACKET_VER;
  
  serial_port.write(packet, 16);
}

/*The main loop*/
void draw()
{      
  //write X pixel data to serial
  //for (int y=0;y<48;y++) {   ***Don't actually need to increment y right now b/c it is incremented
  //                              the correct amount below in the fullR loop -- which is convenient!   
  if (ack) {
    myPort.write(101);
    for (int x=0;x<cols;x++) {
      //get pixel, extract rgb vals from pixel using bit shifting (faster)
      color rgb = img.pixels[y*cols + x];
      int r = rgb >> 16 & 0xFF; //Equivalent to, but faster than red(rgb)
      int g = rgb >> 8 & 0xFF;
      int b = rgb & 0xFF;
      myPort.write(r);
      myPort.write(g);
      myPort.write(b);
      println("Sent " + x);
      //delay(50);
    }
    // for (int x=0;x<10;x++)
    //      myPort.write(255);
    //set this to wait for another request 
    ack = false;
  }
  //if a full packet has been received, print to screen
  if (fullR)
  { 
    for (int i=1; i<dataIn.length; i++)
    {    
      System.out.println("R " + dataIn[i][0] + " G " + dataIn[i][1] + " B" + dataIn[i][2]);
    }  
    fullR = false;
    for (int x=1; x<dataIn.length; x++)
    {
      fill(dataIn[x][0], dataIn[x][1], dataIn[x][2]);
      //stroke(255);
      rect((x-1)*5, y*5, 5, 5);
    }
    y+=1;
    System.out.println(y);
  }
}//end loop

/*Method is triggered when there is byte on serial port */
void serialEvent(Serial thisPort) {
  if (thisPort == myPort)
  {
    // variable to hold the data on bus      
    int inByte = thisPort.read();

    if (dataIn[0][0] == 100)//if receive header has been received
    {
      /*Put inByte into array
       RGBcount - keeps track of which value (r,g, or b) is to have the value
       - loops after 3 (0,1,2)
       Once 8 pixels have been received (24 rgb vals), fullR=true. Get ready to receive back
       */
      dataIn[count][rgbCount] = inByte;
      System.out.println(inByte + "in Count: " + count + " RGB: " + rgbCount);    
      rgbCount = (rgbCount+1)%3;
      if (rgbCount == 0)
        count++;          
      if (count >= dataIn.length)
      {
        fullR = true;
        System.out.println("ONE ROW");
        dataIn[0][0] = 0;
      }
    }
    else if (inByte == 100)//header character to indicate transmission of a pixel
    {
      System.out.println("Header received");
      dataIn[0][0] = inByte;
      count=1;
    }
    else if (inByte == 87 && dataIn[0][0] == 0)//ack received from arduino && waiting for ack; 
    {
      //we will only receive ack to start transmission if we are done receiving
      //when finished receiving, dataIn is set to 0;
      //set ack = true to start transmission
      //technically, checking dataIn is irrelavent b/c it is checked before inbyte is checked for 87,
      // but we are preventing future challenges if we rearrange the order of this cascading if statement
      ack = true;
      System.out.println("Ack 87 received");
    }
     else if (inByte == 89 && dataIn[0][0] == 0)//ack received from arduino && waiting for ack; 
    {
      //we will only receive ack to start transmission if we are done receiving
      //when finished receiving, dataIn is set to 0;
      //set ack = true to start transmission
      //technically, checking dataIn is irrelavent b/c it is checked before inbyte is checked for 87,
      // but we are preventing future challenges if we rearrange the order of this cascading if statement
      myPort.write(90);
      ack = true;
      System.out.println("Ack 90 received");
    }
  }
}//end serialEvent   

int main(int argc, char* argv[])
{
	printf("[i] Start... \n");

	Serial serial;
#if defined(WIN32)
	serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE, true);
#elif defined(LINUX)
	serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE);
#endif

	if(!serial.connected())
	{
		printf("[!] Cant open port!\n");
		return -1;
	}

	char c = 'y';
	int res = 12;

	char buf[BUF_SIZE1];
	memset(buf, 0, BUF_SIZE1);

	while(true)
	{

		serial.write( &c, 1 );
#if 0
		if(res = serial.available()){
			if( res = serial.Read(buf, res) ){
				printf("%d %s\n", res, buf);
			}
		}
#else
		if(serial.waitInput(1000)==0)
			printf("[i] timeout!\n");
		else
		{
			if(res = serial.available())
			{
				res = serial.read(buf, res);
				printf("%d %s\n", res, buf);
			}
		}
#endif
	}
	serial.close();

	printf("[i] End.\n");
	return 0;
}

int main(int argc, char **argv){

    cout<<"Connecting to the k3 robot..."<<endl;
	
	try {
        k3_ser.open();
    }
    catch(std::exception e) {
        std::stringstream output;
        output<<"Failed to open serial port "<< k3_ser.getPort() << "error: " << e.what() <<endl;
    }

    if(k3_ser.isOpen()){

        cout<<"k3_ser is Connected on Port: "<<k3_ser.getPort()<<" at Baudrate: "<<k3_ser.getBaudrate()<<endl;
    }
    else{

        ROS_ERROR("serial port not openened, verify the k3 robot selector position");
    }
	//test serial command
    k3_ser.write("D,0,0\n");

	ros::init(argc, argv, "k3");
	
	cout<<"hello ros";
	
    ros::NodeHandle n;

   cmdVelSubscriber = n.subscribe("/robot_motors", 10, handlerVelocity);

    ros::Rate loop_rate(10);
    while(ros::ok()){
		k3Motors();
		//k3Cmd();
        ros::spinOnce(); //Allow ROS to check for new ROS Messages
        loop_rate.sleep(); //Sleep for some amount of time determined by loop_rate

    }

    return 0;
}

TEST(Serial, WriteCOM) {

	Serial serial;

	serial.open("COM7", 9600);

	if(!serial.connected()){
		FAIL()<<"cant open serial port!";
	}

	char buf[1024];
	int res = 0;
	while(1){

		char wbuf[] = {0xff, 0xff, 0x00, 0x00, 0x00, 0x00 ,0xff};

		serial.write(wbuf, sizeof(wbuf));

		if( serial.waitInput(1000) > 0){
			if( (res = serial.available()) > 0 ){
				if( res = serial.read(buf, res) ){
					printf("[i] read data(%d): \n", res);
					for(int i=0; i<res; i++){
						printf("%02X ", (unsigned char)buf[i]);
						if(i>0 && (i+1)%16 == 0) {
							printf("\t");
							for(int j=i-15; j<=i; j++){
								printf("%c", buf[j]);
							}
							printf("\n");
						}
					}
					printf("\n");
					res = 0;
				}
			}
		}
		Sleep(1000);
	}
}

//.........这里部分代码省略.........
  
  memset(packet, 0, PACKET_MAX_SIZE);
  
#define PUT_BYTE(X) packet[idx++] = (unsigned char)(X)
#define PUT_BYTE_FLOAT(X) packet[idx++] = (unsigned char)(float_to_byte(X))
#define GETFLTOPT(X) (option_file.get_option_float(X, 1.0f))
  
  // Packet header
  PUT_BYTE(PACKET_SYNC);
  PUT_BYTE(PACKET_VER);
  
  // Convert data for output by servos
  // Adjust the constant values to map to servo range
  
  PUT_BYTE_FLOAT(fabs(telemetry.speed) *        GETFLTOPT("factor_speed")); // Absolute value for when reversing
  PUT_BYTE_FLOAT(telemetry.engine_rpm *         GETFLTOPT("factor_engine_rpm"));
  PUT_BYTE_FLOAT(telemetry.brake_air_pressure * GETFLTOPT("factor_brake_air_pressure"));
  PUT_BYTE_FLOAT(telemetry.brake_temperature *  GETFLTOPT("factor_brake_temperature"));
  PUT_BYTE_FLOAT(fuel_ratio *                   GETFLTOPT("factor_fuel_ratio")); // Fuel level
  PUT_BYTE_FLOAT(telemetry.oil_pressure *       GETFLTOPT("factor_oil_pressure"));
  PUT_BYTE_FLOAT(telemetry.oil_temperature *    GETFLTOPT("factor_oil_temperature"));
  PUT_BYTE_FLOAT(telemetry.water_temperature *  GETFLTOPT("factor_water_temperature"));
  PUT_BYTE_FLOAT(telemetry.battery_voltage *    GETFLTOPT("factor_battery_voltage"));
  
  
  // Pack data for LEDs into bytes
  
  // Truck lights
  PUT_BYTE(PACKBOOL(
    0, telemetry.light_parking,
    telemetry.light_lblinker, telemetry.light_rblinker,
    telemetry.light_low_beam, telemetry.light_high_beam,
    telemetry.light_brake, telemetry.light_reverse));
    
  // Warning lights
  PUT_BYTE(PACKBOOL(
    telemetry.parking_brake, telemetry.motor_brake,
    telemetry.brake_air_pressure_warning, telemetry.brake_air_pressure_emergency,
    telemetry.fuel_warning, telemetry.battery_voltage_warning,
    telemetry.oil_pressure_warning, telemetry.water_temperature_warning));
  
  // Enabled flags
  PUT_BYTE(PACKBOOL(
    0,0,0,0,0, 0,
    telemetry.electric_enabled, telemetry.engine_enabled));
    

  // Build string for LCD display
    
  std::stringstream ss;
  
  ss.width(3);
  ss << abs(int(speed_mph));
  ss << " MPH";
  
  ss << "   G  ";
  
  if (telemetry.engine_gear > 0)
  {
    ss << "D";
    ss.width(2);
    ss << telemetry.engine_gear;
  }
  else if (telemetry.engine_gear == 0)
  {
    ss << "N  ";
  }
  else
  {
    ss << "R";
    ss.width(2);
    ss << abs(telemetry.engine_gear);
  }
  
  ss << "\n";
  
  ss.width(3);
  ss << abs(int(speed_kph));
  ss << " KPH";
  
  ss << "   F ";
  
  ss.width(3);
  ss << int(fuel_ratio * 100.0f) << "%";
  
  
  ss.width(3);
  ss << abs(int(speed_kph));
  
  std::string display_str(ss.str());
  
  // Write length of string
  PUT_BYTE(display_str.size());
  // Followed by string
  display_str.copy((char*)&packet[idx], PACKET_MAX_SIZE - idx);
  idx += display_str.size();
  
  
  serial_port.write(packet, idx);
}

//function to write serial commands for motors
void koalaMotors(){
	    char sercmd[12];
        sprintf(sercmd,"D,%d,%d\n",speedLeft,speedRight);
        koala_ser.write(sercmd);
	}

void koalaCmd(){
		koala_ser.write("D,50,50\n");
		ros::Duration(3).sleep();
		koala_ser.write("D,-50,-50\n");
		ros::Duration(3).sleep();
	}