+/**
+ * Global stuff we need
+ */
+OsModel::Os os;
+OsModel::Timer::self_t timer;
+Roomba roomba;
+RoombaUart roomba_uart(os);
+ControlledMotion ctrl_motion;
+char * roomba_id;
+char * ground_type;
+
+/**
+ * Sensor data we need, filled in callback
+ */
+struct SensorData {
+
+ SensorData() :
+ capacity(0), charge(0), charging(0), current(0), temperature(0),
+ voltage(0), abs_left_encoder_counts(0), abs_right_encoder_counts(0) {
+ }
+
+ uint16_t capacity, charge;
+ uint8_t charging;
+ int16_t current;
+ int8_t temperature;
+ uint16_t voltage;
+ /** absolute encoder counts; i.e. not overflown, but consecutive */
+ int abs_left_encoder_counts, abs_right_encoder_counts;
+} sensor_data;
+
+/**
+ * Returns the difference between two unsigned short values. The calculated
+ * value is always smaller or equal to 0x8000.
+ * This is useful if you have an overflowing counter and you want to determine
+ * when you have to "wrap over" the value.
+ */
+int nearest_diff(unsigned short last, unsigned short current) {
+ int d = current - last;
+ if(d < -0x8000) { // overflow in positive direction
+ d = (0x10000 - last + current);
+ }
+ if(d >= 0x8000) { // overflow in negative direction
+ d = -(0x10000 - current + last);
+ }
+}
+
+/**
+ * Callback that fills the sensor data when data is available
+ */
+struct DataAvailable {
+ int latest_encoder_left_, latest_encoder_right_;
+
+ DataAvailable() :
+ latest_encoder_left_(0), latest_encoder_right_(0) {
+ }
+
+ 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.abs_left_encoder_counts += nearest_diff(latest_encoder_left_,
+ roomba().left_encoder_counts);
+ latest_encoder_left_ = roomba().left_encoder_counts;
+ sensor_data.abs_right_encoder_counts += nearest_diff(latest_encoder_right_,
+ roomba().right_encoder_counts);
+ latest_encoder_right_ = roomba().right_encoder_counts;
+ }
+} data_available;
+