correct tick diff calculation for first iteration

diff --git a/main.cc b/main.cc
index b598a4e..e373c51 100644 (file)
--- a/main.cc
+++ b/main.cc
@@ -103,10 +103,23 @@ int nearestDiff(unsigned short last, unsigned short current) {
 struct DataAvailable {
   int latest_ticks_left_, latest_ticks_right_;
 
-  DataAvailable() :
-    latest_ticks_left_(0), latest_ticks_right_(0) {
+  /**
+   * Initialisation. Calling this function is optional, but strongly encouraged
+   * if you want to get right results.
+   * @param init_ticks_left Initial ticks of left wheel used to calculate the
+   *  difference
+   * @param init_ticks_left Initial ticks of right wheel used to calculate the
+   *  difference
+   */
+  void init(int init_ticks_left, int init_ticks_right) {
+    latest_ticks_left_ = init_ticks_left;
+    latest_ticks_right_ = init_ticks_right;
   }
 
+  /**
+   * Callback for Roomba<...>::register_state_callback()
+   * @param state Callback parameter that describes the state ot the data
+   */
   void cb(int state) {
     if(state != Roomba::DATA_AVAILABLE) {
       return;
@@ -285,6 +298,7 @@ int main(int argc, char ** argv) {
 
   // fill it once
   roomba.notify_state_receivers(Roomba::DATA_AVAILABLE);
+  data_available.init(sensor_data.raw_left_ticks, sensor_data.raw_right_ticks);
 
   // actual tests
   roomba_id = argv[2];