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

git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@3662 f8795833-4959-0410-8ae9-8bcb0cfab428

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;
 
 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) {
 
 /**
  * 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();
   // 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;
 
 
   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.reset_angle();
   ctrl_motion.init(roomba);
 

+  roomba.register_state_callback<DataAvailable, &DataAvailable::cb> (
+    &data_available);
+  roomba.notify_state_receivers(Roomba::DATA_AVAILABLE);
+

   while(true) {
   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);

   }
 }
   }
 }

diff --git a/main.cc b/main.cc
index af67cf5..2bff299 100644 (file)
--- a/main.cc
+++ b/main.cc
@@ -53,12 +53,51 @@ typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
 
 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(
 /**
  * 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));

 }
 
 /**
 }
 
 /**


@@ -100,11 +139,11 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
         cout << "input_distance=" << input_distance << " velocity=" << velocity
           << " internal_distance=" << roomba.distance() << " deviation_x="
           << deviation_x << " deviation_y=" << deviation_y
         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;
+          << " 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;
       }
       } else {
         break;
       }


@@ -159,11 +198,11 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
 
     cout << "turn_angle=" << turn_angle << " measured_angle=" << measured_angle
       << " velocity=" << velocity << " internal_angle=" << roomba.angle()
 
     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 + measured_angle) % 360;
 
     // new orientation
     cur_angle = (cur_angle + measured_angle) % 360;


@@ -181,12 +220,6 @@ int main(int argc, char ** argv) {
     exit(-1);
   }
 
     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);
 
   // init stuff
   QApplication app(argc, argv);
 


@@ -201,6 +234,14 @@ int main(int argc, char ** argv) {
   roomba.reset_angle();
   ctrl_motion.init(roomba);
 
   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);
   // actual tests
   if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
     turn(roomba, ctrl_motion);