Added LeadLag controller

CMakeLists.txt

@@ -18,12 +18,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(
@@ -52,6 +54,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.
 
 **/