From f4b9567ab8a48c42c58bc4c5caedbd7b6672bc8d Mon Sep 17 00:00:00 2001 From: svnhieber Date: Tue, 18 Jan 2011 18:19:04 +0000 Subject: [PATCH] some minor refactoring git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@4091 f8795833-4959-0410-8ae9-8bcb0cfab428 --- main.cc | 89 ++++++++++++++++++++++++++++++--------------------------- 1 file changed, 47 insertions(+), 42 deletions(-) diff --git a/main.cc b/main.cc index 6051236..b598a4e 100644 --- a/main.cc +++ b/main.cc @@ -67,7 +67,7 @@ struct SensorData { SensorData() : capacity(0), charge(0), charging(0), current(0), temperature(0), - voltage(0), abs_left_encoder_counts(0), abs_right_encoder_counts(0) { + voltage(0), diff_left_ticks(0), diff_right_ticks(0) { } uint16_t capacity, charge; @@ -75,8 +75,10 @@ struct SensorData { int16_t current; int8_t temperature; uint16_t voltage; + /** 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 abs_left_encoder_counts, abs_right_encoder_counts; + int diff_left_ticks, diff_right_ticks; } sensor_data; /** @@ -85,7 +87,7 @@ struct SensorData { * This is useful if you have an overflowing counter and you want to determine * when you have to "wrap over" the value. */ -int nearest_diff(unsigned short last, unsigned short current) { +int nearestDiff(unsigned short last, unsigned short current) { int d = current - last; if(d < -0x8000) { // overflow in positive direction d = (0x10000 - last + current); @@ -99,10 +101,10 @@ int nearest_diff(unsigned short last, unsigned short current) { * Callback that fills the sensor data when data is available */ struct DataAvailable { - int latest_encoder_left_, latest_encoder_right_; + int latest_ticks_left_, latest_ticks_right_; DataAvailable() : - latest_encoder_left_(0), latest_encoder_right_(0) { + latest_ticks_left_(0), latest_ticks_right_(0) { } void cb(int state) { @@ -114,23 +116,38 @@ struct DataAvailable { sensor_data.charging = roomba().charging; sensor_data.current = roomba().current; sensor_data.voltage = roomba().voltage; - sensor_data.abs_left_encoder_counts += nearest_diff(latest_encoder_left_, + 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_encoder_left_ = roomba().left_encoder_counts; - sensor_data.abs_right_encoder_counts += nearest_diff(latest_encoder_right_, + latest_ticks_left_ = roomba().left_encoder_counts; + sensor_data.diff_right_ticks += nearestDiff(latest_ticks_right_, roomba().right_encoder_counts); - latest_encoder_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. */ @@ -141,14 +158,14 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) { 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::min(), numeric_limits::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; @@ -157,32 +174,26 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) { 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::min(), numeric_limits::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::min(), numeric_limits::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 - << " abs_encoder_ticks_left=" << sensor_data.abs_left_encoder_counts - << " abs_encoder_ticks_right=" << sensor_data.abs_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.abs_left_encoder_counts = 0; - sensor_data.abs_right_encoder_counts = 0; + sensor_data.diff_left_ticks = 0; + sensor_data.diff_right_ticks = 0; } } @@ -194,7 +205,7 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) { 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; @@ -202,14 +213,14 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) { 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::min() + 360, numeric_limits::max() - 360, 1, &ok); @@ -221,27 +232,21 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) { 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::max(), 1, &ok); if(!ok) { break; } - cout << "svn=" << SVNREVISION << " roomba_id=" << roomba_id - << " move=turn " << " 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() - << " abs_encoder_ticks_left=" << sensor_data.abs_left_encoder_counts - << " abs_encoder_ticks_right=" << sensor_data.abs_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.abs_left_encoder_counts = 0; - sensor_data.abs_right_encoder_counts = 0; + sensor_data.diff_left_ticks = 0; + sensor_data.diff_right_ticks = 0; // new orientation cur_angle = (cur_angle + measured_angle) % 360; -- 2.20.1