X-Git-Url: https://git.rohieb.name/bachelor-thesis/roomba_tests.git/blobdiff_plain/7682d848008691dd9c01b5b1b2663dfd8c2952cc..46028ef249b14aef45c0020fb1a379cb1128a1f9:/battery_test.cc

diff --git a/battery_test.cc b/battery_test.cc
index e884c42..5ee5807 100644
--- a/battery_test.cc
+++ b/battery_test.cc
@@ -43,33 +43,63 @@ 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;
+  cerr << "Got roomba at " << roomba_uart.address() << endl;
 
   roomba.reset_distance();
   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);
   }
 }