[Fawkes Git] branch/timn/robotino-direct: 2 revs updated. (0.5.0-3094-g65f0da0)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Tue Apr 12 22:54:30 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, timn/robotino-direct has been updated
        to  65f0da04d5666dec75b0c1931147de84bbb25ca9 (commit)
       via  25cb4fc4bf22a28bb78cf0b5a64cee0e5b98dd3e (commit)
      from  1bcb9f08a375c4b148c5a474d55ebb36ae6407e2 (commit)

http://git.fawkesrobotics.org/fawkes.git/timn/robotino-direct

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 25cb4fc4bf22a28bb78cf0b5a64cee0e5b98dd3e
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Tue Apr 12 11:04:49 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Tue Apr 12 11:04:49 2016 +0200

    robotino: update configuration layout
    
    Clearly separate driver-specific settings. Add more settings, e.g., for
    the motor layout. Adapt threads to read correct values.

http://git.fawkesrobotics.org/fawkes.git/commit/25cb4fc
http://trac.fawkesrobotics.org/changeset/25cb4fc

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 65f0da04d5666dec75b0c1931147de84bbb25ca9
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Tue Apr 12 22:48:08 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Tue Apr 12 22:48:08 2016 +0200

    robotino: add velocity controller and safety checks
    
    Restructure the module to move the drive execution into the (faster
    timed) com thread. Add a simple linear controller for the velocities to
    smooth the driving behavior. Add safety checks and bounds on velocities
    of the omni drive model and the motor RPMs.
    
    The overall behavior in driving is much smoother and improved.
    
    I have seen more transmission errors. Some improvements to error
    handling have been made. Investigating further why the connection errors
    increased seems worthwhile.

http://git.fawkesrobotics.org/fawkes.git/commit/65f0da0
http://trac.fawkesrobotics.org/changeset/65f0da0

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 cfg/conf.d/robotino.yaml                         |   91 +++++++-
 src/plugins/robotino/act_thread.cpp              |  134 +++---------
 src/plugins/robotino/act_thread.h                |   23 +-
 src/plugins/robotino/com_thread.cpp              |  237 +++++++++++++++++++
 src/plugins/robotino/com_thread.h                |   62 ++++-
 src/plugins/robotino/direct_com_thread.cpp       |  274 +++++++++++++++-------
 src/plugins/robotino/direct_com_thread.h         |   20 +-
 src/plugins/robotino/openrobotino_com_thread.cpp |   13 +-
 src/plugins/robotino/openrobotino_com_thread.h   |   12 +-
 9 files changed, 617 insertions(+), 249 deletions(-)


- *Diffs* -------------------------------------------------------------

- *commit* 25cb4fc4bf22a28bb78cf0b5a64cee0e5b98dd3e - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Tue Apr 12 11:04:49 2016 +0200
Subject: robotino: update configuration layout

 cfg/conf.d/robotino.yaml                         |   60 +++++++++++++++++----
 src/plugins/robotino/act_thread.cpp              |    2 +-
 src/plugins/robotino/direct_com_thread.cpp       |    2 +-
 src/plugins/robotino/openrobotino_com_thread.cpp |    6 +-
 4 files changed, 53 insertions(+), 17 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/robotino.yaml b/cfg/conf.d/robotino.yaml
index 086956e..ab4cff5 100644
--- a/cfg/conf.d/robotino.yaml
+++ b/cfg/conf.d/robotino.yaml
@@ -4,24 +4,59 @@
 doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/robotino
 ---
 hardware/robotino:
-  # Host to connect to
-  hostname: 127.0.0.1
 
-  # If true quit Fawkes on disconnect from Robotino Com server, otherwise
-  # retry connection. If run from LCD menu should be set to true.
-  quit_on_disconnect: true
+  # Hardware access driver mode, can be one of:
+  # - direct: directly communicate with Robotino base through USB
+  #           NOTE: disable controld3! This has only been tested
+  #           on Robotino 3 (firmware 1.1.1)
+  # - openrobotino: use OpenRobotino API to communicate with base
+  #                 This compiles with API 1 or 2 (preferring 2).
+  driver: direct
 
-  # Time threshold after all motor velocities are set to zero if no new messages arrived inbetween
-  deadman_time_threshold: 0.5
+  # *** Common Settings for all drivers
 
-  # Base coordinate frame
-  base_frame: !frame base_link
-
-  # Minimum time in milliseconds between two Robotino sensor readings.
+  # Minimum time in milliseconds between two data updates.
   # Lower values increase the machine load (considerably) but decrease
   # the average sensor data age. Zero means receive as fast as possible
   # without waiting between receive calls
-  sensor_update_cycle_time: 5
+  cycle-time: 5
+
+  act:
+    # Time threshold after all motor velocities are set to zero if no new messages arrived inbetween
+    deadman_time_threshold: 0.5
+
+  openrobotino:
+    # Configuration settings specific to the OpenRobotino driver
+
+    # Host to connect to
+    hostname: 127.0.0.1
+
+    # If true quit Fawkes on disconnect from Robotino Com server, otherwise
+    # retry connection. If run from LCD menu should be set to true.
+    quit_on_disconnect: true
+
+  direct:
+    # Configuration settings specific to the direct comm driver
+
+    # Path to device file to access base at
+    # It is strongly recommended NOT to set this. In that case, the plugin will
+    # introspect the system for a matching USB device and determine the device
+    # path automatically. This is most often the better way.
+    # A typical problem is not that the device cannot be determined, but rather
+    # that the user running fawkes has no permission to access such device. In
+    # particular, on the default Ubuntu installation add the user robotino to
+    # the group dialout: usermod -a -G dialout robotino (and reboot)
+    # device: /dev/ttyACM1
+
+  motor-layout:
+    # Distance from Robotino center to wheel center in meters
+    rb: 0.175
+
+    # Wheel radius in meters
+    rw: 0.060
+
+    # Gear ratio between motors and wheels
+    gear: 32.0
 
   bumper:
     # Set to true to enable the emergency stop on bumper contact (provided
@@ -52,6 +87,7 @@ hardware/robotino:
     # robotino plugin and instead use the imu plugin with the gyro connected
     # directly to the Fawkes machine.
     mode: calc
+    #mode: copy
 
     # Odometry coordinate frame
     frame: !frame odom
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index 74d71ad..e33e535 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -64,7 +64,7 @@ RobotinoActThread::init()
 	cfg_bumper_estop_enabled_ = config->get_bool("/hardware/robotino/bumper/estop_enabled");
 	cfg_odom_time_offset_     = config->get_float("/hardware/robotino/odometry/time_offset");
 	cfg_odom_frame_           = config->get_string("/hardware/robotino/odometry/frame");
-	cfg_base_frame_           = config->get_string("/hardware/robotino/base_frame");
+	cfg_base_frame_           = config->get_string("/frames/base");
 	std::string odom_mode     = config->get_string("/hardware/robotino/odometry/mode");
 	cfg_odom_corr_phi_        =
 		config->get_float("/hardware/robotino/odometry/calc/correction/phi");
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index 4bfe0a2..7425ad0 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -67,7 +67,7 @@ DirectRobotinoComThread::init()
 {
 	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
 	cfg_sensor_update_cycle_time_ =
-		config->get_uint("/hardware/robotino/sensor_update_cycle_time");
+		config->get_uint("/hardware/robotino/cycle-time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
 
 	// -------------------------------------------------------------------------- //
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index d9178e8..4b2492b 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -73,10 +73,10 @@ OpenRobotinoComThread::~OpenRobotinoComThread()
 void
 OpenRobotinoComThread::init()
 {
-	cfg_hostname_ = config->get_string("/hardware/robotino/hostname");
-	cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/quit_on_disconnect");
+	cfg_hostname_ = config->get_string("/hardware/robotino/openrobotino/hostname");
+	cfg_quit_on_disconnect_ = config->get_bool("/hardware/robotino/openrobotino/quit_on_disconnect");
 	cfg_sensor_update_cycle_time_ =
-		config->get_uint("/hardware/robotino/sensor_update_cycle_time");
+		config->get_uint("/hardware/robotino/cycle-time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
 	cfg_enable_gyro_ = config->get_bool("/hardware/robotino/gyro/enable");
 

- *commit* 65f0da04d5666dec75b0c1931147de84bbb25ca9 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Tue Apr 12 22:48:08 2016 +0200
Subject: robotino: add velocity controller and safety checks

 cfg/conf.d/robotino.yaml                         |   33 +++-
 src/plugins/robotino/act_thread.cpp              |  132 +++---------
 src/plugins/robotino/act_thread.h                |   23 +-
 src/plugins/robotino/com_thread.cpp              |  237 +++++++++++++++++++
 src/plugins/robotino/com_thread.h                |   62 ++++-
 src/plugins/robotino/direct_com_thread.cpp       |  272 +++++++++++++++-------
 src/plugins/robotino/direct_com_thread.h         |   20 +-
 src/plugins/robotino/openrobotino_com_thread.cpp |    7 +
 src/plugins/robotino/openrobotino_com_thread.h   |   12 +-
 9 files changed, 565 insertions(+), 233 deletions(-)

_Diff for modified files_:
diff --git a/cfg/conf.d/robotino.yaml b/cfg/conf.d/robotino.yaml
index ab4cff5..43c182b 100644
--- a/cfg/conf.d/robotino.yaml
+++ b/cfg/conf.d/robotino.yaml
@@ -48,7 +48,20 @@ hardware/robotino:
     # the group dialout: usermod -a -G dialout robotino (and reboot)
     # device: /dev/ttyACM1
 
-  motor-layout:
+
+  # The motor and drive settings and limits are enforced for direct
+  # communication, for the openrobotino driver, these values are
+  # checked by controld3, cf. /etc/robotino/controld3.conf on the
+  # robotino base
+
+  motor:
+    # Minimum and maximum acceleration of motors
+    acceleration-limits: [-11500, 11500]
+
+    rpm-max: 3000
+  
+  drive:
+    layout:
     # Distance from Robotino center to wheel center in meters
     rb: 0.175
 
@@ -58,6 +71,14 @@ hardware/robotino:
     # Gear ratio between motors and wheels
     gear: 32.0
 
+    # Maximum acceleration and deceleration for translation, m/s^2
+    trans-acceleration: 0.4
+    trans-deceleration: 0.4
+
+    # Maximum acceleration and deceleration for rotation, deg/s^2
+    rot-acceleration: 0.4
+    rot-deceleration: 0.4
+
   bumper:
     # Set to true to enable the emergency stop on bumper contact (provided
     # by OpenRobotino, or false to keep going (warning, you should handle
@@ -120,3 +141,13 @@ hardware/robotino:
 
     # ID of the Gripper Interface
     gripper_id: Robotino
+
+    # Maximum current in A for the motor output.
+    # If the current is larger than maxcurrent for more than
+    # max-time milliseconds the set-point is set to 0. To recover
+    # from the over current situation decrease the set-point in your
+    # application below the set-point when the over current flag had
+    # been set
+    # Currently not implemented
+    #current-max: 1
+    #current-max-time: 400
diff --git a/src/plugins/robotino/act_thread.cpp b/src/plugins/robotino/act_thread.cpp
index e33e535..18642b3 100644
--- a/src/plugins/robotino/act_thread.cpp
+++ b/src/plugins/robotino/act_thread.cpp
@@ -63,7 +63,7 @@ RobotinoActThread::init()
 	cfg_gripper_enabled_      = config->get_bool("/hardware/robotino/gripper/enable_gripper");
 	cfg_bumper_estop_enabled_ = config->get_bool("/hardware/robotino/bumper/estop_enabled");
 	cfg_odom_time_offset_     = config->get_float("/hardware/robotino/odometry/time_offset");
-	cfg_odom_frame_           = config->get_string("/hardware/robotino/odometry/frame");
+	cfg_odom_frame_           = config->get_string("/frames/odom");
 	cfg_base_frame_           = config->get_string("/frames/base");
 	std::string odom_mode     = config->get_string("/hardware/robotino/odometry/mode");
 	cfg_odom_corr_phi_        =
@@ -71,9 +71,18 @@ RobotinoActThread::init()
 	cfg_odom_corr_trans_      =
 		config->get_float("/hardware/robotino/odometry/calc/correction/trans");
 
-	cfg_rb_   = config->get_float("/hardware/robotino/motor-layout/rb");
-	cfg_rw_   = config->get_float("/hardware/robotino/motor-layout/rw");
-	cfg_gear_ = config->get_float("/hardware/robotino/motor-layout/gear");
+	cfg_rb_   = config->get_float("/hardware/robotino/drive/layout/rb");
+	cfg_rw_   = config->get_float("/hardware/robotino/drive/layout/rw");
+	cfg_gear_ = config->get_float("/hardware/robotino/drive/layout/gear");
+
+	cfg_trans_accel_ = config->get_float("/hardware/robotino/drive/trans-acceleration");
+	cfg_trans_decel_ = config->get_float("/hardware/robotino/drive/trans-deceleration");
+	cfg_rot_accel_   = config->get_float("/hardware/robotino/drive/rot-acceleration");
+	cfg_rot_decel_   = config->get_float("/hardware/robotino/drive/rot-deceleration");
+
+	com_->set_drive_layout(cfg_rb_, cfg_rw_, cfg_gear_);
+	com_->set_drive_limits(cfg_trans_accel_, cfg_trans_decel_,
+	                       cfg_rot_accel_, cfg_rot_decel_);
 
 	std::string imu_if_id;
 
@@ -95,10 +104,6 @@ RobotinoActThread::init()
 	msg_received_ = false;
 	msg_zero_vel_ = false;
 
-	des_vx_    = 0.;
-	des_vy_    = 0.;
-	des_omega_ = 0.;
-
 	odom_x_ = odom_y_ = odom_phi_ = 0.;
 	odom_time_ = new Time(clock);
 
@@ -155,10 +160,8 @@ RobotinoActThread::loop()
 		return;
 	}
 
-	bool set_speed_points = false;
-	float s1 = 0., s2 = 0., s3 = 0.;
 	bool reset_odometry = false;
-
+	bool set_des_vel = false;
 	while (! motor_if_->msgq_empty()) {
 		if (MotorInterface::SetMotorStateMessage *msg = motor_if_->msgq_first_safe(msg))
 		{
@@ -171,18 +174,15 @@ RobotinoActThread::loop()
 
 		else if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg))
 		{
-			project(&s1, &s2, &s3, msg->vx(), msg->vy(), msg->omega());
-
 			des_vx_    = msg->vx();
 			des_vy_    = msg->vy();
 			des_omega_ = msg->omega();
 
-			set_speed_points = true;
-        
 			last_msg_time_ = clock->now();
-			msg_received_ = true;
-	
-			msg_zero_vel_ = (s1 == 0.0 && s2 == 0.0 && s3 == 0.0);
+			msg_received_ = true;	
+
+			set_des_vel = true;
+			msg_zero_vel_ = (des_vx_ == 0.0 && des_vy_ == 0.0 && des_omega_ == 0.0);
 		}
 
 		else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())
@@ -226,21 +226,22 @@ RobotinoActThread::loop()
 	if (diff >= cfg_deadman_threshold_ && msg_received_ && !msg_zero_vel_) {
 		logger->log_error(name(), "Time-Gap between TransRotMsgs too large "
 		                  "(%f sec.), motion planner alive?", diff);
-		s1 = s2 = s3 = 0.;
-		set_speed_points = true;
+		des_vx_ = des_vy_ = des_omega_ = 0.;
+		msg_zero_vel_ = true;
+		set_des_vel = true;
 		msg_received_ = false;
 	}
 
 	if (motor_if_->motor_state() == MotorInterface::MOTOR_DISABLED) {
-		if (set_speed_points && ((s1 != 0.0) || (s2 != 0.0) || (s3 != 0.0))) {
+		if (set_des_vel && ((des_vx_ != 0.0) || (des_vy_ != 0.0) || (des_omega_ != 0.0))) {
 			logger->log_warn(name(), "Motor command received while disabled, ignoring");
 		}
-		s1 = s2 = s3 = 0.;
-		set_speed_points = true;
+		des_vx_ = des_vy_ = des_omega_ = 0.;
+		set_des_vel = true;
 	}
 
-	if (set_speed_points)  com_->set_speed_points(s1, s2, s3);
-	if (reset_odometry)    com_->reset_odometry();
+	if (reset_odometry)  com_->reset_odometry();
+	if (set_des_vel)     com_->set_desired_vel(des_vx_, des_vy_, des_omega_);
 
 	publish_odometry();
 
@@ -263,7 +264,7 @@ RobotinoActThread::publish_odometry()
 		last_seqnum_ = seq;
 
 		float vx = 0., vy = 0., omega = 0.;
-		unproject(&vx, &vy, &omega, a1, a2, a3);
+		com_->unproject(&vx, &vy, &omega, a1, a2, a3);
 
 		motor_if_->set_vx(vx);
 		motor_if_->set_vy(vy);
@@ -410,82 +411,3 @@ RobotinoActThread::publish_gripper()
 	}
 }
 
-/** Project the velocity of the robot in cartesian coordinates to single motor speeds.
- *
- * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
- * The code has been released under a 2-clause BSD license.
- *
- * @param m1		The resulting speed of motor 1 in rpm
- * @param m2		The resulting speed of motor 2 in rpm
- * @param m3		The resulting speed of motor 3 in rpm
- * @param vx		Velocity in x-direction in m/s
- * @param vy		Velocity in y-direction in m/s
- * @param omega	Angular velocity in rad/s
- */
-//Redistribution and use in source and binary forms, with or without
-//modification, are permitted provided that the following conditions
-//are met:
-//1) Redistributions of source code must retain the above copyright
-//notice, this list of conditions and the following disclaimer.
-//2) Redistributions in binary form must reproduce the above copyright
-//notice, this list of conditions and the following disclaimer in the
-//documentation and/or other materials provided with the distribution.
-//
-//THIS SOFTWARE IS PROVIDED BY REC ROBOTICS EQUIPMENT CORPORATION GMBH
-//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL REC
-//ROBOTICS EQUIPMENT CORPORATION GMBH BE LIABLE FOR ANY DIRECT,
-//INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-//SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
-//HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
-//STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
-//OF THE POSSIBILITY OF SUCH DAMAGE.
-void
-RobotinoActThread::project( float* m1, float* m2, float* m3, float vx, float vy, float omega ) const
-{
-	//Projection matrix
-	static const double v0[2] = { -0.5 * sqrt( 3.0 ),  0.5 };
-	static const double v1[2] = {  0.0              , -1.0 };
-	static const double v2[2] = {  0.5 * sqrt( 3.0 ),  0.5 };
-
-	//Scale omega with the radius of the robot
-	double vOmegaScaled = cfg_rb_ * (double)omega ;
-
-	//Convert from m/s to RPM
-	const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
-
-	//Compute the desired velocity
-	*m1 = static_cast<float>( ( v0[0] * (double)vx + v0[1] * (double)vy + vOmegaScaled ) * k );
-	*m2 = static_cast<float>( ( v1[0] * (double)vx + v1[1] * (double)vy + vOmegaScaled ) * k );
-	*m3 = static_cast<float>( ( v2[0] * (double)vx + v2[1] * (double)vy + vOmegaScaled ) * k );
-}
-
-/** Project single motor speeds to velocity in cartesian coordinates.
- *
- * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
- * The code has been released under a 2-clause BSD license.
- *
- * @param vx		The resulting speed in x-direction in m/s
- * @param vy		The resulting speed in y-direction in m/s
- * @param omega	The resulting angular velocity in rad/s
- * @param m1		Speed of motor 1 in rpm
- * @param m2		Speed of motor 2 in rpm
- * @param m3		Speed of motor 3 in rpm
- * @throws		RobotinoException if no valid drive layout parameters are available.
- */
-void
-RobotinoActThread::unproject( float* vx, float* vy, float* omega, float m1, float m2, float m3 ) const
-{
-	//Convert from RPM to mm/s
-	const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
-
-	*vx = static_cast<float>( ( (double)m3 - (double)m1 ) / sqrt( 3.0 ) / k );
-	*vy = static_cast<float>( 2.0 / 3.0 * ( (double)m1 + 0.5 * ( (double)m3 - (double)m1 ) - (double)m2 ) / k );
-
-	double vw = (double)*vy + (double)m2 / k;
-
-	*omega = static_cast<float>( vw / cfg_rb_ );
-}
diff --git a/src/plugins/robotino/act_thread.h b/src/plugins/robotino/act_thread.h
index 2e099e4..64dd26e 100644
--- a/src/plugins/robotino/act_thread.h
+++ b/src/plugins/robotino/act_thread.h
@@ -75,9 +75,6 @@ class RobotinoActThread
 	void publish_odometry();
 	void publish_gripper();
 
-	void project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const;
-	void unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const;
-	
  private:
 	RobotinoComThread              *com_;
 
@@ -93,23 +90,23 @@ class RobotinoActThread
 	bool        			  msg_zero_vel_;
 	fawkes::Time 			  last_msg_time_;
 
-	float        			  cfg_deadman_threshold_;
-	float        			  cfg_odom_time_offset_;
-	bool 				  cfg_gripper_enabled_;
+	float        			              cfg_deadman_threshold_;
+	float        			              cfg_odom_time_offset_;
+	bool 				                    cfg_gripper_enabled_;
 	std::string                     cfg_odom_frame_;
 	std::string                     cfg_base_frame_;
 	OdometryMode                    cfg_odom_mode_;
 	unsigned int                    cfg_imu_deadman_loops_;
 	float                           cfg_odom_corr_phi_;
 	float                           cfg_odom_corr_trans_;
-	bool                            cfg_bumper_estop_enabled_;
+	bool                            cfg_bumper_estop_enabled_;	
 	float                           cfg_rb_;
 	float                           cfg_rw_;
 	float                           cfg_gear_;
-	
-	float                           des_vx_;
-	float                           des_vy_;
-	float                           des_omega_;
+	float                           cfg_trans_accel_;
+	float                           cfg_trans_decel_;
+	float                           cfg_rot_accel_;
+	float                           cfg_rot_decel_;
 
 	bool                            gripper_close_;
 
@@ -118,6 +115,10 @@ class RobotinoActThread
 	float                           odom_phi_;
 	float                           odom_gyro_origin_;
 	fawkes::Time                   *odom_time_;
+
+	float                           des_vx_;
+	float                           des_vy_;
+	float                           des_omega_;
 };
 
 
diff --git a/src/plugins/robotino/com_thread.cpp b/src/plugins/robotino/com_thread.cpp
index 8ac72bf..6936d08 100644
--- a/src/plugins/robotino/com_thread.cpp
+++ b/src/plugins/robotino/com_thread.cpp
@@ -71,6 +71,11 @@ using namespace fawkes;
  * @fn void RobotinoComThread::set_bumper_estop_enabled(bool enabled) = 0
  * Enable or disable emergency stop on bumper contact.
  * @param enabled true to enable, false to disable
+ *
+ * @fn void set_motor_accel_limits(float min_accel, float max_accel) = 0
+ * Set acceleration limits of motors.
+ * @param min_accel minimum acceleration
+ * @param max_accel maximum acceleration
  */
 
 /** @class RobotinoComThread::SensorData "com_thread.h"
@@ -96,6 +101,31 @@ RobotinoComThread::RobotinoComThread(const char *thread_name)
 {
 	data_mutex_  = new Mutex();
 	new_data_    = false;
+
+	vel_mutex_ = new Mutex();
+	vel_last_update_ = new Time();
+	vel_last_zero_ = false;
+	des_vx_    = 0.;
+	des_vy_    = 0.;
+	des_omega_ = 0.;
+
+	set_vx_    = 0.;
+	set_vy_    = 0.;
+	set_omega_ = 0.;
+
+	cfg_rb_   = 0.;
+	cfg_rw_   = 0.;
+	cfg_gear_ = 0.;	
+
+	cfg_trans_accel_ = 0.;
+	cfg_trans_decel_ = 0.;
+	cfg_rot_accel_   = 0.;
+	cfg_rot_decel_   = 0.;
+
+#ifdef USE_VELOCITY_RECORDING
+	f_ = fopen("comdata.csv", "w");
+	start_ = new Time();
+#endif
 }
 
 
@@ -103,6 +133,9 @@ RobotinoComThread::RobotinoComThread(const char *thread_name)
 RobotinoComThread::~RobotinoComThread()
 {
 	delete data_mutex_;
+#ifdef USE_VELOCITY_RECORDING
+	fclose(f_);
+#endif
 }
 
 
@@ -124,3 +157,207 @@ RobotinoComThread::get_data(SensorData &sensor_data)
 		return false;
 	}
 }
+
+
+void
+RobotinoComThread::set_drive_layout(float rb, float rw, float gear)
+{
+	cfg_rb_   = rb;
+	cfg_rw_   = rw;
+	cfg_gear_ = gear;	
+}
+
+
+void
+RobotinoComThread::set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel)
+{
+	cfg_trans_accel_ = trans_accel;
+	cfg_trans_decel_ = trans_decel;
+	cfg_rot_accel_   = rot_accel;
+	cfg_rot_decel_   = rot_decel;
+}
+
+void
+RobotinoComThread::set_desired_vel(float vx, float vy, float omega)
+{
+	des_vx_    = vx;
+	des_vy_    = vy;
+	des_omega_ = omega;
+}
+
+
+bool
+RobotinoComThread::update_velocities()
+{
+	bool set_speed = false;
+
+	Time now(clock);
+	float diff_sec = now - vel_last_update_;
+	*vel_last_update_ = now;
+
+	set_vx_    = update_speed(des_vx_, set_vx_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
+	set_vy_    = update_speed(des_vy_, set_vy_, cfg_trans_accel_, cfg_trans_decel_, diff_sec);
+	set_omega_ = update_speed(des_omega_, set_omega_, cfg_rot_accel_, cfg_rot_decel_, diff_sec);
+
+	/*
+	logger->log_info(name(), "VX: %.2f -> %.2f (%.2f)   VY: %.2f -> %.2f (%.2f)   Omg: %.2f -> %.2f (%.2f)",
+	                 old_set_vx, set_vx_, des_vx_,
+	                 old_set_vy, set_vy_, des_vy_,
+	                 old_set_omega, set_omega_);
+	*/
+
+	if (set_vx_ == 0.0 && set_vy_ == 0.0 && set_omega_ == 0.0) {
+		if (! vel_last_zero_) {
+			set_speed = true;
+			vel_last_zero_ = true;
+		}
+	} else {
+		set_speed = true;
+		vel_last_zero_ = false;
+	}
+
+	if (set_speed) {
+		float s1 = 0., s2 = 0., s3 = 0.;
+		project(&s1, &s2, &s3, set_vx_, set_vy_, set_omega_);
+		set_speed_points(s1, s2, s3);
+
+#ifdef USE_VELOCITY_RECORDING
+		{
+			Time now(clock);
+			float time_diff = now - start_;
+	
+			fprintf(f_, "%f\t%f\t%f\t%f\t%f\t%f\t%f\n", time_diff,
+			        des_vx_, set_vx_, des_vy_, set_vy_, des_omega_, set_omega_);
+		}
+#endif
+
+	}
+
+	return ! vel_last_zero_;
+}
+
+float
+RobotinoComThread::update_speed(float des, float set, float accel, float decel, float diff_sec)
+{
+	if (des >= 0 && set < 0) {
+		const float decrement = std::copysign(decel, set) * diff_sec;
+		if (des > set - decrement) {
+			//logger->log_debug(name(), "    Case 1a  %f  %f  %f", decrement, decel, diff_sec);
+			set -= decrement;
+		} else {
+			//logger->log_debug(name(), "    Case 1b");
+			set = des;
+		}
+
+	} else if (des <= 0 && set > 0) {
+		const float decrement = std::copysign(decel, set) * diff_sec;
+		if (des < set - decrement ) {
+			//logger->log_debug(name(), "    Case 1c  %f  %f  %f", decrement, decel, diff_sec);
+			set -= decrement;
+		} else {
+			//logger->log_debug(name(), "    Case 1d");
+			set = des;
+		}
+
+	} else if (fabs(des) > fabs(set)) {
+		const float increment = std::copysign(accel, des) * diff_sec;
+		if (fabs(des) > fabs(set + increment)) {
+			//logger->log_debug(name(), "    Case 2a  %f  %f", increment, accel, diff_sec);
+			set += increment;
+		} else {
+			//logger->log_debug(name(), "    Case 2b");
+			set  = des;
+		}
+	} else if (fabs(des) < fabs(set)) {
+		const float decrement = std::copysign(decel, des) * diff_sec;
+		if (fabs(des) < fabs(set - decrement)) {
+			//logger->log_debug(name(), "    Case 3a  %f  %f  %f", decrement, decel, diff_sec);
+			set -= decrement;
+		} else {
+			//logger->log_debug(name(), "    Case 3b");
+			set  = des;
+		}
+	}
+
+	return set;
+}
+
+/** Project the velocity of the robot in cartesian coordinates to single motor speeds.
+ *
+ * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
+ * The code has been released under a 2-clause BSD license.
+ *
+ * @param m1		The resulting speed of motor 1 in rpm
+ * @param m2		The resulting speed of motor 2 in rpm
+ * @param m3		The resulting speed of motor 3 in rpm
+ * @param vx		Velocity in x-direction in m/s
+ * @param vy		Velocity in y-direction in m/s
+ * @param omega	Angular velocity in rad/s
+ */
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+//1) Redistributions of source code must retain the above copyright
+//notice, this list of conditions and the following disclaimer.
+//2) Redistributions in binary form must reproduce the above copyright
+//notice, this list of conditions and the following disclaimer in the
+//documentation and/or other materials provided with the distribution.
+//
+//THIS SOFTWARE IS PROVIDED BY REC ROBOTICS EQUIPMENT CORPORATION GMBH
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL REC
+//ROBOTICS EQUIPMENT CORPORATION GMBH BE LIABLE FOR ANY DIRECT,
+//INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+//SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+//HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+//STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
+//OF THE POSSIBILITY OF SUCH DAMAGE.
+void
+RobotinoComThread::project( float* m1, float* m2, float* m3, float vx, float vy, float omega ) const
+{
+	//Projection matrix
+	static const double v0[2] = { -0.5 * sqrt( 3.0 ),  0.5 };
+	static const double v1[2] = {  0.0              , -1.0 };
+	static const double v2[2] = {  0.5 * sqrt( 3.0 ),  0.5 };
+
+	//Scale omega with the radius of the robot
+	double vOmegaScaled = cfg_rb_ * (double)omega ;
+
+	//Convert from m/s to RPM
+	const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
+
+	//Compute the desired velocity
+	*m1 = static_cast<float>( ( v0[0] * (double)vx + v0[1] * (double)vy + vOmegaScaled ) * k );
+	*m2 = static_cast<float>( ( v1[0] * (double)vx + v1[1] * (double)vy + vOmegaScaled ) * k );
+	*m3 = static_cast<float>( ( v2[0] * (double)vx + v2[1] * (double)vy + vOmegaScaled ) * k );
+}
+
+/** Project single motor speeds to velocity in cartesian coordinates.
+ *
+ * From OpenRobotino API2 (C) REC Robotics Equipment Corporation GmbH, Planegg, Germany.
+ * The code has been released under a 2-clause BSD license.
+ *
+ * @param vx		The resulting speed in x-direction in m/s
+ * @param vy		The resulting speed in y-direction in m/s
+ * @param omega	The resulting angular velocity in rad/s
+ * @param m1		Speed of motor 1 in rpm
+ * @param m2		Speed of motor 2 in rpm
+ * @param m3		Speed of motor 3 in rpm
+ * @throws		RobotinoException if no valid drive layout parameters are available.
+ */
+void
+RobotinoComThread::unproject( float* vx, float* vy, float* omega, float m1, float m2, float m3 ) const
+{
+	//Convert from RPM to mm/s
+	const double k = 60.0 * cfg_gear_ / ( 2.0 * M_PI * cfg_rw_ );
+
+	*vx = static_cast<float>( ( (double)m3 - (double)m1 ) / sqrt( 3.0 ) / k );
+	*vy = static_cast<float>( 2.0 / 3.0 * ( (double)m1 + 0.5 * ( (double)m3 - (double)m1 ) - (double)m2 ) / k );
+
+	double vw = (double)*vy + (double)m2 / k;
+
+	*omega = static_cast<float>( vw / cfg_rb_ );
+}
diff --git a/src/plugins/robotino/com_thread.h b/src/plugins/robotino/com_thread.h
index 168d39a..55b530f 100644
--- a/src/plugins/robotino/com_thread.h
+++ b/src/plugins/robotino/com_thread.h
@@ -24,25 +24,22 @@
 #include <core/threading/thread.h>
 #include <aspect/logging.h>
 #include <aspect/clock.h>
-#include <aspect/configurable.h>
-#include <aspect/blackboard.h>
 
 #include <utils/time/time.h>
 
+#include <cstdio>
+
 namespace fawkes {
 	class Mutex;
 	class Clock;
-	class TimeWait;
-
-	class BatteryInterface;
-	class RobotinoSensorInterface;
-	class IMUInterface;
 }
 
 #define NUM_IR_SENSORS 9
 
 class RobotinoComThread
-: public fawkes::Thread
+: public fawkes::Thread,
+	public fawkes::ClockAspect,
+	public fawkes::LoggingAspect
 {
  public:
 	struct SensorData {
@@ -88,20 +85,59 @@ class RobotinoComThread
 	virtual void set_speed_points(float s1, float s2, float s3) = 0;
 	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t) = 0;
 	virtual void get_odometry(double &x, double &y, double &phi) = 0;
+
 	virtual void reset_odometry() = 0;
 	virtual void set_bumper_estop_enabled(bool enabled) = 0;
+	virtual void set_motor_accel_limits(float min_accel, float max_accel) = 0;
 	
 	virtual bool get_data(SensorData &sensor_data);
 
+	        void set_drive_layout(float rb, float rw, float gear);
+	        void set_drive_limits(float trans_accel, float trans_decel, float rot_accel, float rot_decel);
+	virtual void set_desired_vel(float vx, float vy, float omega);
+
+	void  project(float *m1, float *m2, float *m3, float vx, float vy, float omega) const;
+	void  unproject(float *vx, float *vy, float *omega, float m1, float m2, float m3) const;
+
+ protected:
+	bool update_velocities();
+	
+ private:
+
+	float update_speed(float des, float set, float accel, float decel, float diff_sec);
+
  protected:
 	/** Mutex to protect data_. Lock whenever accessing it. */
-	fawkes::Mutex    *data_mutex_;
+	fawkes::Mutex  *data_mutex_;
 	/** Data struct that must be updated whenever new data is available. */
-	SensorData        data_;
+	SensorData      data_;
 	/** Flag to indicate new data, set to true if data_ is modified. */
-	bool              new_data_;
-
-
+	bool            new_data_;
+
+ private:
+	float           cfg_rb_;
+	float           cfg_rw_;
+	float           cfg_gear_;
+	float           cfg_trans_accel_;
+	float           cfg_trans_decel_;
+	float           cfg_rot_accel_;
+	float           cfg_rot_decel_;
+	
+	fawkes::Mutex  *vel_mutex_;
+	fawkes::Time   *vel_last_update_;
+	bool            vel_last_zero_;
+	float           des_vx_;
+	float           des_vy_;
+	float           des_omega_;
+
+	float           set_vx_;
+	float           set_vy_;
+	float           set_omega_;
+
+#ifdef USE_VELOCITY_RECORDING
+	FILE *f_;
+	fawkes::Time     *start_;
+#endif
 };
 
 
diff --git a/src/plugins/robotino/direct_com_thread.cpp b/src/plugins/robotino/direct_com_thread.cpp
index 7425ad0..1a77809 100644
--- a/src/plugins/robotino/direct_com_thread.cpp
+++ b/src/plugins/robotino/direct_com_thread.cpp
@@ -50,7 +50,7 @@ using namespace fawkes;
 DirectRobotinoComThread::DirectRobotinoComThread()
 	: RobotinoComThread("DirectRobotinoComThread"),
 	  serial_(io_service_), io_service_work_(io_service_), deadline_(io_service_),
-	  request_timer_(io_service_), nodata_timer_(io_service_)
+	  request_timer_(io_service_), nodata_timer_(io_service_), drive_timer_(io_service_)
 {
 	set_prepfin_conc_loop(true);
 }
@@ -69,6 +69,7 @@ DirectRobotinoComThread::init()
 	cfg_sensor_update_cycle_time_ =
 		config->get_uint("/hardware/robotino/cycle-time");
 	cfg_gripper_enabled_ = config->get_bool("/hardware/robotino/gripper/enable_gripper");
+	cfg_rpm_max_ = config->get_float("/hardware/robotino/motor/rpm-max");
 
 	// -------------------------------------------------------------------------- //
 
@@ -85,8 +86,10 @@ DirectRobotinoComThread::init()
 	deadline_.expires_at(boost::posix_time::pos_infin);
 	check_deadline();
 
-	open_device();
-	opened_ = true;
+	request_timer_.expires_from_now(boost::posix_time::milliseconds(-1));
+	drive_timer_.expires_at(boost::posix_time::pos_infin);
+
+	open_device(/* wait for replies */ true);
 	open_tries_ = 0;
 }
 
@@ -97,6 +100,8 @@ DirectRobotinoComThread::prepare_finalize_user()
 	//logger->log_info(name(), "Prepare Finalize");
 	request_timer_.cancel();
 	nodata_timer_.cancel();
+	drive_timer_.cancel();
+	drive_timer_.expires_at(boost::posix_time::pos_infin);
 	request_timer_.expires_at(boost::posix_time::pos_infin);
 	nodata_timer_.expires_at(boost::posix_time::pos_infin);
 	deadline_.expires_at(boost::posix_time::pos_infin);
@@ -135,22 +140,27 @@ DirectRobotinoComThread::loop()
 			update_nodata_timer();
 		} catch (Exception &e) {
 			if (! finalize_prepared) {
-				logger->log_warn(name(), "Transmission error, sending ping");
-				logger->log_warn(name(), e);
-				input_buffer_.consume(input_buffer_.size());
-				DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
-				send_message(req);
-				//} else {
-				//logger->log_warn(name(), "Transmission error, but finalize prepared");
-				//logger->log_warn(name(), e);
+				if (opened_) {
+					logger->log_warn(name(), "Transmission error, sending ping");
+					logger->log_warn(name(), e);
+					input_buffer_.consume(input_buffer_.size());
+					try {
+						DirectRobotinoComMessage req(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
+						send_message(req);
+						request_data();
+					} catch (Exception &e) {}
+				} else {
+					logger->log_warn(name(), "Transmission error, connection closed");
+				}
 			}
 		}
 	} else {
 		try {
 			logger->log_info(name(), "Re-opening device");
-			open_device();
-			opened_ = true;
+			open_device(/* wait for replies */ false);
 			logger->log_info(name(), "Connection re-established after %u tries", open_tries_ + 1);
+			open_tries_ = 0;
+			request_data();
 		} catch (Exception &e) {
 			open_tries_ += 1;
 			if (open_tries_ >= (1000 / cfg_sensor_update_cycle_time_)) {
@@ -241,11 +251,35 @@ DirectRobotinoComThread::process_message(DirectRobotinoComMessage::pointer m)
 void
 DirectRobotinoComThread::reset_odometry()
 {
-	DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_ODOMETRY);
-	m.add_float(0.); // X (m)
-	m.add_float(0.); // Y (m)
-	m.add_float(0.); // rot (rad)
-	send_message(m);
+	try {
+		DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_ODOMETRY);
+		m.add_float(0.); // X (m)
+		m.add_float(0.); // Y (m)
+		m.add_float(0.); // rot (rad)
+		send_message(m);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Resetting odometry failed, exception follows");
+		logger->log_error(name(), e);
+	}
+}
+
+
+void
+DirectRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
+{
+	try {
+		DirectRobotinoComMessage req;
+		for (int i = 0; i < 2; ++i) {
+			req.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_ACCEL_LIMITS);
+			req.add_uint8(i);
+			req.add_float(min_accel);
+			req.add_float(max_accel);
+		}
+		send_message(req);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting motor accel limits failed, exception follows");
+		logger->log_error(name(), e);
+	}
 }
 
 
@@ -288,17 +322,26 @@ DirectRobotinoComThread::is_gripper_open()
 void
 DirectRobotinoComThread::set_speed_points(float s1, float s2, float s3)
 {
-	DirectRobotinoComMessage m;
-	m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
-	m.add_uint8(0);
-	m.add_uint16((uint16_t)roundf(s1));
-	m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
-	m.add_uint8(1);
-	m.add_uint16((uint16_t)roundf(s2));
-	m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
-	m.add_uint8(2);
-	m.add_uint16((uint16_t)roundf(s3));
-	send_message(m);
+	float bounded_s1 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s1));
+	float bounded_s2 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s2));
+	float bounded_s3 = std::max(-cfg_rpm_max_, std::min(cfg_rpm_max_, s3));
+	
+	try {
+		DirectRobotinoComMessage m;
+		m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+		m.add_uint8(0);
+		m.add_uint16((uint16_t)roundf(bounded_s1));
+		m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+		m.add_uint8(1);
+		m.add_uint16((uint16_t)roundf(bounded_s2));
+		m.add_command(DirectRobotinoComMessage::CMDID_SET_MOTOR_SPEED);
+		m.add_uint8(2);
+		m.add_uint16((uint16_t)roundf(bounded_s3));
+		send_message(m);
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting speed points failed, exception follows");
+		logger->log_error(name(), e);
+	}
 }
 
 void
@@ -309,12 +352,17 @@ DirectRobotinoComThread::set_gripper(bool opened)
 void
 DirectRobotinoComThread::set_bumper_estop_enabled(bool enabled)
 {
-	DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
-	m.add_uint8(enabled ? 1 : 0);
-	send_message(m);
+	try {
+		DirectRobotinoComMessage m(DirectRobotinoComMessage::CMDID_SET_EMERGENCY_BUMPER);
+		m.add_uint8(enabled ? 1 : 0);
+		send_message(m);
 
-	MutexLocker lock(data_mutex_);
-	data_.bumper_estop_enabled = enabled;
+		MutexLocker lock(data_mutex_);
+		data_.bumper_estop_enabled = enabled;
+	} catch (Exception &e) {
+		logger->log_error(name(), "Setting bumper estop state failed, exception follows");
+		logger->log_error(name(), e);
+	}
 }
 
 
@@ -392,7 +440,7 @@ DirectRobotinoComThread::find_device_udev()
 }
 
 void
-DirectRobotinoComThread::open_device()
+DirectRobotinoComThread::open_device(bool wait_replies)
 {
 	if (finalize_prepared)  return;
 
@@ -405,10 +453,8 @@ DirectRobotinoComThread::open_device()
 		//serial_.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::none));
 		serial_.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
 		serial_.set_option(boost::asio::serial_port::baud_rate(115200));
-    
-		//send_init_packet(/* enable transfer */ true);
-
-		//resync();
+		
+		opened_ = true;
 	} catch (boost::system::system_error &e) {
 		throw Exception("RobotinoDirect failed I/O: %s", e.what());
 	}
@@ -417,34 +463,44 @@ DirectRobotinoComThread::open_device()
 		DirectRobotinoComMessage req;
 		req.add_command(DirectRobotinoComMessage::CMDID_GET_HW_VERSION);
 		req.add_command(DirectRobotinoComMessage::CMDID_GET_SW_VERSION);
-		DirectRobotinoComMessage::pointer m = send_and_recv(req);
-
-		//logger->log_info(name(), "Escaped:\n%s", m->to_string(true).c_str());
-		//logger->log_info(name(), "Un-Escaped:\n%s", m->to_string(false).c_str());
-		std::string hw_version, sw_version;
-		DirectRobotinoComMessage::command_id_t msgid;
-		while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
-			if (msgid == DirectRobotinoComMessage::CMDID_HW_VERSION) {
-				hw_version = m->get_string();
-			} else if (msgid == DirectRobotinoComMessage::CMDID_SW_VERSION) {
-				sw_version = m->get_string();
+
+		if (wait_replies) {
+			DirectRobotinoComMessage::pointer m = send_and_recv(req);
+
+			//logger->log_info(name(), "Escaped:\n%s", m->to_string(true).c_str());
+			//logger->log_info(name(), "Un-Escaped:\n%s", m->to_string(false).c_str());
+			std::string hw_version, sw_version;
+			DirectRobotinoComMessage::command_id_t msgid;
+			while ((msgid = m->next_command()) != DirectRobotinoComMessage::CMDID_NONE) {
+				if (msgid == DirectRobotinoComMessage::CMDID_HW_VERSION) {
+					hw_version = m->get_string();
+				} else if (msgid == DirectRobotinoComMessage::CMDID_SW_VERSION) {
+					sw_version = m->get_string();
 				
-			} else if (msgid == DirectRobotinoComMessage::CMDID_CHARGER_ERROR) {
-				uint8_t id = m->get_uint8();
-				uint32_t mtime = m->get_uint32();
-				std::string error = m->get_string();
-				logger->log_warn(name(), "Charger error (ID %u, Time: %u): %s",
-				                 id, mtime, error.c_str());
-				//} else {
-				//logger->log_debug(name(), "  - %u\n", msgid);
+				} else if (msgid == DirectRobotinoComMessage::CMDID_CHARGER_ERROR) {
+					uint8_t id = m->get_uint8();
+					uint32_t mtime = m->get_uint32();
+					std::string error = m->get_string();
+					logger->log_warn(name(), "Charger error (ID %u, Time: %u): %s",
+					                 id, mtime, error.c_str());
+					//} else {
+					//logger->log_debug(name(), "  - %u\n", msgid);
+				}
+			}
+			if (hw_version.empty() || sw_version.empty()) {
+				close_device();
+				throw Exception("RobotinoDirect: no reply to version inquiry from robot");
+			}
+			logger->log_debug(name(), "Connected, HW Version: %s  SW Version: %s",
+			                  hw_version.c_str(), sw_version.c_str());
+		} else {
+			try {
+				send_message(req);
+			} catch (Exception &e) {
+				logger->log_error(name(), "Requesting version information failed, exception follows");
+				logger->log_error(name(), e);
 			}
 		}
-		if (hw_version.empty() || sw_version.empty()) {
-			close_device();
-			throw Exception("RobotinoDirect: no reply to version inquiry from robot");
-		}
-		logger->log_debug(name(), "Connected, HW Version: %s  SW Version: %s",
-		                  hw_version.c_str(), sw_version.c_str());
 	}
 }
 
@@ -452,9 +508,10 @@ DirectRobotinoComThread::open_device()
 void
 DirectRobotinoComThread::close_device()
 {
-	boost::mutex::scoped_lock lock(io_mutex_);
 	serial_.cancel();
 	serial_.close();
+	opened_ = false;
+	open_tries_ = 0;
 }
 
 
@@ -496,25 +553,36 @@ DirectRobotinoComThread::send_message(DirectRobotinoComMessage &msg)
 	boost::mutex::scoped_lock lock(io_mutex_);
 	if (opened_) {
 		//logger->log_warn(name(), "Sending");
-		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
+		boost::system::error_code ec;
+		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()), ec);
+
+		if (ec) {
+			close_device();
+			throw Exception("Error while writing message (%s), closing connection",
+			                ec.message().c_str());
+		}
 	}
 }
 
-/*
-  std::shared_ptr<DirectRobotinoComMessage>
-  DirectRobotinoComMessage::send_and_recv(std::shared_ptr<DirectRobotinoComMessage> msg)
-  {
-	
-  }
-*/
-
 std::shared_ptr<DirectRobotinoComMessage>
 DirectRobotinoComThread::send_and_recv(DirectRobotinoComMessage &msg)
 {
 	boost::mutex::scoped_lock lock(io_mutex_);
-	boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()));
-	std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
-	return m;
+	if (opened_) {
+		boost::system::error_code ec;
+		boost::asio::write(serial_, boost::asio::const_buffers_1(msg.buffer()), ec);
+		if (ec) {
+			logger->log_error(name(), "Error while writing message (%s), closing connection",
+			                  ec.message().c_str());
+
+			close_device();
+			throw Exception("RobotinoDirect: write failed (%s)", ec.message().c_str());
+		}
+		std::shared_ptr<DirectRobotinoComMessage> m = read_packet();
+		return m;
+	} else {
+		throw Exception("RobotinoDirect: serial device not opened");
+	}
 }
 
 /// @cond INTERNAL
@@ -571,9 +639,9 @@ DirectRobotinoComThread::read_packet()
 	}
 
 	// Read all potential junk before the start header
-	if (bytes_read > 1) {
-		logger->log_warn(name(), "Read junk off line");
-	}
+	// if (bytes_read > 1) {
+	// 	logger->log_warn(name(), "Read junk off line");
+	// }
 	input_buffer_.consume(bytes_read - 1);
 	
 	// start timeout for remaining packet
@@ -660,9 +728,11 @@ DirectRobotinoComThread::request_data()
 {
 	if (finalize_prepared)  return;
 
-	request_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_sensor_update_cycle_time_));
-	request_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_request_data, this,
-	                                      boost::asio::placeholders::error));
+	if (request_timer_.expires_from_now() < boost::posix_time::milliseconds(0)) {
+		request_timer_.expires_from_now(boost::posix_time::milliseconds(cfg_sensor_update_cycle_time_));
+		request_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_request_data, this,
+		                                      boost::asio::placeholders::error));
+	}
 }
 
 void
@@ -693,16 +763,48 @@ DirectRobotinoComThread::handle_request_data(const boost::system::error_code &ec
 			logger->log_warn(name(), e);
 		}
 
+	} else {		
+		logger->log_warn(name(), "Request timer failed: %s", ec.message().c_str());
+	}
+
+	if (! finalize_prepared && opened_) {
 		request_data();
 	}
 }
 
+void
+DirectRobotinoComThread::set_desired_vel(float vx, float vy, float omega)
+{
+	RobotinoComThread::set_desired_vel(vx, vy, omega);
+	drive();
+}
+
+
+void
+DirectRobotinoComThread::drive()
+{
+	if (finalize_prepared)  return;
+
+	drive_timer_.expires_from_now(boost::posix_time::milliseconds(10));
+	drive_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_drive, this,
+	                                    boost::asio::placeholders::error));	
+}
+
+
+void
+DirectRobotinoComThread::handle_drive(const boost::system::error_code &ec)
+{
+	if (! ec) {
+		if (update_velocities())  drive();
+	}
+}
+
 
 void
 DirectRobotinoComThread::update_nodata_timer()
 {
 	nodata_timer_.cancel();
-	nodata_timer_.expires_from_now(boost::posix_time::milliseconds(1000));
+	nodata_timer_.expires_from_now(boost::posix_time::milliseconds(2000));
 	nodata_timer_.async_wait(boost::bind(&DirectRobotinoComThread::handle_nodata, this,
 	                                     boost::asio::placeholders::error));
 }
@@ -714,7 +816,5 @@ DirectRobotinoComThread::handle_nodata(const boost::system::error_code &ec)
 	if (! ec) {
 		logger->log_error(name(), "No data received for too long, re-establishing connection");
 		close_device();
-		opened_ = false;
-		open_tries_ = 0;
 	}
 }
diff --git a/src/plugins/robotino/direct_com_thread.h b/src/plugins/robotino/direct_com_thread.h
index a20684d..f419604 100644
--- a/src/plugins/robotino/direct_com_thread.h
+++ b/src/plugins/robotino/direct_com_thread.h
@@ -24,8 +24,6 @@
 #include "com_thread.h"
 #include "direct_com_message.h"
 #include <core/threading/thread.h>
-#include <aspect/logging.h>
-#include <aspect/clock.h>
 #include <aspect/configurable.h>
 #include <aspect/blackboard.h>
 
@@ -39,7 +37,6 @@ class DirectRobotinoComMessage;
 
 namespace fawkes {
 	class Mutex;
-	class Clock;
 	class TimeWait;
 
 	class BatteryInterface;
@@ -49,9 +46,7 @@ namespace fawkes {
 
 class DirectRobotinoComThread
 : public RobotinoComThread,
-	public fawkes::LoggingAspect,
-	public fawkes::ConfigurableAspect,
-	public fawkes::ClockAspect
+	public fawkes::ConfigurableAspect
 {
  public:
 	DirectRobotinoComThread();
@@ -71,15 +66,19 @@ class DirectRobotinoComThread
 	virtual void set_speed_points(float s1, float s2, float s3);
 	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
 	virtual void get_odometry(double &x, double &y, double &phi);
+
 	virtual void reset_odometry();
 	virtual void set_bumper_estop_enabled(bool enabled);
+	virtual void set_motor_accel_limits(float min_accel, float max_accel);
+
+	virtual void set_desired_vel(float vx, float vy, float omega);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
 
  private:
 	std::string find_device_udev();
-	void open_device();
+	void open_device(bool wait_replies);
 	void close_device();
 	void flush_device();
 	void check_deadline();
@@ -87,7 +86,10 @@ class DirectRobotinoComThread
 	void handle_request_data(const boost::system::error_code &ec);
 	void handle_nodata(const boost::system::error_code &ec);
 	void update_nodata_timer();
-	
+
+	void drive();
+	void handle_drive(const boost::system::error_code &ec);
+
 	DirectRobotinoComMessage::pointer read_packet();
 	void send_message(DirectRobotinoComMessage &msg);
 	DirectRobotinoComMessage::pointer
@@ -99,6 +101,7 @@ class DirectRobotinoComThread
 	bool            cfg_enable_gyro_;
 	unsigned int    cfg_sensor_update_cycle_time_;
 	bool            cfg_gripper_enabled_;
+	float           cfg_rpm_max_;
 
 	bool opened_;
 	unsigned int open_tries_;
@@ -112,6 +115,7 @@ class DirectRobotinoComThread
 
 	boost::asio::deadline_timer   request_timer_;
 	boost::asio::deadline_timer   nodata_timer_;
+	boost::asio::deadline_timer   drive_timer_;
 	
 };
 
diff --git a/src/plugins/robotino/openrobotino_com_thread.cpp b/src/plugins/robotino/openrobotino_com_thread.cpp
index 4b2492b..c12e542 100644
--- a/src/plugins/robotino/openrobotino_com_thread.cpp
+++ b/src/plugins/robotino/openrobotino_com_thread.cpp
@@ -382,6 +382,13 @@ OpenRobotinoComThread::reset_odometry()
 }
 
 
+void
+OpenRobotinoComThread::set_motor_accel_limits(float min_accel, float max_accel)
+{
+	throw Exception("Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
+}
+
+
 /** Check if we are connected to OpenRobotino.
  * @return true if the connection has been established, false otherwise
  */
diff --git a/src/plugins/robotino/openrobotino_com_thread.h b/src/plugins/robotino/openrobotino_com_thread.h
index bfc177c..b9b7869 100644
--- a/src/plugins/robotino/openrobotino_com_thread.h
+++ b/src/plugins/robotino/openrobotino_com_thread.h
@@ -23,8 +23,6 @@
 
 #include "com_thread.h"
 #include <core/threading/thread.h>
-#include <aspect/logging.h>
-#include <aspect/clock.h>
 #include <aspect/configurable.h>
 #include <aspect/blackboard.h>
 
@@ -68,10 +66,6 @@ namespace fawkes {
 	class Mutex;
 	class Clock;
 	class TimeWait;
-
-	class BatteryInterface;
-	class RobotinoSensorInterface;
-	class IMUInterface;
 }
 
 class OpenRobotinoComThread
@@ -79,9 +73,7 @@ class OpenRobotinoComThread
 #ifdef HAVE_OPENROBOTINO_API_1
 	public rec::robotino::com::Com,
 #endif
-	public fawkes::LoggingAspect,
-	public fawkes::ConfigurableAspect,
-	public fawkes::ClockAspect
+	public fawkes::ConfigurableAspect
 {
  public:
 	OpenRobotinoComThread();
@@ -99,8 +91,10 @@ class OpenRobotinoComThread
 	virtual void set_speed_points(float s1, float s2, float s3);
 	virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
 	virtual void get_odometry(double &x, double &y, double &phi);
+
 	virtual void reset_odometry();
 	virtual void set_bumper_estop_enabled(bool enabled);
+	virtual void set_motor_accel_limits(float min_accel, float max_accel);
 
 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list