Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3865] pkg/trunk
|
From: <stu...@us...> - 2008-09-02 17:20:34
|
Revision: 3865
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3865&view=rev
Author: stuglaser
Date: 2008-09-02 17:20:31 +0000 (Tue, 02 Sep 2008)
Log Message:
-----------
Moved the Pid class into mechanism_model.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/CMakeLists.txt
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h
pkg/trunk/mechanism/mechanism_model/src/pid.cpp
Removed Paths:
-------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h
pkg/trunk/controllers/generic_controllers/src/pid.cpp
Modified: pkg/trunk/controllers/generic_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-09-02 17:20:31 UTC (rev 3865)
@@ -3,4 +3,4 @@
rospack(generic_controllers)
genmsg()
gensrv()
-rospack_add_library(generic_controllers src/pid.cpp src/joint_effort_controller.cpp src/joint_position_controller.cpp src/joint_velocity_controller.cpp src/ramp_effort_controller.cpp src/sine_sweep_controller.cpp src/joint_pd_controller.cpp)
+rospack_add_library(generic_controllers src/joint_effort_controller.cpp src/joint_position_controller.cpp src/joint_velocity_controller.cpp src/ramp_effort_controller.cpp src/sine_sweep_controller.cpp src/joint_pd_controller.cpp)
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -54,7 +54,7 @@
#include <ros/node.h>
#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
// Services
#include <generic_controllers/SetPDCommand.h>
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -55,7 +55,7 @@
#include <ros/node.h>
#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
// Services
#include <generic_controllers/SetCommand.h>
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -54,7 +54,7 @@
#include <ros/node.h>
#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
// Services
#include <generic_controllers/SetCommand.h>
Deleted: pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -1,188 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * 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 Willow Garage 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.
- *********************************************************************/
-#pragma once
-
-/***************************************************/
-/*! \class controller::Pid
- \brief A basic pid class.
-
- This class implements a generic structure that
- can be used to create a wide range of pid
- controllers. It can function independently or
- be subclassed to provide more specific controls
- based on a particular control loop.
-
- In particular, this class implements the standard
- pid equation:
-
- command = -p_term_ - i_term_ - d_term_
-
- where: <br>
- <UL TYPE="none">
- <LI> p_term_ = p_gain_ * p_error_
- <LI> i_term_ = i_gain_ * i_error_
- <LI> d_term_ = d_gain_ * d_error_
- <LI> i_error_ = i_error_ + p_error_ * dt
- <LI> d_error_ = d_error_ + (p_error_ - p_error_last_) / dt
- </UL>
-
- given:<br>
- <UL TYPE="none">
- <LI> p_error_ = p_state-p_target.
- </UL>
-
-*/
-/***************************************************/
-class TiXmlElement;
-namespace controller
-{
-
-class Pid
-{
-public:
-
- /*!
- * \brief Constructor, zeros out Pid values when created and
- * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- Pid(double P = 0.0, double I = 0.0, double D = 0.0, double I1 = 0.0, double I2 = -0.0);
-
- /*!
- * \brief Destructor of Pid class.
- */
- ~Pid();
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param p_error Error since last call (p_state-p_target)
- * \param dt Change in time since last call
- */
- double updatePid(double p_error, double dt);
-
- /*!
- * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- void initPid(double P, double I, double D, double I1, double I2);
-
- void initXml(TiXmlElement *config);
-
- /*!
- * \brief Set current command for this PID controller
- */
- void setCurrentCmd(double cmd);
-
- /*!
- * \brief Return current command for this PID controller
- */
- double getCurrentCmd();
-
- /*!
- * \brief Return PID error terms for the controller.
- * \param pe The proportional error.
- * \param ie The integral error.
- * \param de The derivative error.
- */
- void getCurrentPIDErrors(double *pe, double *ie, double *de);
-
- /*!
- * \brief Set PID gains for the controller.
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param i_max
- * \param i_min
- */
- void setGains(double P, double I, double D, double i_max, double i_min);
-
- /*!
- * \brief Get PID gains for the controller.
- * \param p The proportional gain.
- * \param i The integral gain.
- * \param d The derivative gain.
- * \param i_max
- * \param i_mim
- */
- void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param error Error since last call (p_state-p_target)
- * \param error_dot d(Error)/dt since last call
- * \param dt Change in time since last call
- */
- double updatePid(double error, double error_dot, double dt);
-
- Pid &operator =(const Pid& p)
- {
- if (this == &p)
- return *this;
-
- p_gain_ = p.p_gain_;
- i_gain_ = p.i_gain_;
- d_gain_ = p.d_gain_;
- i_max_ = p.i_max_;
- i_min_ = p.i_min_;
-
- p_error_last_ = p_error_ = i_error_ = d_error = cmd_ = 0.0;
- return *this;
- }
-
-private:
- double p_error_last_; /**< _Save position state for derivative state calculation. */
- double p_error_; /**< Position error. */
- double d_error_; /**< Derivative error. */
- double i_error_; /**< Integator error. */
- double p_gain_; /**< Proportional gain. */
- double i_gain_; /**< Integral gain. */
- double d_gain_; /**< Derivative gain. */
- double i_max_; /**< Maximum allowable integral term. */
- double i_min_; /**< Minimum allowable integral term. */
- double cmd_; /**< Command to send. */
-};
-
-}
Deleted: pkg/trunk/controllers/generic_controllers/src/pid.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/pid.cpp 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/src/pid.cpp 2008-09-02 17:20:31 UTC (rev 3865)
@@ -1,193 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * 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 Willow Garage 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.
- *********************************************************************/
-#include <generic_controllers/pid.h>
-#include "tinyxml/tinyxml.h"
-
-using namespace controller;
-
-Pid::Pid(double P, double I, double D, double I1, double I2) :
- p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
-{
- p_error_last_ = 0.0;
- p_error_ = 0.0;
- d_error_ = 0.0;
- i_error_ = 0.0;
- cmd_ = 0.0;
-}
-
-Pid::~Pid()
-{
-}
-
-void Pid::initPid(double P, double I, double D, double I1, double I2)
-{
- p_gain_ = P;
- i_gain_ = I;
- d_gain_ = D;
- i_max_ = I1;
- i_min_ = I2;
- p_error_last_ = 0.0;
- p_error_ = 0.0;
- d_error_ = 0.0;
- i_error_ = 0.0;
- cmd_ = 0.0;
-}
-
-void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
-{
- p = p_gain_;
- i = i_gain_;
- d = d_gain_;
- i_max = i_max_;
- i_min = i_min_;
-}
-
-void Pid::setGains(double P, double I, double D, double I1, double I2)
-{
- p_gain_ = P;
- i_gain_ = I;
- d_gain_ = D;
- i_max_ = I1;
- i_min_ = I2;
-}
-
-void Pid::initXml(TiXmlElement *config)
-{
- p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
- i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
- d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
- i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
- i_min_ = -i_max_;
-}
-
-double Pid::updatePid(double error, double dt)
-{
- double p_term, d_term, i_term;
- p_error_ = error; //this is pError = pState-pTarget
-
- if (dt == 0)
- {
- throw "dividebyzero"; //TODO: not sure how to deal with this
- }
- else
- {
- // Calculate proportional contribution to command
- p_term = p_gain_ * p_error_;
-
- // Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
-
- //Calculate integral contribution to command
- i_term = i_gain_ * i_error_;
-
- // Limit i_term so that the limit is meaningful in the output
- if (i_term > i_max_)
- {
- i_term = i_max_;
- }
- else if (i_term < i_min_)
- {
- i_term = i_min_;
- }
-
- // Calculate the derivative error
- if (dt != 0)
- {
- d_error_ = (p_error_ - p_error_last_) / dt;
- p_error_last_ = p_error_;
- }
- // Calculate derivative contribution to command
- d_term = d_gain_ * d_error_;
- cmd_ = -p_term - i_term - d_term;
- }
- return cmd_;
-}
-
-
-double Pid::updatePid(double error, double error_dot, double dt)
-{
- double p_term, d_term, i_term;
- p_error_ = error; //this is pError = pState-pTarget
- d_error_ = error_dot;
- if (dt == 0)
- {
- throw "dividebyzero"; //TODO: not sure how to deal with this
- }
- else
- {
- // Calculate proportional contribution to command
- p_term = p_gain_ * p_error_;
-
- // Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
-
- //Calculate integral contribution to command
- i_term = i_gain_ * i_error_;
-
- // Limit i_term so that the limit is meaningful in the output
- if (i_term > i_max_)
- {
- i_term = i_max_;
- }
- else if (i_term < i_min_)
- {
- i_term = i_min_;
- }
-
- // Calculate derivative contribution to command
- d_term = d_gain_ * d_error_;
- cmd_ = -p_term - i_term - d_term;
- }
- return cmd_;
-}
-
-
-
-void Pid::setCurrentCmd(double cmd)
-{
- cmd_ = cmd;
-}
-
-double Pid::getCurrentCmd()
-{
- return cmd_;
-}
-
-void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
-{
- *pe = p_error_;
- *ie = i_error_;
- *de = d_error_;
-}
-
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-09-02 17:20:31 UTC (rev 3865)
@@ -32,7 +32,7 @@
#include <pr2Core/pr2Core.h>
#include <vector>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
namespace gazebo
{
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-02 17:20:31 UTC (rev 3865)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp src/pid.cpp)
Copied: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h (from rev 3864, pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h)
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -0,0 +1,188 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+#pragma once
+
+/***************************************************/
+/*! \class controller::Pid
+ \brief A basic pid class.
+
+ This class implements a generic structure that
+ can be used to create a wide range of pid
+ controllers. It can function independently or
+ be subclassed to provide more specific controls
+ based on a particular control loop.
+
+ In particular, this class implements the standard
+ pid equation:
+
+ command = -p_term_ - i_term_ - d_term_
+
+ where: <br>
+ <UL TYPE="none">
+ <LI> p_term_ = p_gain_ * p_error_
+ <LI> i_term_ = i_gain_ * i_error_
+ <LI> d_term_ = d_gain_ * d_error_
+ <LI> i_error_ = i_error_ + p_error_ * dt
+ <LI> d_error_ = d_error_ + (p_error_ - p_error_last_) / dt
+ </UL>
+
+ given:<br>
+ <UL TYPE="none">
+ <LI> p_error_ = p_state-p_target.
+ </UL>
+
+*/
+/***************************************************/
+class TiXmlElement;
+namespace controller
+{
+
+class Pid
+{
+public:
+
+ /*!
+ * \brief Constructor, zeros out Pid values when created and
+ * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ Pid(double P = 0.0, double I = 0.0, double D = 0.0, double I1 = 0.0, double I2 = -0.0);
+
+ /*!
+ * \brief Destructor of Pid class.
+ */
+ ~Pid();
+
+ /*!
+ * \brief Update the Pid loop with nonuniform time step size.
+ *
+ * \param p_error Error since last call (p_state-p_target)
+ * \param dt Change in time since last call
+ */
+ double updatePid(double p_error, double dt);
+
+ /*!
+ * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ void initPid(double P, double I, double D, double I1, double I2);
+
+ void initXml(TiXmlElement *config);
+
+ /*!
+ * \brief Set current command for this PID controller
+ */
+ void setCurrentCmd(double cmd);
+
+ /*!
+ * \brief Return current command for this PID controller
+ */
+ double getCurrentCmd();
+
+ /*!
+ * \brief Return PID error terms for the controller.
+ * \param pe The proportional error.
+ * \param ie The integral error.
+ * \param de The derivative error.
+ */
+ void getCurrentPIDErrors(double *pe, double *ie, double *de);
+
+ /*!
+ * \brief Set PID gains for the controller.
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param i_max
+ * \param i_min
+ */
+ void setGains(double P, double I, double D, double i_max, double i_min);
+
+ /*!
+ * \brief Get PID gains for the controller.
+ * \param p The proportional gain.
+ * \param i The integral gain.
+ * \param d The derivative gain.
+ * \param i_max
+ * \param i_mim
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+ /*!
+ * \brief Update the Pid loop with nonuniform time step size.
+ *
+ * \param error Error since last call (p_state-p_target)
+ * \param error_dot d(Error)/dt since last call
+ * \param dt Change in time since last call
+ */
+ double updatePid(double error, double error_dot, double dt);
+
+ Pid &operator =(const Pid& p)
+ {
+ if (this == &p)
+ return *this;
+
+ p_gain_ = p.p_gain_;
+ i_gain_ = p.i_gain_;
+ d_gain_ = p.d_gain_;
+ i_max_ = p.i_max_;
+ i_min_ = p.i_min_;
+
+ p_error_last_ = p_error_ = i_error_ = d_error_ = cmd_ = 0.0;
+ return *this;
+ }
+
+private:
+ double p_error_last_; /**< _Save position state for derivative state calculation. */
+ double p_error_; /**< Position error. */
+ double d_error_; /**< Derivative error. */
+ double i_error_; /**< Integator error. */
+ double p_gain_; /**< Proportional gain. */
+ double i_gain_; /**< Integral gain. */
+ double d_gain_; /**< Derivative gain. */
+ double i_max_; /**< Maximum allowable integral term. */
+ double i_min_; /**< Minimum allowable integral term. */
+ double cmd_; /**< Command to send. */
+};
+
+}
Copied: pkg/trunk/mechanism/mechanism_model/src/pid.cpp (from rev 3864, pkg/trunk/controllers/generic_controllers/src/pid.cpp)
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/pid.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/src/pid.cpp 2008-09-02 17:20:31 UTC (rev 3865)
@@ -0,0 +1,193 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+#include "mechanism_model/pid.h"
+#include "tinyxml/tinyxml.h"
+
+using namespace controller;
+
+Pid::Pid(double P, double I, double D, double I1, double I2) :
+ p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
+{
+ p_error_last_ = 0.0;
+ p_error_ = 0.0;
+ d_error_ = 0.0;
+ i_error_ = 0.0;
+ cmd_ = 0.0;
+}
+
+Pid::~Pid()
+{
+}
+
+void Pid::initPid(double P, double I, double D, double I1, double I2)
+{
+ p_gain_ = P;
+ i_gain_ = I;
+ d_gain_ = D;
+ i_max_ = I1;
+ i_min_ = I2;
+ p_error_last_ = 0.0;
+ p_error_ = 0.0;
+ d_error_ = 0.0;
+ i_error_ = 0.0;
+ cmd_ = 0.0;
+}
+
+void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
+{
+ p = p_gain_;
+ i = i_gain_;
+ d = d_gain_;
+ i_max = i_max_;
+ i_min = i_min_;
+}
+
+void Pid::setGains(double P, double I, double D, double I1, double I2)
+{
+ p_gain_ = P;
+ i_gain_ = I;
+ d_gain_ = D;
+ i_max_ = I1;
+ i_min_ = I2;
+}
+
+void Pid::initXml(TiXmlElement *config)
+{
+ p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
+ i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
+ d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
+ i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
+ i_min_ = -i_max_;
+}
+
+double Pid::updatePid(double error, double dt)
+{
+ double p_term, d_term, i_term;
+ p_error_ = error; //this is pError = pState-pTarget
+
+ if (dt == 0)
+ {
+ throw "dividebyzero"; //TODO: not sure how to deal with this
+ }
+ else
+ {
+ // Calculate proportional contribution to command
+ p_term = p_gain_ * p_error_;
+
+ // Calculate the integral error
+ i_error_ = i_error_ + dt * p_error_;
+
+ //Calculate integral contribution to command
+ i_term = i_gain_ * i_error_;
+
+ // Limit i_term so that the limit is meaningful in the output
+ if (i_term > i_max_)
+ {
+ i_term = i_max_;
+ }
+ else if (i_term < i_min_)
+ {
+ i_term = i_min_;
+ }
+
+ // Calculate the derivative error
+ if (dt != 0)
+ {
+ d_error_ = (p_error_ - p_error_last_) / dt;
+ p_error_last_ = p_error_;
+ }
+ // Calculate derivative contribution to command
+ d_term = d_gain_ * d_error_;
+ cmd_ = -p_term - i_term - d_term;
+ }
+ return cmd_;
+}
+
+
+double Pid::updatePid(double error, double error_dot, double dt)
+{
+ double p_term, d_term, i_term;
+ p_error_ = error; //this is pError = pState-pTarget
+ d_error_ = error_dot;
+ if (dt == 0)
+ {
+ throw "dividebyzero"; //TODO: not sure how to deal with this
+ }
+ else
+ {
+ // Calculate proportional contribution to command
+ p_term = p_gain_ * p_error_;
+
+ // Calculate the integral error
+ i_error_ = i_error_ + dt * p_error_;
+
+ //Calculate integral contribution to command
+ i_term = i_gain_ * i_error_;
+
+ // Limit i_term so that the limit is meaningful in the output
+ if (i_term > i_max_)
+ {
+ i_term = i_max_;
+ }
+ else if (i_term < i_min_)
+ {
+ i_term = i_min_;
+ }
+
+ // Calculate derivative contribution to command
+ d_term = d_gain_ * d_error_;
+ cmd_ = -p_term - i_term - d_term;
+ }
+ return cmd_;
+}
+
+
+
+void Pid::setCurrentCmd(double cmd)
+{
+ cmd_ = cmd;
+}
+
+double Pid::getCurrentCmd()
+{
+ return cmd_;
+}
+
+void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
+{
+ *pe = p_error_;
+ *ie = i_error_;
+ *de = d_error_;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
