#else // QT_VERSION > 0x040500
#define getInt QInputDialog::getInt
#endif // QT_VERSION
-
// UART port on which we communicate with the Roomba
char uart[] = "/dev/ttyUSB0";
* Sensor data we need, filled in callback
*/
struct SensorData {
+
+ SensorData() :
+ capacity(0), charge(0), charging(0), current(0), temperature(0),
+ voltage(0), diff_left_ticks(0), diff_right_ticks(0) {
+ }
+
uint16_t capacity, charge;
uint8_t charging;
int16_t current;
int8_t temperature;
uint16_t voltage;
- int16_t left_encoder_counts, right_encoder_counts;
+ /** raw encoder counts; i.e. overflown, not consecutive */
+ int raw_left_ticks, raw_right_ticks;
+ /** absolute encoder counts; i.e. not overflown, but consecutive */
+ int diff_left_ticks, diff_right_ticks;
} 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 nearestDiff(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_ticks_left_, latest_ticks_right_;
+
+ DataAvailable() :
+ latest_ticks_left_(0), latest_ticks_right_(0) {
+ }
+
void cb(int state) {
if(state != Roomba::DATA_AVAILABLE) {
return;
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;
+ sensor_data.raw_left_ticks = roomba().left_encoder_counts;
+ sensor_data.raw_right_ticks = roomba().right_encoder_counts;
+ sensor_data.diff_left_ticks += nearestDiff(latest_ticks_left_,
+ roomba().left_encoder_counts);
+ latest_ticks_left_ = roomba().left_encoder_counts;
+ sensor_data.diff_right_ticks += nearestDiff(latest_ticks_right_,
+ roomba().right_encoder_counts);
+ latest_ticks_right_ = roomba().right_encoder_counts;
}
} data_available;
/**
* return battery status as QString
*/
-QString chargeText(Roomba& roomba) {
+QString chargeText() {
return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
sensor_data.charge) / float(sensor_data.capacity) * 100.0));
}
+/**
+ * return log text for global values
+ */
+QString logText() {
+ return QString("svn=%1 roomba_id=%2 ground_type=%3 diff_left_ticks=%4 "
+ "diff_right_ticks=%5 raw_ticks_left=%6 raw_ticks_right=%7 batt_charge=%8 "
+ "batt_capacity=%9 batt_voltage=%10 batt_current=%11").arg(SVNREVISION).arg(
+ roomba_id).arg(ground_type).arg(sensor_data.diff_left_ticks).arg(
+ sensor_data.diff_right_ticks).arg(sensor_data.raw_left_ticks).arg(
+ sensor_data.raw_right_ticks).arg(sensor_data.charge).arg(
+ sensor_data.capacity).arg(sensor_data.voltage).arg(sensor_data.current);
+}
+
/**
* drive iterations. logs values to stdout.
*/
while(true) {
// new distance to drive
- input_distance = getInt(0, "Input distance", chargeText(roomba)
+ input_distance = getInt(0, "Input distance", chargeText()
+ "Input new distance in mm:", input_distance,
numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
if(!ok) {
break;
}
// new velocity
- velocity = getInt(0, "Input velocity", chargeText(roomba)
+ velocity = getInt(0, "Input velocity", chargeText()
+ "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
if(!ok) {
break;
roomba.wait_for_stop();
// measured deviation
- deviation_x = getInt(0, "Input x deviation", chargeText(roomba)
+ deviation_x = getInt(0, "Input x deviation", chargeText()
+ "Input travelled distance on x axis in mm:", deviation_x,
numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
if(!ok) {
break;
}
- deviation_y = getInt(0, "Input y deviation", chargeText(roomba)
+ deviation_y = getInt(0, "Input y deviation", chargeText()
+ "Input travelled distance on y axis in mm:", deviation_y,
numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
if(!ok) {
break;
}
- cout << "svn=" << SVNREVISION << " roomba_id=" << roomba_id
- << " move=straight" << " ground_type=" << ground_type
- << " 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;
+ cout << logText().toAscii().constData() << " move=straight input_distance="
+ << input_distance << " velocity=" << velocity << " internal_distance="
+ << roomba.distance() << " deviation_x=" << deviation_x << " deviation_y="
+ << deviation_y << endl;
+
+ // reset, because we only need the difference between two drive commands
+ sensor_data.diff_left_ticks = 0;
+ sensor_data.diff_right_ticks = 0;
}
}
bool ok = false;
// current angle
- cur_angle = getInt(0, "Input current orientation", chargeText(roomba)
+ cur_angle = getInt(0, "Input current orientation", chargeText()
+ "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok);
if(!ok) {
return;
while(true) {
// new turn velocity
- velocity = getInt(0, "Input velocity", chargeText(roomba)
+ velocity = getInt(0, "Input velocity", chargeText()
+ "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
if(!ok) {
break;
}
// angle to turn about
- turn_angle = getInt(0, "Input turn angle", chargeText(roomba)
+ turn_angle = getInt(0, "Input turn angle", chargeText()
+ "Input angle in degree to turn about:", turn_angle,
numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
&ok);
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 "
+ measured_angle = getInt(0, "Input measured angle", chargeText() + 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 << "svn=" << SVNREVISION << " roomba_id=" << roomba_id
- << " move=straight" << " ground_type=" << ground_type << "turn_angle="
+ cout << logText().toAscii().constData() << " move=turn 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;
+ << velocity << " internal_angle=" << roomba.angle() << endl;
+
+ // reset, because we only need the difference between two turns
+ sensor_data.diff_left_ticks = 0;
+ sensor_data.diff_right_ticks = 0;
// new orientation
cur_angle = (cur_angle + measured_angle) % 360;
projects / bachelor-thesis / roomba_tests.git / blobdiff