Retrieving distance and angle of roobma create2

Question

The goal of my project is to get a real-time location information of roobma. The information includes the x position, the y position and the angle (12 o'clock direction is 0 degree, 3 o'clock is 90 degree and so on).

I am using C++ to program the roomba.

I want to use "Stream" (Opcode: 148) to get the date, but it doesn't work for me.

This is the part I am getting stuck at:

unsigned char streamCommand[3];
streamCommand[0]=148;
streamCommand[1]=1;
streamCommand[2]=19;
write(robot,&streamCommand,3);
signed char readStream[6];


bool indicator=true;
while(indicator){
    string command="";
    cout<<"Please input your command"<<endl;
    cin>>command;
    action(command);
    read(robot,&readStream,6);
    cout<<"[0]="<<readStream[0]<<endl;
    cout<<"[1]="<<readStream[1]<<endl;
    cout<<"[2]="<<readStream[2]<<endl;
    cout<<"[3]="<<readStream[3]<<endl;
    cout<<"[4]="<<readStream[4]<<endl;
    cout<<"[5]="<<readStream[5]<<endl;
}

return 0;

}

streamCommand is a read command to ask roomba to send back data in stream.
readStream is the array that I store my retrieve data.

Why is this code not working?

Answer 1

You say, "It doesn't work for me." What does that mean? You get nothing? You get numbers that don't make sense? Does it lock up? Something else?

First, I'd refer you to the Create2 Documentation where I'd suggest you read up on the Opcode 148 protocol, starting on Page 20.

Notably:

Serial sequence: [148] [Number of packets] [Packet ID 1] [Packet ID 2] etc.

In your question title you say you want distance and angle (packets 19 and 20) but in your question you say you want a date (which doesn't exist). In your code though, you're only requesting packet 19, not 19 and 20. Your code should be:

streamCommand[0]=148;
streamCommand[1]=2;
streamCommand[2]=19;
streamCommand[3]=20;

On the next page in the documentation:

The format of the data returned is:

[19][N-bytes][Packet ID 1][Packet 1 data...][Packet ID 2][Packet 2 data...][Checksum]

So, let's say for the moment that you really DO just want packet 19 right now, for troubleshooting/development purposes. In that case, the data coming back to you would look like the following:

[19][3][19][2 bytes of data][checksum]

The first byte is the header, 19. The second byte is the number of bytes between the second byte and the checksum, which is three: the packet ID (the number 19) and the two bytes of data for that packet. The third byte is the packet ID (19) The fourth and fifth bytes are the data for that packet. The sixth byte is the checksum.

The problem here that you might be having if you're getting data but it doesn't look correct is that you don't appear to ever check that you're starting in the right location; you need to parse the data you read to be sure that you're getting valid data.

Ideally, you should read bytes one at a time until you get the correct header (19). Then you should read the next byte, which is supposed to be the number of "data" bytes. Then you read however many bytes the second byte tells you to (3), then you read one more for the checksum.

Run the checksum calculation and compare your calculated checksum to the provided checksum. If the values don't match, then you either have bad data or the header you read (19) isn't the actual header but something else (maybe the packet ID - it's also 19).

In your case, you might be able to skate by knowing that you should expect [19][3][19] (or, when you get to reading distance AND angle, [19][6][19]).

If you have a three-byte sequence that goes [19][3][19], then you could probably assume that's [header][number of bytes][packet ID].

Again, though, you should really be checking the checksum, because it could be [packet ID][byte 1][byte 0], where byte 1 is 3 and byte 0 is 19. Not likely to happen, but definitely could.

Also keep in mind that the Create2 starts writing data to you every 15ms after you issue the stream command; I'm not sure how your language/hardware handles buffers but you might get a buffer overflow and/or garbage data if you don't periodically clear that input buffer.

You can handle that by reading the entire buffer, parse the data for the first (or last, or arbitrary, etc.) message, then ignoring/deleting the rest of the data, or you could enter a while/for loop to read through all the data and display all of the valid data contained in the buffer.

As a final note, again you're constantly getting data returned to you right now, but you're pausing for input in your while loop. The messages will continue to back up while you're paused, and again you don't read/clear the buffer after the pause, you only read the first six bytes.