From 2a37ab9fea9ce823a193d6d7d305471e245eb804 Mon Sep 17 00:00:00 2001 From: svnhieber Date: Wed, 1 Dec 2010 21:09:14 +0000 Subject: [PATCH] added information about current Roomba orientation; chargeText() function git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@3657 f8795833-4959-0410-8ae9-8bcb0cfab428 --- main.cc | 91 +++++++++++++++++++++++++++++++++------------------------ 1 file changed, 53 insertions(+), 38 deletions(-) diff --git a/main.cc b/main.cc index cfba07b..13c5403 100644 --- a/main.cc +++ b/main.cc @@ -46,7 +46,6 @@ using namespace std; // UART port on which we communicate with the Roomba char uart[] = "/dev/ttyUSB0"; -char battery_string[] = "Battery: %1%\nPress Cancel to exit.\n\n"; typedef wiselib::PCOsModel OsModel; typedef wiselib::StandaloneMath Math; @@ -54,6 +53,14 @@ typedef wiselib::PCComUartModel RoombaUart; typedef wiselib::RoombaModel Roomba; typedef wiselib::ControlledMotion ControlledMotion; +/** + * 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)); +} + /** * drive iterations. logs values to stdout. */ @@ -62,16 +69,14 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) { bool ok = false; while(true) { - QString chargeText = QString(battery_string).arg(int(float(roomba().charge) - / float(roomba().capacity) * 100.0)); // new distance to drive - input_distance = getInt(0, "Input distance", chargeText + input_distance = getInt(0, "Input distance", chargeText(roomba) + "Input new distance in mm:", input_distance, numeric_limits::min(), numeric_limits::max(), 1, &ok); if(ok) { // new turn velocity - velocity = getInt(0, "Input velocity", chargeText + velocity = getInt(0, "Input velocity", chargeText(roomba) + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok); if(ok) { ctrl_motion.move_distance(input_distance, velocity); @@ -84,11 +89,11 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) { } // measured deviation - deviation_x = getInt(0, "Input x deviation", chargeText + deviation_x = getInt(0, "Input x deviation", chargeText(roomba) + "Input travelled distance on x axis in mm:", deviation_x, numeric_limits::min(), numeric_limits::max(), 1, &ok); if(ok) { - deviation_y = getInt(0, "Input y deviation", chargeText + deviation_y = getInt(0, "Input y deviation", chargeText(roomba) + "Input travelled distance on y axis in mm:", deviation_y, numeric_limits::min(), numeric_limits::max(), 1, &ok); if(ok) { @@ -113,46 +118,56 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) { * turn iterations. logs values to stdout. */ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) { - int input_angle = 0, measured_angle = 0, velocity = 100; + int cur_angle = 0, turn_angle = 0, measured_angle = 0, velocity = 100; bool ok = false; + // current angle + cur_angle = getInt(0, "Input current orientation", chargeText(roomba) + + "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok); + if(!ok) { + return; + } + while(true) { - QString chargeText = QString(battery_string).arg(int(float(roomba().charge) - / float(roomba().capacity) * 100.0)); + // new turn velocity + velocity = getInt(0, "Input velocity", chargeText(roomba) + + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok); + if(!ok) { + break; + } - // new angle to turn about - input_angle = getInt(0, "Input angle", chargeText - + "Input new angle in degree to turn about:", input_angle, - numeric_limits::min(), numeric_limits::max(), 1, &ok); - if(ok) { - // new turn velocity - velocity = getInt(0, "Input velocity", chargeText - + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok); - if(ok) { - ctrl_motion.turn_about(Math::degrees_to_radians(input_angle), velocity); - roomba.wait_for_stop(); - } else { - break; - } - } else { + // angle to turn about + turn_angle = getInt(0, "Input turn angle", chargeText(roomba) + + "Input angle in degree to turn about:", turn_angle, + numeric_limits::min() + 360, numeric_limits::max() - 360, 1, + &ok); + if(!ok) { break; } - // measured angle - measured_angle = getInt(0, "Input measured angle", chargeText - + "Input measured angle in degree:", input_angle, - numeric_limits::min(), numeric_limits::max(), 1, &ok); - if(ok) { - cout << "input_angle=" << input_angle << " velocity=" << velocity - << " measured_angle=" << measured_angle << " 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; - } else { + ctrl_motion.turn_about(Math::degrees_to_radians(turn_angle), velocity); + roomba.wait_for_stop(); + + // new current angle + measured_angle = getInt(0, "Input measured angle", chargeText(roomba) + + QString("Orientation should be %1 degree now.\n\n").arg((cur_angle + + turn_angle) % 360) + "Input measured angle in degree the Roomba has " + "turned:", turn_angle, 0, 359, 1, &ok); + if(!ok) { break; } + + 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; + + // new orientation + cur_angle = (cur_angle + turn_angle) % 360; + } } -- 2.20.1