better use only sensor data when it is available, not the probably corrupted roomba...

[bachelor-thesis/roomba_tests.git] / battery_test.cc

diff --git a/battery_test.cc b/battery_test.cc
index e884c42..f5fb28f 100644 (file)
--- a/battery_test.cc
+++ b/battery_test.cc
@@ -43,22 +43,48 @@ typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
 
+/**
+ * Global objects 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;
+} sensor_data;
+
+/**
+ * Callback that fills the sensor data when data is available
+ */
+struct DataAvailable {
+  void cb(int foo) {
+    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;
+  }
+} data_available;
+
 /**
  * main function
  */
 int main(int argc, char ** argv) {
 
-  OsModel::Os os;
-  OsModel::Timer::self_t timer;
-  Roomba roomba;
-  RoombaUart roomba_uart(os);
-  ControlledMotion ctrl_motion;
-
   // init stuff
   roomba_uart.set_baudrate(19200);
   roomba_uart.enable_serial_comm();
-  roomba.init(roomba_uart, timer, Roomba::POSITION
-    | Roomba::BATTERY_AND_TEMPERATURE);
+  roomba.init(roomba_uart, timer, Roomba::BATTERY_AND_TEMPERATURE);
 
   cout << "Got roomba at " << roomba_uart.address() << endl;
 
@@ -66,10 +92,14 @@ int main(int argc, char ** argv) {
   roomba.reset_angle();
   ctrl_motion.init(roomba);
 
+  roomba.register_state_callback<DataAvailable, &DataAvailable::cb> (
+    &data_available);
+  roomba.notify_state_receivers(Roomba::DATA_AVAILABLE);
+
   while(true) {
-    cout << "batt_charge=" << roomba().charge << " batt_capacity="
-      << roomba().capacity << " batt_voltage=" << roomba().voltage
-      << " batt_current=" << roomba().current << endl;
-    sleep(1);
+    cout << "batt_charge=" << sensor_data.charge << "\t batt_capacity="
+      << sensor_data.capacity << "\t batt_voltage=" << sensor_data.voltage
+      << "\t batt_current=" << sensor_data.current << endl;
+    sleep(1000);
   }
 }