From b034d7b3c7417271a61e8e183a373d31674404ab Mon Sep 17 00:00:00 2001 From: svnhieber Date: Tue, 30 Nov 2010 17:10:10 +0000 Subject: [PATCH] simple Roomba test suite with Qt dialogs git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@3642 f8795833-4959-0410-8ae9-8bcb0cfab428 --- Makefile | 2 + main.cc | 189 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 191 insertions(+) create mode 100644 main.cc diff --git a/Makefile b/Makefile index 6818a48..ae4cfeb 100644 --- a/Makefile +++ b/Makefile @@ -1,4 +1,6 @@ export TARGET=roomba_test export SOURCES=main.cc +export CXXFLAGS=-I/usr/include/qt4 -I/usr/include/qt4/QtCore \ + -I/usr/include/qt4/QtGui -lQtGui include ../Makefile.base diff --git a/main.cc b/main.cc new file mode 100644 index 0000000..1a181fe --- /dev/null +++ b/main.cc @@ -0,0 +1,189 @@ +/** + * @file main.cc + * @date 27 Nov 2010 + * Copyright © 2010 Roland Hieber + * + * Permission is hereby granted, free of charge, to any person obtaining + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +//#include "main.h" +#include +#include +//#include +//#include "util/standalone_math.h" +//#include "util/delegates/delegate.hpp" +#include +#include +#include +#include +#include +#include +#include + +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; +typedef wiselib::PCComUartModel RoombaUart; +typedef wiselib::RoombaModel Roomba; +typedef wiselib::ControlledMotion ControlledMotion; + +/** + * drive iterations. logs values to stdout. + */ +void drive(Roomba& roomba, ControlledMotion& ctrl_motion) { + int input_distance = 0, deviation_x = 0, deviation_y = 0, velocity = 100; + 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 = QInputDialog::getInt(0, "Input distance", chargeText + + "Input new distance in mm:", input_distance, + numeric_limits::min(), numeric_limits::max(), 1, &ok); + if(ok) { + // new turn velocity + velocity = QInputDialog::getInt(0, "Input velocity", chargeText + + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok); + if(ok) { + ctrl_motion.move_distance(input_distance, velocity); + roomba.wait_for_stop(); + } else { + break; + } + } else { + break; + } + + // measured deviation + deviation_x = QInputDialog::getInt(0, "Input x deviation", chargeText + + "Input travelled distance on x axis in mm:", deviation_x, + numeric_limits::min(), numeric_limits::max(), 1, &ok); + if(ok) { + deviation_y = QInputDialog::getInt(0, "Input y deviation", chargeText + + "Input travelled distance on y axis in mm:", deviation_y, + numeric_limits::min(), numeric_limits::max(), 1, &ok); + if(ok) { + 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; + } else { + break; + } + } else { + break; + } + } +} + +/** + * turn iterations. logs values to stdout. + */ +void turn(Roomba& roomba, ControlledMotion& ctrl_motion) { + int input_angle = 0, measured_angle = 0, velocity = 100; + bool ok = false; + + while(true) { + QString chargeText = QString(battery_string).arg(int(float(roomba().charge) + / float(roomba().capacity) * 100.0)); + + // new angle to turn about + input_angle = QInputDialog::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 = QInputDialog::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 { + break; + } + + // measured angle + measured_angle = QInputDialog::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 { + break; + } + } +} + +/** + * main function + */ +int main(int argc, char ** argv) { + + if(argc < 2) { + cerr << "Usage: " << argv[0] << " --turn|-t|--drive|-d" << endl; + 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); + + roomba_uart.set_baudrate(19200); + roomba_uart.enable_serial_comm(); + roomba.init(roomba_uart, timer, Roomba::POSITION + | Roomba::BATTERY_AND_TEMPERATURE); + + cout << "Got roomba at " << roomba_uart.address() << endl; + + roomba.reset_distance(); + roomba.reset_angle(); + ctrl_motion.init(roomba); + + // actual tests + if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) { + turn(roomba, ctrl_motion); + } else if(strcmp(argv[1], "--drive") == 0 || strcmp(argv[1], "-d") == 0) { + drive(roomba, ctrl_motion); + } +} -- 2.20.1