X-Git-Url: http://git.rohieb.name/bachelor-thesis/roomba_tests.git/blobdiff_plain/2a37ab9fea9ce823a193d6d7d305471e245eb804..f6be7e61cd8ee3cfadf1d83c48a1b5b8d1628c0d:/main.cc?ds=inline

diff --git a/main.cc b/main.cc
index 13c5403..2363146 100644
--- a/main.cc
+++ b/main.cc
@@ -22,12 +22,9 @@
  * THE SOFTWARE.
  */
 
-//#include "main.h"
+#include "svnrevision.h"
 #include <iostream>
 #include <stdint.h>
-//#include <unistd.h>
-//#include "util/standalone_math.h"
-//#include "util/delegates/delegate.hpp"
 #include <external_interface/pc/pc_os_model.h>
 #include <external_interface/pc/pc_com_uart.h>
 #include <external_interface/pc/pc_timer.h>
@@ -53,12 +50,51 @@ typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
 
+/**
+ * Global stuff we need
+ */
+OsModel::Os os;
+OsModel::Timer::self_t timer;
+Roomba roomba;
+RoombaUart roomba_uart(os);
+ControlledMotion ctrl_motion;
+
+/**
+ * Sensor data we need, filled in callback
+ */
+struct SensorData {
+  uint16_t capacity, charge;
+  uint8_t charging;
+  int16_t current;
+  int8_t temperature;
+  uint16_t voltage;
+  int16_t left_encoder_counts, right_encoder_counts;
+} sensor_data;
+
+/**
+ * Callback that fills the sensor data when data is available
+ */
+struct DataAvailable {
+  void cb(int state) {
+    if(state != Roomba::DATA_AVAILABLE) {
+      return;
+    }
+    sensor_data.capacity = roomba().capacity;
+    sensor_data.charge = roomba().charge;
+    sensor_data.charging = roomba().charging;
+    sensor_data.current = roomba().current;
+    sensor_data.voltage = roomba().voltage;
+    sensor_data.left_encoder_counts = roomba().left_encoder_counts;
+    sensor_data.right_encoder_counts = roomba().right_encoder_counts;
+  }
+} data_available;
+
 /**
  * return battery status as QString
  */
 QString chargeText(Roomba& roomba) {
   return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
-    roomba().charge) / float(roomba().capacity) * 100.0));
+    sensor_data.charge) / float(sensor_data.capacity) * 100.0));
 }
 
 /**
@@ -97,14 +133,15 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
         + "Input travelled distance on y axis in mm:", deviation_y,
         numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
       if(ok) {
-        cout << "input_distance=" << input_distance << " velocity=" << velocity
-          << " internal_distance=" << roomba.distance() << " deviation_x="
-          << deviation_x << " deviation_y=" << deviation_y
-          << " encoder_ticks_left=" << roomba().left_encoder_counts
-          << " encoder_ticks_right=" << roomba().right_encoder_counts
-          << " batt_charge=" << roomba().charge << " batt_capacity="
-          << roomba().capacity << " batt_voltage=" << roomba().voltage
-          << " batt_current=" << roomba().current << endl;
+        cout << "svn=" << SVNREVISION << " input_distance=" << input_distance
+          << " velocity=" << velocity << " internal_distance="
+          << roomba.distance() << " deviation_x=" << deviation_x
+          << " deviation_y=" << deviation_y << " encoder_ticks_left="
+          << sensor_data.left_encoder_counts << " encoder_ticks_right="
+          << sensor_data.right_encoder_counts << " batt_charge="
+          << sensor_data.charge << " batt_capacity=" << sensor_data.capacity
+          << " batt_voltage=" << sensor_data.voltage << " batt_current="
+          << sensor_data.current << endl;
       } else {
         break;
       }
@@ -152,21 +189,21 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
     measured_angle = getInt(0, "Input measured angle", chargeText(roomba)
       + QString("Orientation should be %1 degree now.\n\n").arg((cur_angle
       + turn_angle) % 360) + "Input measured angle in degree the Roomba has "
-      "turned:", turn_angle, 0, 359, 1, &ok);
+      "turned:", turn_angle, 0, numeric_limits<int>::max(), 1, &ok);
     if(!ok) {
       break;
     }
 
     cout << "turn_angle=" << turn_angle << " measured_angle=" << measured_angle
       << " velocity=" << velocity << " internal_angle=" << roomba.angle()
-      << " encoder_ticks_left=" << roomba().left_encoder_counts
-      << " encoder_ticks_right=" << roomba().right_encoder_counts
-      << " batt_charge=" << roomba().charge << " batt_capacity="
-      << roomba().capacity << " batt_voltage=" << roomba().voltage
-      << " batt_current=" << roomba().current << endl;
+      << " encoder_ticks_left=" << sensor_data.left_encoder_counts
+      << " encoder_ticks_right=" << sensor_data.right_encoder_counts
+      << " batt_charge=" << sensor_data.charge << " batt_capacity="
+      << sensor_data.capacity << " batt_voltage=" << sensor_data.voltage
+      << " batt_current=" << sensor_data.current << endl;
 
     // new orientation
-    cur_angle = (cur_angle + turn_angle) % 360;
+    cur_angle = (cur_angle + measured_angle) % 360;
 
   }
 }
@@ -181,12 +218,6 @@ int main(int argc, char ** argv) {
     exit(-1);
   }
 
-  OsModel::Os os;
-  OsModel::Timer::self_t timer;
-  Roomba roomba;
-  RoombaUart roomba_uart(os);
-  ControlledMotion ctrl_motion;
-
   // init stuff
   QApplication app(argc, argv);
 
@@ -195,12 +226,20 @@ int main(int argc, char ** argv) {
   roomba.init(roomba_uart, timer, Roomba::POSITION
     | Roomba::BATTERY_AND_TEMPERATURE);
 
-  cout << "Got roomba at " << roomba_uart.address() << endl;
+  cerr << "Got roomba at " << roomba_uart.address() << endl;
 
   roomba.reset_distance();
   roomba.reset_angle();
   ctrl_motion.init(roomba);
 
+  // we do not want the probably corrupted data from roomba(), instead we fill
+  // our own values when data is available
+  roomba.register_state_callback<DataAvailable, &DataAvailable::cb> (
+    &data_available);
+
+  // fill it once
+  roomba.notify_state_receivers(Roomba::DATA_AVAILABLE);
+
   // actual tests
   if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
     turn(roomba, ctrl_motion);