Added LeadLag controller

CMakeLists.txt

@@ -17,12 +17,14 @@ find_package(TinyXML REQUIRED)
 # Dynamics reconfigure
 generate_dynamic_reconfigure_options(
   cfg/Parameters.cfg
+  cfg/LeadLag.cfg
   )
 
 # Add services and generate them
 add_service_files(
   FILES
   SetPidGains.srv
+  SetLeadLagGains.srv
   )
 
 generate_messages(
@@ -50,6 +52,8 @@ include_directories(
   )
 
 add_library(${PROJECT_NAME}
+  src/lead_lag.cpp
+  src/lead_lag_gains_setter.cpp
   src/pid.cpp
   src/pid_gains_setter.cpp
   src/sine_sweep.cpp
@@ -68,6 +72,9 @@ if(CATKIN_ENABLE_TESTING)
   catkin_add_gtest(pid_tests test/pid_tests.cpp)
   target_link_libraries(pid_tests ${catkin_LIBRARIES} ${PROJECT_NAME})
 
+  catkin_add_gtest(lead_lag_tests test/lead_lag_tests.cpp)
+  target_link_libraries(lead_lag_tests ${catkin_LIBRARIES} ${PROJECT_NAME})
+
   # add_executable(test_linear test/linear.cpp)
 endif()
 

cfg/LeadLag.cfg

@@ -0,0 +1,62 @@
+#! /usr/bin/env python
+#*********************************************************************
+#* Software License Agreement (BSD License)
+#*
+#*  Copyright (c) 2017, Open Source Robotics Foundation
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * 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.
+#*   * Neither the name of the Open Source Robotics Foundation
+#*     nor the names of its contributors may be
+#*     used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "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 THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS 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.
+#*********************************************************************/
+
+#   Author: Aris Synodinos
+#   Desc:   Allows Lead Lag parameters, etc to be tuned in realtime using dynamic reconfigure
+
+PACKAGE='control_toolbox'
+
+def generate(gen):
+	#        Name       Type    Level  Description      Default  Min      Max
+	gen.add( "k" ,      double_t, 1,"Gain.",            10.0 , -100000 , 100000)
+	gen.add( "a" ,      double_t, 1,"Lead gain.",        0.1 , -1000   , 1000)
+	gen.add( "b" ,      double_t, 1,"Lag gain.",         1.0 , -1000   , 1000)
+	                 # PkgName  #NodeName         #Prefix for generated .h include file, e.g. ParametersConfig.py
+	exit(gen.generate(PACKAGE, "control_toolbox", "LeadLag"))
+
+# try catkin generator first
+try:
+	from dynamic_reconfigure.parameter_generator_catkin import *
+	gen = ParameterGenerator()
+	generate(gen)
+# reason for catching IndexError
+# parameter_generator_catkin expects 4 arguments while rosbuild only passes in 2
+# not thrilled with this solution
+except IndexError:
+	print 'ERROR', PACKAGE, 'Parameters.cfg failed using parameter_generator_catkin, using rosbuild instead'
+	from dynamic_reconfigure.parameter_generator import *
+	gen = ParameterGenerator()
+	generate(gen)

doc.h

@@ -4,9 +4,11 @@
 
 The control_toolbox package contains the following C++ classes which may be useful when implementing controllers.
  \li \ref control_toolbox::Pid "Pid" for implementing position/integral/derivative control loops.
+ \li \ref control_toolbox::LeadLag "Lead Lag" for implementing lead or lag compensator control loops.
  \li \ref control_toolbox::Ramp "Ramp Output"
  \li \ref control_toolbox::SineSweep "Sine Sweep"
  \li \ref control_toolbox::Dither "Dither" for giving white noise at specified amplitude
  \li \ref control_toolbox::PidGainsSetter "Pid gains setter" for advertising a service for automatically changing PID gains.
+ \li \ref control_toolbox::LeadLagGainsSetter "Lead Lag gains setter" for advertising a service for automatically changing LeadLag gains.
 
 **/