The idea was that the controller must be ease to program, for that I used Arduino (full program available in the files section), so here are an explanation of the parts of the code that I considered more useful to understand and, of course, a general review of all the code.
The first part are de libraries used:
By default ros in the ros serial examples the TX/RX in the IC handle the nodes connections, but is possible to change that defining a new class and using it in the ros::NodeHandle declaration:
The function that fill the variables with data is:
The last part is:
So that's all, if you have any question just ask in the comments.