* THE SOFTWARE.
*/
-//#include "main.h"
+#include "svnrevision.h"
#include <iostream>
#include <stdint.h>
-//#include <unistd.h>
-//#include "util/standalone_math.h"
-//#include "util/delegates/delegate.hpp"
#include <external_interface/pc/pc_os_model.h>
#include <external_interface/pc/pc_com_uart.h>
#include <external_interface/pc/pc_timer.h>
#include <QApplication>
#include <QInputDialog>
+#if QT_VERSION < 0x040500
+#define getInt QInputDialog::getInteger
+#else // QT_VERSION > 0x040500
+#define getInt QInputDialog::getInt
+#endif // QT_VERSION
+
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::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(
+ sensor_data.charge) / float(sensor_data.capacity) * 100.0));
+}
+
/**
* drive iterations. logs values to stdout.
*/
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_distance = getInt(0, "Input distance", chargeText(roomba)
+ "Input new distance in mm:", input_distance,
numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
if(ok) {
// new turn velocity
- velocity = QInputDialog::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);
}
// measured deviation
- deviation_x = QInputDialog::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<int>::min(), numeric_limits<int>::max(), 1, &ok);
if(ok) {
- deviation_y = QInputDialog::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<int>::min(), numeric_limits<int>::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;
+ cout << "svn=" << SVNREVISION << " input_distance=" << input_distance
+ << " velocity=" << velocity << " internal_distance="
+ << roomba.distance() << " deviation_x=" << deviation_x
+ << " deviation_y=" << deviation_y << " 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;
}
* 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 = QInputDialog::getInt(0, "Input angle", chargeText
- + "Input new angle in degree to turn about:", input_angle,
- numeric_limits<int>::min(), numeric_limits<int>::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 {
+ // angle to turn about
+ turn_angle = getInt(0, "Input turn angle", chargeText(roomba)
+ + "Input angle in degree to turn about:", turn_angle,
+ numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
+ &ok);
+ if(!ok) {
break;
}
- // measured angle
- measured_angle = QInputDialog::getInt(0, "Input measured angle", chargeText
- + "Input measured angle in degree:", input_angle,
- numeric_limits<int>::min(), numeric_limits<int>::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, numeric_limits<int>::max(), 1, &ok);
+ if(!ok) {
break;
}
+
+ cout << "turn_angle=" << turn_angle << " measured_angle=" << measured_angle
+ << " velocity=" << velocity << " internal_angle=" << roomba.angle()
+ << " 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;
+
}
}
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.init(roomba_uart, timer, Roomba::POSITION
| Roomba::BATTERY_AND_TEMPERATURE);
- cout << "Got roomba at " << roomba_uart.address() << endl;
+ cerr << "Got roomba at " << roomba_uart.address() << endl;
roomba.reset_distance();
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);
projects / bachelor-thesis / roomba_tests.git / blobdiff