Robotsystem of the Robot Group Leipzig: twoaxisservo.h Source File

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

- Search for |

- File List
- File Members

# twoaxisservo.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: twoaxisservo.h,v $
    00023  *   Revision 1.4  2007/07/17 07:17:40  martius
    00024  *   joints limits are set
    00025  *   damping is accessable for both axis
    00026  *
    00027  *   Revision 1.3  2007/07/03 13:02:22  martius
    00028  *   maximum velocity check
    00029  *   new pid with stepsize
    00030  *
    00031  *   Revision 1.2  2007/04/03 16:31:02  der
    00032  *   use fixed version of pid
    00033  *   new default values
    00034  *
    00035  *   Revision 1.1  2007/02/12 13:28:20  martius
    00036  *   twoaxisservo and some minor changes
    00037  *
    00038  *
    00039  *
    00040  *                                                                 *
    00041  ***************************************************************************/
    00042 #ifndef __SERVO2_H
    00043 #define __SERVO2_H
    00044
    00045 #include "joint.h"
    00046 #include "pid.h"
    00047
    00048 namespace lpzrobots {
    00049
    00050   /** general servo motor for 2 axis joints
    00051    */
    00052   class TwoAxisServo {
    00053   public:
    00054     /** min and max values are understood as travel bounds. Min should be less than 0.*/
    00055
    00056     TwoAxisServo(TwoAxisJoint* joint, double _min1, double _max1, double power1,
    00057                  double _min2, double _max2, double power2,
    00058                  double damp=0.2, double integration=2, double maxVel=10.0)
    00059       : joint(joint),
    00060         pid1(power1, integration, damp),
    00061         pid2(power2, integration, damp),
    00062         maxVel(maxVel) {
    00063       assert(joint);
    00064       setMinMax1(_min1,_max1);
    00065       setMinMax2(_min2,_max2);
    00066       assert(min1<max1); assert(min2<max2);
    00067       assert(min1 <= 0); assert(min2 <= 0);
    00068       assert(max1 >= 0); assert(max2 >= 0);
    00069       assert(power1 >=0 && power2 >=0 && damp >=0 && integration >=0);
    00070     }
    00071     virtual ~TwoAxisServo(){}
    00072
    00073     /** sets the set point of the servo.
    00074         Position must be between -1 and 1. It is scaled to fit into min, max
    00075     */
    00076     virtual void set(double pos1, double pos2){
    00077       if(pos1 > 0){
    00078         pos1 *= max1;
    00079       }else{
    00080         pos1 *= -min1;
    00081       }
    00082       pid1.setTargetPosition(pos1);
    00083       // double force1 = pid1.stepWithD(joint->getPosition1(), joint->getPosition1Rate());
    00084       double force1 = pid1.step(joint->getPosition1(), joint->odeHandle.getTime());
    00085       // limit force to 1*KP
    00086       force1 = std::min(pid1.KP, std::max(-pid1.KP,force1));// limit force to 1*KP
    00087
    00088       if(pos2 > 0){
    00089         pos2 *= max2;
    00090       }else{
    00091         pos2 *= -min2;
    00092       }
    00093       pid2.setTargetPosition(pos2);
    00094       //      double force2 = pid2.stepWithD(joint->getPosition2(), joint->getPosition2Rate());
    00095       double force2 = pid2.step(joint->getPosition2(), joint->odeHandle.getTime());
    00096       // limit force to 1*KP
    00097       force2 = std::min(pid2.KP, std::max(-pid2.KP,force2));// limit force to 1*KP
    00098       joint->addForces(force1, force2);
    00099       joint->getPart1()->limitLinearVel(maxVel);
    00100       joint->getPart2()->limitLinearVel(maxVel);
    00101     }
    00102
    00103     /** returns the position of the servo (joint) of 1. axis in ranges [-1, 1] (scaled by min1, max1)*/
    00104     virtual double get1(){
    00105       double pos =  joint->getPosition1();
    00106       if(pos > 0){
    00107         pos /= max1;
    00108       }else{
    00109         pos /= -min1;
    00110       }
    00111       return pos;
    00112     }
    00113
    00114     /** returns the position of the servo (joint) of 2. axis in ranges [-1, 1] (scaled by min2, max2)*/
    00115     virtual double get2(){
    00116       double pos =  joint->getPosition2();
    00117       if(pos > 0){
    00118         pos /= max2;
    00119       }else{
    00120         pos /= -min2;
    00121       }
    00122       return pos;
    00123     }
    00124
    00125     /** returns the positions of the joint in ranges [-1, 1] (scaled by min, max)*/
    00126     void get(double& p1, double& p2){
    00127       p1=get1();
    00128       p2=get2();
    00129     }
    00130
    00131
    00132     /** adjusts the power of the servo*/
    00133     virtual void setPower(double power1, double power2) {
    00134       pid1.KP = power1;
    00135       pid2.KP = power2;
    00136     };
    00137     /** returns the power of the servo*/
    00138     virtual double& power1() {
    00139       return pid1.KP;
    00140     };
    00141     /** returns the power of the servo*/
    00142     virtual double& power2() {
    00143       return pid2.KP;
    00144     };
    00145
    00146     /** returns the damping of the servo*/
    00147     virtual double& damping1() {
    00148       return pid1.KD;
    00149     };
    00150
    00151     /** returns the damping of the servo*/
    00152     virtual double& damping2() {
    00153       return pid2.KD;
    00154     };
    00155
    00156     /** returns the damping of the servo*/
    00157     virtual double& offsetCanceling() {
    00158       return pid1.KI;
    00159     };
    00160
    00161     virtual void setMinMax1(double _min, double _max){
    00162       min1=_min;
    00163       max1=_max;
    00164       joint->setParam(dParamLoStop, _min * 1.3);
    00165       joint->setParam(dParamHiStop, _max * 1.3);
    00166     }
    00167
    00168     virtual void setMinMax2(double _min, double _max){
    00169       min2=_min;
    00170       max2=_max;
    00171       joint->setParam(dParamLoStop2, _min * 1.3);
    00172       joint->setParam(dParamHiStop2, _max * 1.3);
    00173     }
    00174
    00175
    00176   private:
    00177     TwoAxisJoint* joint;
    00178     double min1;
    00179     double max1;
    00180     double min2;
    00181     double max2;
    00182     PID pid1;
    00183     PID pid2;
    00184     double maxVel;
    00185   };
    00186
    00187   typedef TwoAxisServo UniversalServo;
    00188 }
    00189 #endif