Robot Simulator of the Robotics Group for Self-Organization of Control: oneaxisservo.h Source File

- Main Page
- Namespaces
- Classes
- Files
- Directories
- Related Pages
- Examples

- Search for |

- File List
- File Members

# oneaxisservo.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: oneaxisservo.h,v $
    00023  *   Revision 1.11  2009/08/12 10:28:29  der
    00024  *   Centered servos use stepNoCutoff which is much more stable
    00025  *
    00026  *   Revision 1.10  2009/08/10 14:46:41  der
    00027  *   power() functions removed because references are bad vor velocity servo
    00028  *   setPower() functions added
    00029  *
    00030  *   Revision 1.9  2009/05/11 15:43:22  martius
    00031  *   new velocity controlling servo motors
    00032  *
    00033  *   Revision 1.8  2009/02/04 09:37:05  martius
    00034  *   fixed offset of 2 in centered servos
    00035  *
    00036  *   Revision 1.7  2008/11/14 11:23:05  martius
    00037  *   added centered Servos! This is useful for highly nonequal min max values
    00038  *   skeleton has now also a joint in the back
    00039  *
    00040  *   Revision 1.6  2007/07/03 13:02:14  martius
    00041  *   maximum velocity check
    00042  *   new pid with stepsize
    00043  *
    00044  *   Revision 1.5  2007/04/03 16:29:24  der
    00045  *   use fixed version of pid
    00046  *   new default values
    00047  *
    00048  *   Revision 1.4  2007/02/23 15:14:17  martius
    00049  *   *** empty log message ***
    00050  *
    00051  *   Revision 1.3  2007/02/21 16:07:23  der
    00052  *   min and max are adjustable during runtime
    00053  *   jointlimits are set by servo to 1.3 fold of servo limits
    00054  *
    00055  *   Revision 1.2  2007/02/12 13:28:20  martius
    00056  *   twoaxisservo and some minor changes
    00057  *
    00058  *   Revision 1.1  2007/01/26 12:04:38  martius
    00059  *   servos combinied into OneAxisServo
    00060  *
    00061  *
    00062  *                                                                 *
    00063  ***************************************************************************/
    00064 #ifndef __SERVO1_H
    00065 #define __SERVO1_H
    00066
    00067 #include "joint.h"
    00068 #include "pid.h"
    00069 #include "angularmotor.h"
    00070 #include <selforg/controller_misc.h>
    00071
    00072 namespace lpzrobots {
    00073
    00074   /** general servo motor to achieve position control
    00075    */
    00076   class OneAxisServo {
    00077   public:
    00078     /** min and max values are understood as travel bounds. Min should be less than 0.*/
    00079
    00080     OneAxisServo(OneAxisJoint* joint, double _min, double _max,
    00081                  double power, double damp=0.2, double integration=2, double maxVel=10.0,
    00082                  double jointLimit = 1.3, bool minmaxCheck = true)
    00083       : joint(joint), pid(power, integration, damp), maxVel(maxVel), jointLimit(jointLimit) {
    00084       assert(joint);
    00085       setMinMax(_min,_max);
    00086       assert(min<max);
    00087       assert(!minmaxCheck || min <= 0);
    00088       assert(!minmaxCheck || max >= 0);
    00089       assert(power>=0 && damp >=0 && integration >=0);
    00090     }
    00091
    00092     virtual ~OneAxisServo(){}
    00093
    00094     /** sets the set point of the servo.
    00095         Position must be between -1 and 1. It is scaled to fit into min, max
    00096     */
    00097     virtual void set(double pos){
    00098       pos = clip(pos, -1.0, 1.0);
    00099       if(pos > 0){
    00100         pos *= max;
    00101       }else{
    00102         pos *= -min;
    00103       }
    00104       pid.setTargetPosition(pos);
    00105
    00106       double force = pid.step(joint->getPosition1(), joint->odeHandle.getTime());
    00107       force = std::min(pid.KP, std::max(-pid.KP,force));// limit force to 1*KP
    00108       joint->addForce1(force);
    00109       if(maxVel>0){
    00110         joint->getPart1()->limitLinearVel(maxVel);
    00111         joint->getPart2()->limitLinearVel(maxVel);
    00112       }
    00113     }
    00114
    00115     /** returns the position of the slider in ranges [-1, 1] (scaled by min, max)*/
    00116     virtual double get(){
    00117       double pos =  joint->getPosition1();
    00118       if(pos > 0){
    00119         pos /= max;
    00120       }else{
    00121         pos /= -min;
    00122       }
    00123       return pos;
    00124     }
    00125
    00126     virtual void setMinMax(double _min, double _max){
    00127       min=_min;
    00128       max=_max;
    00129       joint->setParam(dParamLoStop, min * jointLimit);
    00130       joint->setParam(dParamHiStop, max * jointLimit);
    00131     }
    00132
    00133     /** adjusts the power of the servo*/
    00134     virtual void setPower(double power) {
    00135       pid.KP = power;
    00136     };
    00137
    00138     /** returns the power of the servo*/
    00139     virtual double getPower() {
    00140       return pid.KP;
    00141     };
    00142
    00143     /** returns the damping of the servo*/
    00144     virtual double& damping() {
    00145       return pid.KD;
    00146     };
    00147     /** returns the damping of the servo*/
    00148     virtual double& offsetCanceling() {
    00149       return pid.KI;
    00150     };
    00151
    00152     /** adjusts maximal speed of servo*/
    00153     virtual void setMaxVel(double maxVel) {
    00154       this->maxVel = maxVel;
    00155     };
    00156     /** adjusts maximal speed of servo*/
    00157     virtual double getMaxVel() {
    00158       return maxVel;
    00159     };
    00160
    00161
    00162   protected:
    00163     OneAxisJoint* joint;
    00164     double min;
    00165     double max;
    00166     PID pid;
    00167     double maxVel;
    00168     double jointLimit; ///< joint limit with respect to servo limit
    00169   };
    00170
    00171   typedef OneAxisServo SliderServo;
    00172   typedef OneAxisServo HingeServo;
    00173   typedef OneAxisServo Hinge2Servo;
    00174
    00175
    00176
    00177   /** general servo motor to achieve position control with zero position centered
    00178    */
    00179   class OneAxisServoCentered : public OneAxisServo {
    00180   public:
    00181     /** min and max values are understood as travel bounds.
    00182         The zero position is max-min/2
    00183     */
    00184     OneAxisServoCentered(OneAxisJoint* joint, double _min, double _max,
    00185                          double power, double damp=0.2, double integration=2,
    00186                          double maxVel=10.0, double jointLimit = 1.3)
    00187       : OneAxisServo(joint, _min, _max, power, damp, integration, maxVel, jointLimit, false){
    00188     }
    00189     virtual ~OneAxisServoCentered(){}
    00190
    00191     /** sets the set point of the servo.
    00192         Position must be between -1 and 1. It is scaled to fit into min, max,
    00193         however 0 is just in the center of min and max
    00194     */
    00195     virtual void set(double pos){
    00196       pos = clip(pos, -1.0, 1.0);
    00197       pos = (pos+1)*(max-min)/2 + min;
    00198
    00199       pid.setTargetPosition(pos);
    00200       double force = pid.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00201       force = clip(force,-10*pid.KP, 10*pid.KP); // limit force to 10*KP
    00202       joint->addForce1(force);
    00203       if(maxVel>0){
    00204         joint->getPart1()->limitLinearVel(maxVel);
    00205         joint->getPart2()->limitLinearVel(maxVel);
    00206       }
    00207     }
    00208     /** returns the position of the slider in ranges [-1, 1] (scaled by min, max, centered)*/
    00209     virtual double get(){
    00210       double pos =  joint->getPosition1();
    00211
    00212       return 2*(pos-min)/(max-min) - 1;
    00213     }
    00214
    00215   };
    00216
    00217   /** general servo motor to achieve position control
    00218    *  that that internally controls the velocity of the motor (much more stable)
    00219    *  with centered zero position
    00220    */
    00221   class OneAxisServoVel : public OneAxisServo {
    00222   public:
    00223     /** min and max values are understood as travel bounds.
    00224         The zero position is max-min/2
    00225     */
    00226     OneAxisServoVel(const OdeHandle& odeHandle,
    00227                     OneAxisJoint* joint, double _min, double _max,
    00228                     double power, double damp=0.01, double maxVel=20, double jointLimit = 1.3)
    00229       : OneAxisServo(joint, _min, _max, maxVel/2, damp, 0, 0, jointLimit, false),
    00230         motor(odeHandle, joint, power)
    00231     {
    00232     }
    00233
    00234     virtual ~OneAxisServoVel(){}
    00235
    00236     /** adjusts the power of the servo*/
    00237     virtual void setPower(double power) {
    00238       motor.setPower(power);
    00239     };
    00240     /** returns the power of the servo*/
    00241     virtual double getPower() {
    00242       return motor.getPower();
    00243     };
    00244     /** offetCanceling does not exist for this type of servo */
    00245     virtual double& offsetCanceling() {
    00246       dummy=0;
    00247       return dummy;
    00248     };
    00249
    00250     /** adjusts maximal speed of servo*/
    00251     virtual void setMaxVel(double maxVel) {
    00252       this->maxVel = maxVel;
    00253       pid.KP=maxVel/2;
    00254     };
    00255     /** adjusts maximal speed of servo*/
    00256     virtual double getMaxVel() {
    00257       return maxVel;
    00258     };
    00259
    00260
    00261     /** sets the set point of the servo.
    00262         Position must be between -1 and 1. It is scaled to fit into min, max,
    00263         however 0 is just in the center of min and max
    00264     */
    00265     virtual void set(double pos){
    00266       pos = clip(pos, -1.0, 1.0);
    00267       pos = (pos+1)*(max-min)/2 + min;
    00268       pid.setTargetPosition(pos);
    00269       double vel = pid.stepNoCutoff(joint->getPosition1(), joint->odeHandle.getTime());
    00270       motor.set(0, vel);
    00271     }
    00272
    00273     /** returns the position of the servo in ranges [-1, 1] (scaled by min, max, centered)*/
    00274     virtual double get(){
    00275       double pos =  joint->getPosition1();
    00276       return 2*(pos-min)/(max-min) - 1;
    00277     }
    00278   protected:
    00279     AngularMotor1Axis motor;
    00280     double dummy;
    00281   };
    00282
    00283 }
    00284 #endif