diff --git a/ReadMe.md b/ReadMe.md
index bfbd3253543efa55f8668967dbe61bba335a1565..ac809c61deee0853d0d111679cec9210b5133368 100644
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -3,26 +3,35 @@ model_car_drivers {#mainpage}
## Description
-Drivers for model car platform
+This C++ library includes the Linux drivers for the four Arduino based control boards present in the model car:
-## Installation
+* [Actuators](doc/actuators.md): To control the speed and steering angle of the model car.
+* [Egomotion](doc/egomotion.md): To get the wheel encoder the IMU sensor data.
+* [Ultrasounds](doc/ultrasounds.md): To get the distance measured by the ultrasound sensors.
+* [Batteries](doc/batteries.md): To get the current state of both batteries.
-* Add the labrobotica repository if it is not already added: run the commands on _add repository_ and _add key_ from [labrobotica_how_to installation](https://gitlab.iri.upc.edu/labrobotica/labrobotica_how_to/-/blob/master/README.md#installation)
-* Install the package:
+All these drivers inherit from a common base class as shown in the next image.
- sudo apt update && sudo apt install iri-model-car-drivers-dev
+<img align="center" src="doc/images/model_car_drivers.png" alt="Class hierarchy of the model car drivers" width="977" height="388">
-## Disclaimer
+This base class implements the following features, and the inherited classes implement the specific functionality required. See the description of each driver for more detailed description.
-Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-Mantainer IRI labrobotica (labrobotica@iri.upc.edu)
+* Handles the serial port.
+* Handles the communication protocol for transmission and reception.
+* Provides asynchronous notifications when new data is available.
+* Provides a virtual function to process the received data in the inherited classes.
+* Provides the timestamp of the most recent data received.
-This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
+The four on-board control boards can be accessed through a ttyACM* Linux device. However, it is not possible to differentiate them before reading the unique ID by software. For this reason, the drivers loop through all available devices looking for the one matching the desired ID. It a driver loops through all available devices without finding the desired one, it will report a failure. In general this type of errors are caused by the concurrent access of multiple drivers to the same physical device, and can be solved by repeating the process.
-In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+## Installation
-You should have received a copy of the GNU Lesser General Public License
-along with this program. If not, see <http://www.gnu.org/licenses/>
+* Add the labrobotica repository if it is not already added: run the commands on _add repository_ and _add key_ from [labrobotica_how_to installation](https://gitlab.iri.upc.edu/labrobotica/labrobotica_how_to/-/blob/master/README.md#installation)
+* Install the package:
+
+```
+sudo apt update && sudo apt install iri-model-car-drivers-dev
+```
## For developers
@@ -41,27 +50,38 @@ Under linux all of these utilities are available in ready-to-use packages.
Under MacOS most of the packages are available via [fink](http://www.finkproject.org/ "Fink's Homepage")
-This package also requires of the following dependencies:
-- comm
+This package also requires of the following IRI dependencies:
+
+* [iriutils](https://gitlab.iri.upc.edu/labrobotica/algorithms/iriutils "iriutils gitlab page"), a set of basic tools.
+* [comm](https://gitlab.iri.upc.edu/labrobotica/drivers/comm "comm gitlab page"), a set of basic communication drivers.
## Compilation and installation from source
Clone this repository and create a build folder inside:
- mkdir build
+```
+mkdir build
+```
Inside the build folder execute the following commands:
- cmake ..
+```
+cmake ..
+```
The default build mode is DEBUG. That is, objects and executables include debug information.
The RELEASE build mode optimizes for speed. To build in this mode execute instead
- cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+
+```
+cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+```
The release mode will be kept until next time cmake is executed.
- make -j $(nproc)
+```
+make -j $(nproc)
+```
In case no errors are reported, the generated libraries (if any) will be located at the
_lib_ folder and the executables (if any) will be located at the _bin_ folder.
@@ -69,7 +89,9 @@ _lib_ folder and the executables (if any) will be located at the _bin_ folder.
In order to be able to use the library, it it necessary to copy it into the system.
To do that, execute
- make install
+```
+make install
+```
as root and the shared libraries will be copied to */usr/local/lib/iri/model_car_drivers* directory
and the header files will be copied to */usr/local/include/iri/model_car_drivers* directory. At
@@ -77,31 +99,49 @@ this point, the library may be used by any user.
To remove the library from the system, exceute
- make uninstall
+```
+make uninstall
+```
as root, and all the associated files will be removed from the system.
To generate the documentation execute the following command:
- make doc
+```
+make doc
+```
## How to use it
To use this library in another library or application, in the CMakeLists.txt file, first it is necessary to locate if the library has been installed or not using the following command
- FIND_PACKAGE(model_car_drivers)
+```
+FIND_PACKAGE(model_car_drivers)
+```
In the case that the package is present, it is necessary to add the header files directory to the include directory path by using
- INCLUDE_DIRECTORIES(${model_car_drivers_INCLUDE_DIRS})
+```
+INCLUDE_DIRECTORIES(${model_car_drivers_INCLUDE_DIRS})
+```
and it is also necessary to link with the desired libraries by using the following command
- TARGET_LINK_LIBRARIES(<executable name> ${model_car_drivers_LIBRARIES})
+```
+TARGET_LINK_LIBRARIES(<executable name> ${model_car_drivers_LIBRARIES})
+```
-## Examples
+</p>
+</details>
-There are examples that show how to use the different classes.
+## Disclaimer
-</p>
-</details>
\ No newline at end of file
+Copyright (C) Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+Mantainer IRI labrobotica (labrobotica@iri.upc.edu)
+
+This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
+
+In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>
\ No newline at end of file
diff --git a/doc/actuators.md b/doc/actuators.md
new file mode 100644
index 0000000000000000000000000000000000000000..ddd342c6d4f6bb90be668602eb3cd381bda5e3df
--- /dev/null
+++ b/doc/actuators.md
@@ -0,0 +1,58 @@
+# Actuators driver
+
+This driver allows to send motion commands to the model car platform. The motions commands are:
+
+* **speed**: this is desired speed of the car. This value represents a percentage of the maximum speed and the valid range is from -100.0 to 100.0.
+* **steering**: This is the desired angle of the servo that controls the steering mechanism of the model car. The actual steering angle of the model car is not directly proportional to this value.
+
+Check section 1,5 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) for further details on the traction motor, its speed controller and the steering servo used in the model car.
+
+The main features of this driver are:
+
+* Send speed and steering commands.
+* A watchdog that forces the motion command to be periodically sent, or otherwise the model car will stop after a while.
+* An emergency stop that, when activated, disables both the traction motor and the steering servo, and prevents the car from moving again. The only way to re-enable them is to reset the associated Arduino board.
+
+## Calibration
+
+The [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) of the model car, in chapter 3, presents the procedure to calibrate and adjust both speed and steering controllers. The factory calibration should be enough, and, in general, it is only necessary to adjust the zero of both the speed and steering controllers using the two potentiometers located on the PCB.
+
+## Example
+
+This driver provides a simple example that sends both speed and steering commands following a sinusoidal function. To test it, make sure the wheels of the model car are not in contact with the floor and execute the following command:
+
+```
+./bin/model_car_actuators_test
+```
+
+## Troubleshooting
+
+If the driver can not connect to the Arduino board:
+
+* Check that at least there exist 4 **ttyACM\*** devices in the */dev* folder. If any **ttyACM\*** device is missing:
+ * check that the USB cable is attached to the min-USB connector. Connect it if necessary.
+ * check that the green LED in the Arduino board is blinking. If the LED is not blinking, check the power supply and contact the technical support.
+* Check that the user belongs to the *dialout* group. To do so, execute the following command on a terminal. An example output is shown below:
+```
+groups
+car adm **dialout** cdrom sudo dip plugdev lpadmin sambashare
+```
+* Contact the technical support.
+
+If the traction motor and/or the steering servo do not move:
+
+* Make sure both the power and monitor connectors of the motors battery are connected. The battery display monitor should display the current voltage of each cell. Connect them if necessary. See the next image.
+
+<img align="center" src="images/motor_battery_troubleshooting.png" alt="Correct connection of the motor battery" width="921" height="371">
+
+* Make sure the batteries are charged. Check the value shown in the battery display monitor and charge them if necessary.
+* Make sure the button on the right side of the model car, labeled Motor, is pressed. Press it if necessary.
+
+<img align="center" src="images/motor_button.png" alt="Correct position of the motor button on the right side of the model car" width="496" height="372">
+
+* Make sure the control board is not configured for RF control (see section 2.5 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r)).
+* Reset the Arduino actuator controller. The actuator Arduino is the one besides the motor battery display monitor.
+* Contact the technical support.
+
+
+
diff --git a/doc/batteries.md b/doc/batteries.md
new file mode 100644
index 0000000000000000000000000000000000000000..d4c1f0a9acba8459d8d346416e1acf8efaa74b19
--- /dev/null
+++ b/doc/batteries.md
@@ -0,0 +1,33 @@
+# Batteries
+
+This driver provides information on the state of the two batteries of the model car:
+
+* Motors battery: powers the traction motor, the speed controller and the steering servo.
+* Payload battery: powers the on-board computer and GPU, Arduino board controllers, sensors, USB hubs and other on-board devices.
+
+The information provided for each battery is the individual cell voltages and the whole battery voltage. It does not provide any fuel-gauging capabilities that estimates the actual state of charge of the batteries.
+
+In addition of the software monitoring of the batteries provided by this driver, it is very important to connect both batteries to the onboard display monitoring modules which will provide an acoustic signal when it is necessary to charge the batteries.
+
+# Example
+
+This driver provides a simple example that shows the measured data of the sensors on screen. To test it execute the following command:
+
+```
+./bin/model_car_batteries_test
+```
+
+This is an example output of the example program:
+
+```
+ motors battery voltage: 8.221
+ motors battery cell1 voltage: 4.051
+ motors battery cell2 voltage: 4.17
+ payload battery voltage: 23.837
+ payload battery cell1 voltage: 3.963
+ payload battery cell2 voltage: 3.992
+ payload battery cell3 voltage: 4.052
+ payload battery cell4 voltage: 4.081
+ payload battery cell5 voltage: 3.963
+ payload battery cell6 voltage: 4.111
+```
\ No newline at end of file
diff --git a/doc/egomotion.md b/doc/egomotion.md
new file mode 100644
index 0000000000000000000000000000000000000000..308c1311702fa4b932ab144e195cd1783ed37a78
--- /dev/null
+++ b/doc/egomotion.md
@@ -0,0 +1,53 @@
+# Egomotion
+
+This driver provides access to the following sensor information:
+
+* The left and right wheel encoder total number of counts.
+* The left and right wheel direction.
+* The linear acceleration, angular velocity and magnetic heading from the IMU.
+
+Each measurement has a timestamp associated to it. This timestamp starts when the car is first powered up or when it is reset and has a resolution of milliseconds. Although it can not be used as an absolute time reference, it can be used to know the interval between consecutive measurements.
+
+Notice that there is no feedback for the actual steering servo angle.
+
+## Wheel encoders
+
+The number of counts for the wheel encoder data represents the total displacement in either direction, and the direction flag must be used two know if the count increment between the current and the last measurement is positive or negative.
+
+The wheel encoders have a relatively low resolution (60 counts per revolution) which makes it difficult to have a good speed control at low speeds. Check section 1.4.4 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) for further details.
+
+## IMU
+
+The IMU used in the car is the [MPU9250](https://invensense.tdk.com/products/motion-tracking/9-axis/mpu-9250/). Check its documentation for further details and section 1.4.3 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) fir further details.
+
+# Example
+
+This driver provides a simple example that shows the measured data of the sensors on screen. To test it execute the following command:
+
+```
+./bin/model_car_egomotion_test
+```
+
+This is an example output of the example program:
+
+```
+ Left Wheel:
+ tachometer: 64
+ direction: 0
+ timestamp: 1124172648
+ Right Wheel:
+ tachometer: 57
+ direction: 0
+ timestamp: 1124172804
+ IMU:
+ ax: -0.0724493
+ ay: -0.760419
+ az: 9.76869
+ gx: 0.00119842
+ gy: -0.00266316
+ gz: -0.00186421
+ mx: -1.31566e-05
+ my: -3.18546e-05
+ mz: 1.53447e-05
+ timestamp: 1124200160
+```
\ No newline at end of file
diff --git a/doc/images/drivers.odp b/doc/images/drivers.odp
new file mode 100644
index 0000000000000000000000000000000000000000..6e2ca7c8d9334165a1605f3afb52e876c9890c15
Binary files /dev/null and b/doc/images/drivers.odp differ
diff --git a/doc/images/model_car_drivers.png b/doc/images/model_car_drivers.png
new file mode 100644
index 0000000000000000000000000000000000000000..11d992ec6f988d0bae90734e9ea52d5a582d4282
Binary files /dev/null and b/doc/images/model_car_drivers.png differ
diff --git a/doc/images/motor_battery_troubleshooting.png b/doc/images/motor_battery_troubleshooting.png
new file mode 100644
index 0000000000000000000000000000000000000000..ebc3ce4567c637b29c576eadb6a65aa4956fef34
Binary files /dev/null and b/doc/images/motor_battery_troubleshooting.png differ
diff --git a/doc/images/motor_button.png b/doc/images/motor_button.png
new file mode 100644
index 0000000000000000000000000000000000000000..b75deacd09240e651b7569ed99cff6a2b6eb0597
Binary files /dev/null and b/doc/images/motor_button.png differ
diff --git a/doc/ultrasounds.md b/doc/ultrasounds.md
new file mode 100644
index 0000000000000000000000000000000000000000..19f2fae3b52a654b944e619cadc7e8ad28e2a7b6
--- /dev/null
+++ b/doc/ultrasounds.md
@@ -0,0 +1,29 @@
+# Ultrasounds
+
+This driver provides the distance measured by each of the ultrasound sensors available on the car:
+
+* Left side
+* Right side
+* Rear left side
+* Rear
+* Rear right side
+
+The distance provided by this driver is in meters. Check section 1.4.2 of the [hardware manual](https://drive.google.com/drive/folders/1OxAIbyjF__c_3nsVdmDuN2BNE6a4JI3r) for more details on the ultrasound sensors used on the model car
+
+# Example
+
+This driver provides a simple example that shows the measured data of the sensors on screen. To test it execute the following command:
+
+```
+./bin/model_car_ultrasounds_test
+```
+
+This is an example output of the example program:
+
+```
+ Side right ultrasound distance: 0.18
+ Side left ultrasound distance: 1.17
+ Rear right ultrasound distance: 1.2
+ Rear center ultrasound distance: 1.17
+ Rear left ultrasound distance: 1.18
+```