From fd83f9977b9254ab4b4c86e8bdfdddab3083c1f9 Mon Sep 17 00:00:00 2001 From: svnhieber Date: Wed, 1 Dec 2010 21:09:18 +0000 Subject: [PATCH] better use only sensor data when it is available, not the probably corrupted roomba() data git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@3662 f8795833-4959-0410-8ae9-8bcb0cfab428 --- battery_test.cc | 54 +++++++++++++++++++++++++++-------- main.cc | 75 ++++++++++++++++++++++++++++++++++++++----------- 2 files changed, 100 insertions(+), 29 deletions(-) diff --git a/battery_test.cc b/battery_test.cc index e884c42..f5fb28f 100644 --- a/battery_test.cc +++ b/battery_test.cc @@ -43,22 +43,48 @@ typedef wiselib::PCComUartModel RoombaUart; typedef wiselib::RoombaModel Roomba; typedef wiselib::ControlledMotion 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 ( + &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); } } diff --git a/main.cc b/main.cc index af67cf5..2bff299 100644 --- a/main.cc +++ b/main.cc @@ -53,12 +53,51 @@ typedef wiselib::PCComUartModel RoombaUart; typedef wiselib::RoombaModel Roomba; typedef wiselib::ControlledMotion 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)); } /** @@ -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 - << " 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; } @@ -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() - << " 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; @@ -181,12 +220,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); @@ -201,6 +234,14 @@ int main(int argc, char ** argv) { 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 ( + &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); -- 2.20.1