From b6883a4de934dc9d5b281e3a44aea1b5224a944f Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Mon, 18 Apr 2016 12:40:55 +0200 Subject: [PATCH 1/7] added possibility to control the accelearion instead of the speed --- controldev.orogen | 8 +++++ tasks/GenericRawToMotion2D.cpp | 65 +++++++++++++++++++++++++++------- tasks/GenericRawToMotion2D.hpp | 2 +- 3 files changed, 61 insertions(+), 14 deletions(-) diff --git a/controldev.orogen b/controldev.orogen index be5e5f8..03f81ce 100644 --- a/controldev.orogen +++ b/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" diff --git a/tasks/GenericRawToMotion2D.cpp b/tasks/GenericRawToMotion2D.cpp index c0cdda9..d529129 100644 --- a/tasks/GenericRawToMotion2D.cpp +++ b/tasks/GenericRawToMotion2D.cpp @@ -39,19 +39,58 @@ bool GenericRawToMotion2D::startHook() void GenericRawToMotion2D::updateHook() { RawCommand rcmd; - base::MotionCommand2D 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); } diff --git a/tasks/GenericRawToMotion2D.hpp b/tasks/GenericRawToMotion2D.hpp index 7cbdf49..00276a5 100644 --- a/tasks/GenericRawToMotion2D.hpp +++ b/tasks/GenericRawToMotion2D.hpp @@ -27,7 +27,7 @@ with configurable axes friend class GenericRawToMotion2DBase; protected: - + base::MotionCommand2D mcmd; public: /** TaskContext constructor for GenericRawToMotion2D From 5a9a53edb56ca8832d61489daf80dd6c2208e9bd Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Mon, 18 Apr 2016 16:36:06 +0200 Subject: [PATCH 2/7] check if buttons exist --- tasks/GenericRawToMotion2D.cpp | 68 +++++++++++++++++++--------------- 1 file changed, 39 insertions(+), 29 deletions(-) diff --git a/tasks/GenericRawToMotion2D.cpp b/tasks/GenericRawToMotion2D.cpp index 343a79b..1992711 100644 --- a/tasks/GenericRawToMotion2D.cpp +++ b/tasks/GenericRawToMotion2D.cpp @@ -35,6 +35,9 @@ bool GenericRawToMotion2D::startHook() { if (! GenericRawToMotion2DBase::startHook()) return false; + + mcmd.translation = 0.0; + mcmd.rotation = 0.0; return true; } void GenericRawToMotion2D::updateHook() @@ -50,7 +53,7 @@ void GenericRawToMotion2D::updateHook() rot_raw = fabs(rot_raw) < _rotation_axis_deadzone ? 0.0 : rot_raw; - if(!_acceleration_mode){ + if (!_acceleration_mode) { mcmd.translation = trans_raw * _maxSpeed; mcmd.rotation = (trans_raw < 0.0) ? rot_raw * _maxRotationSpeed : - rot_raw * _maxRotationSpeed; _motion_command.write(mcmd); @@ -58,41 +61,48 @@ void GenericRawToMotion2D::updateHook() } /* 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; + int button_cnt = rcmd.buttonValue.size(); + if (_stop_button.get() < button_cnt) { + 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); + if (_stop_translation_button.get() < button_cnt) { + 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()); + if (_stop_rotation_button.get() < button_cnt) { + 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); } - _motion_command.write(mcmd); } GenericRawToMotion2DBase::updateHook(); From e3c3fde7520f44f8b32be0810ab608b37e15ebcf Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Mon, 18 Apr 2016 16:39:57 +0200 Subject: [PATCH 3/7] bug fix --- tasks/GenericRawToMotion2D.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tasks/GenericRawToMotion2D.cpp b/tasks/GenericRawToMotion2D.cpp index 1992711..fc01411 100644 --- a/tasks/GenericRawToMotion2D.cpp +++ b/tasks/GenericRawToMotion2D.cpp @@ -101,8 +101,8 @@ void GenericRawToMotion2D::updateHook() // 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); } + _motion_command.write(mcmd); } GenericRawToMotion2DBase::updateHook(); From 73021d87238bd9d4d261e1e885a1188eaf302b67 Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Mon, 18 Apr 2016 17:11:32 +0200 Subject: [PATCH 4/7] added handling for uncorrectly or not initialized button properties --- controldev.orogen | 6 +-- tasks/GenericRawToMotion2D.cpp | 74 +++++++++++++++++++--------------- 2 files changed, 44 insertions(+), 36 deletions(-) diff --git a/controldev.orogen b/controldev.orogen index 03f81ce..ab427f0 100644 --- a/controldev.orogen +++ b/controldev.orogen @@ -125,9 +125,9 @@ task_context "GenericRawToMotion2D" do 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("stop_button", "int", -1).doc("Button number as reported by controldev to stop the entire motion") + property("stop_translation_button", "int", -1).doc("Button number as reported by controldev to set the translational part to zero") + property("stop_rotation_button", "int", -1).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") diff --git a/tasks/GenericRawToMotion2D.cpp b/tasks/GenericRawToMotion2D.cpp index fc01411..5823e06 100644 --- a/tasks/GenericRawToMotion2D.cpp +++ b/tasks/GenericRawToMotion2D.cpp @@ -62,46 +62,54 @@ void GenericRawToMotion2D::updateHook() /* stop if stop button was pressed */ int button_cnt = rcmd.buttonValue.size(); - if (_stop_button.get() < button_cnt) { - 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; - } + uint8_t stop_button_value = 0; + int stop_button = _stop_button.get(); + if (stop_button < button_cnt && stop_button >= 0) { + stop_button_value = rcmd.buttonValue[stop_button]; + } + if (stop_button_value == 1) { + mcmd.translation = 0.0; + mcmd.rotation = 0.0; + _motion_command.write(mcmd); + return; } /* handle the translational part */ - if (_stop_translation_button.get() < button_cnt) { - 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); - } + uint8_t stop_translation_button_value = 0; + int stop_translation_button = _stop_translation_button.get(); + if (stop_translation_button < button_cnt && stop_translation_button >= 0) { + stop_translation_button_value = rcmd.buttonValue[stop_translation_button]; + } + if (stop_translation_button_value == 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 */ - if (_stop_rotation_button.get() < button_cnt) { - 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()); - } + uint8_t stop_rotation_button_value = 0; + int stop_rotation_button = _stop_rotation_button.get(); + if (stop_rotation_button < button_cnt && stop_rotation_button >= 0) { + stop_rotation_button_value = rcmd.buttonValue[stop_rotation_button]; } + + if (stop_rotation_button_value == 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); } From 52f3c8141aa777dacb7373643fc1f130ede3a13e Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Mon, 18 Apr 2016 17:13:22 +0200 Subject: [PATCH 5/7] formatting --- controldev.orogen | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controldev.orogen b/controldev.orogen index ab427f0..253facf 100644 --- a/controldev.orogen +++ b/controldev.orogen @@ -122,13 +122,13 @@ 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("rotation_axis_deadzone", "double", 0.02).doc("Rotation 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", -1).doc("Button number as reported by controldev to stop the entire motion") property("stop_translation_button", "int", -1).doc("Button number as reported by controldev to set the translational part to zero") property("stop_rotation_button", "int", -1).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") From 5708b0c7c17359a30d5bef0d20764daa52086093 Mon Sep 17 00:00:00 2001 From: Limes Robot Date: Thu, 21 Apr 2016 16:05:10 +0200 Subject: [PATCH 6/7] added stop when closing --- tasks/GenericRawToMotion2D.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/tasks/GenericRawToMotion2D.cpp b/tasks/GenericRawToMotion2D.cpp index 5823e06..049fe6b 100644 --- a/tasks/GenericRawToMotion2D.cpp +++ b/tasks/GenericRawToMotion2D.cpp @@ -117,13 +117,25 @@ void GenericRawToMotion2D::updateHook() } void GenericRawToMotion2D::errorHook() { + mcmd.translation = 0.0; + mcmd.rotation = 0.0; + _motion_command.write(mcmd); + GenericRawToMotion2DBase::errorHook(); } void GenericRawToMotion2D::stopHook() { + mcmd.translation = 0.0; + mcmd.rotation = 0.0; + _motion_command.write(mcmd); + GenericRawToMotion2DBase::stopHook(); } void GenericRawToMotion2D::cleanupHook() { + mcmd.translation = 0.0; + mcmd.rotation = 0.0; + _motion_command.write(mcmd); + GenericRawToMotion2DBase::cleanupHook(); } From eb8a8f8054b16ce74718b1bdaa6ec3d85e820f9b Mon Sep 17 00:00:00 2001 From: Alexander Dettmann Date: Mon, 23 May 2016 15:40:13 +0200 Subject: [PATCH 7/7] added scale for analogue inputs --- controldev.orogen | 2 ++ tasks/GenericRawToMotion2D.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/controldev.orogen b/controldev.orogen index 253facf..1a5bfbd 100644 --- a/controldev.orogen +++ b/controldev.orogen @@ -119,6 +119,8 @@ end task_context "GenericRawToMotion2D" do property("translation_axis", "/std/vector").doc("Translation axis as reported by controldev(main and sub axis)") property("rotation_axis", "/std/vector").doc("Rotation axis as reported by controldev(main and sub axis)") + property("translation_scale", "double", 1.0).doc("scales the translation") + property("rotation_scale", "double", 1.0).doc("scales the rotation") 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)") diff --git a/tasks/GenericRawToMotion2D.cpp b/tasks/GenericRawToMotion2D.cpp index 5823e06..cc52b45 100644 --- a/tasks/GenericRawToMotion2D.cpp +++ b/tasks/GenericRawToMotion2D.cpp @@ -52,6 +52,8 @@ void GenericRawToMotion2D::updateHook() trans_raw = fabs(trans_raw) < _translation_axis_deadzone ? 0.0 : trans_raw; rot_raw = fabs(rot_raw) < _rotation_axis_deadzone ? 0.0 : rot_raw; + trans_raw *= _translation_scale.get(); + rot_raw *= _rotation_scale.get(); if (!_acceleration_mode) { mcmd.translation = trans_raw * _maxSpeed; @@ -117,10 +119,17 @@ void GenericRawToMotion2D::updateHook() } void GenericRawToMotion2D::errorHook() { + mcmd.translation = 0.0; + mcmd.rotation = 0.0; + _motion_command.write(mcmd); GenericRawToMotion2DBase::errorHook(); } void GenericRawToMotion2D::stopHook() { + mcmd.translation = 0.0; + mcmd.rotation = 0.0; + _motion_command.write(mcmd); + GenericRawToMotion2DBase::stopHook(); } void GenericRawToMotion2D::cleanupHook()