Acceleration

controldev.orogen

@@ -122,7 +122,15 @@ task_context "GenericRawToMotion2D" do
     property("maxSpeed", "double", 1.0).doc("Maximum speed in m/s")
     property("maxRotationSpeed", "double", Math::PI / 2).doc("Maximum rotational velocity in rad/s")
     property("translation_axis_deadzone", "double", 0.02).doc("Translation axis deadzone (range 0-1)")
+    property("acceleration_mode", "bool", false).doc("in acceleration mode, the input is interpreted as speed change")
+
+    # following properties are just used in acceleration mode
+    property("stop_button", "int").doc("Button number as reported by controldev to stop the entire motion")
+    property("stop_translation_button", "int").doc("Button number as reported by controldev to set the translational part to zero")
+    property("stop_rotation_button", "int").doc("Button number as reported by controldev to set the rotational part to zero")
     property("rotation_axis_deadzone", "double", 0.02).doc("Rotation axis deadzone (range 0-1)")
+    property("maxAcceleration", "double", 0.001).doc("Maximum acceleartion in m/s/period")
+    property("maxRotationAcceleration", "double", 0.001).doc("Maximum rotational acceleartion in rad/s/period")
 
     input_port "raw_command", "controldev/RawCommand"
     output_port "motion_command", "base/MotionCommand2D"

tasks/GenericRawToMotion2D.cpp

@@ -40,19 +40,58 @@ bool GenericRawToMotion2D::startHook()
 void GenericRawToMotion2D::updateHook()
 {
     RawCommand rcmd;
-    base::commands::Motion2D  mcmd;
-    if(_raw_command.read(rcmd) == RTT::NewData){
-        int trans_axis = _translation_axis.get().at(0);
-        int trans_subaxis = _translation_axis.get().at(1);
-
-        int rot_axis = _rotation_axis.get().at(0);
-        int rot_subaxis = _rotation_axis.get().at(1);
-        double trans_raw = rcmd.axisValue[trans_axis][trans_subaxis];
-        double rot_raw   = rcmd.axisValue[rot_axis][rot_subaxis];
-
-        mcmd.translation = fabs(trans_raw) < _translation_axis_deadzone ? 0.0 : trans_raw * _maxSpeed;
-        double w = (trans_raw < 0.0) ? rot_raw * _maxRotationSpeed : - rot_raw * _maxRotationSpeed;
-        mcmd.rotation = fabs(rot_raw) < _rotation_axis_deadzone ? 0.0 : w;
+    if (_raw_command.read(rcmd) == RTT::NewData) {
+
+        double trans_raw = rcmd.axisValue[_translation_axis.get().at(0)][_translation_axis.get().at(1)];
+        double rot_raw = rcmd.axisValue[_rotation_axis.get().at(0)][_rotation_axis.get().at(1)];
+
+        // just use input values when they exceed deadzone
+        trans_raw = fabs(trans_raw) < _translation_axis_deadzone ? 0.0 : trans_raw;
+        rot_raw = fabs(rot_raw) < _rotation_axis_deadzone ? 0.0 : rot_raw;
+
+
+        if(!_acceleration_mode){
+            mcmd.translation = trans_raw * _maxSpeed;
+            mcmd.rotation = (trans_raw < 0.0) ? rot_raw * _maxRotationSpeed : - rot_raw * _maxRotationSpeed;
+            _motion_command.write(mcmd);
+            return;
+        }
+
+        /* stop if stop button was pressed */
+        uint8_t stop_button = rcmd.buttonValue[_stop_button.get()];
+        if (stop_button == 1) {
+            mcmd.translation = 0.0;
+            mcmd.rotation = 0.0;
+            _motion_command.write(mcmd);
+            return;
+        }
+
+        /* handle the translational part */
+        uint8_t stop_translation_button = rcmd.buttonValue[_stop_translation_button.get()];
+        if (stop_translation_button == 1) {
+            mcmd.translation = 0.0;
+        } else {
+            // just accelerate when input values exceed deadzone
+            mcmd.translation += trans_raw * _maxAcceleration;
+
+            // limit translation speed
+            mcmd.translation = (mcmd.translation < 0.0) ? std::max(-_maxSpeed.get(), mcmd.translation) : std::min(_maxSpeed.get(), mcmd.translation);
+        }
+
+        /* handle the rotational part */
+        uint8_t stop_rotation_button = rcmd.buttonValue[_stop_rotation_button.get()];
+        if (stop_rotation_button == 1) {
+            mcmd.rotation = 0.0;
+        } else {
+            // just accelerate when input values exceed deadzone
+            double acc = rot_raw * _maxRotationAcceleration.get();
+
+            // reverse rotation acceleration when going backwards
+            mcmd.rotation = (trans_raw < 0.0) ? mcmd.rotation + acc : mcmd.rotation - acc;
+
+            // limit rotation speed
+            mcmd.rotation = mcmd.rotation < 0.0 ? std::max(mcmd.rotation, -_maxRotationSpeed.get()) : std::min(mcmd.rotation, _maxRotationSpeed.get());
+        }
         _motion_command.write(mcmd);
     }
 

tasks/GenericRawToMotion2D.hpp

@@ -27,7 +27,7 @@ with configurable axes
 	friend class GenericRawToMotion2DBase;
     protected:
 
-
+    base::commands::Motion2D  mcmd;
 
     public:
         /** TaskContext constructor for GenericRawToMotion2D