Robotsystem of the Robot Group Leipzig: 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.6  2007/07/03 13:02:14  martius
    00024  *   maximum velocity check
    00025  *   new pid with stepsize
    00026  *
    00027  *   Revision 1.5  2007/04/03 16:29:24  der
    00028  *   use fixed version of pid
    00029  *   new default values
    00030  *
    00031  *   Revision 1.4  2007/02/23 15:14:17  martius
    00032  *   *** empty log message ***
    00033  *
    00034  *   Revision 1.3  2007/02/21 16:07:23  der
    00035  *   min and max are adjustable during runtime
    00036  *   jointlimits are set by servo to 1.3 fold of servo limits
    00037  *
    00038  *   Revision 1.2  2007/02/12 13:28:20  martius
    00039  *   twoaxisservo and some minor changes
    00040  *
    00041  *   Revision 1.1  2007/01/26 12:04:38  martius
    00042  *   servos combinied into OneAxisServo
    00043  *
    00044  *
    00045  *                                                                 *
    00046  ***************************************************************************/
    00047 #ifndef __SERVO1_H
    00048 #define __SERVO1_H
    00049
    00050 #include "joint.h"
    00051 #include "pid.h"
    00052
    00053 namespace lpzrobots {
    00054
    00055   /** general servo motor
    00056    */
    00057   class OneAxisServo {
    00058   public:
    00059     /** min and max values are understood as travel bounds. Min should be less than 0.*/
    00060
    00061     OneAxisServo(OneAxisJoint* joint, double _min, double _max,
    00062                  double power, double damp=0.2, double integration=2, double maxVel=10.0)
    00063       : joint(joint), pid(power, integration, damp), maxVel(maxVel) {
    00064       assert(joint);
    00065       setMinMax(_min,_max);
    00066       assert(min<max);
    00067       assert(min <= 0);
    00068       assert(max >= 0);
    00069       assert(power>=0 && damp >=0 && integration >=0);
    00070     }
    00071
    00072     virtual ~OneAxisServo(){}
    00073
    00074     /** sets the set point of the servo.
    00075         Position must be between -1 and 1. It is scaled to fit into min, max
    00076     */
    00077     virtual void set(double pos){
    00078       if(pos > 0){
    00079         pos *= max;
    00080       }else{
    00081         pos *= -min;
    00082       }
    00083       pid.setTargetPosition(pos);
    00084
    00085       double force = pid.step(joint->getPosition1(), joint->odeHandle.getTime());
    00086       force = std::min(pid.KP, std::max(-pid.KP,force));// limit force to 1*KP
    00087       joint->addForce1(force);
    00088       joint->getPart1()->limitLinearVel(maxVel);
    00089       joint->getPart2()->limitLinearVel(maxVel);
    00090     }
    00091
    00092     /** returns the position of the slider in ranges [-1, 1] (scaled by min, max)*/
    00093     virtual double get(){
    00094       double pos =  joint->getPosition1();
    00095       if(pos > 0){
    00096         pos /= max;
    00097       }else{
    00098         pos /= -min;
    00099       }
    00100       return pos;
    00101     }
    00102
    00103     virtual void setMinMax(double _min, double _max){
    00104       min=_min;
    00105       max=_max;
    00106       joint->setParam(dParamLoStop, min * 1.3);
    00107       joint->setParam(dParamHiStop, max * 1.3);
    00108     }
    00109
    00110     /** adjusts the power of the servo*/
    00111     virtual void setPower(double power) {
    00112       pid.KP = power;
    00113     };
    00114     /** returns the power of the servo*/
    00115     virtual double& power() {
    00116       return pid.KP;
    00117     };
    00118
    00119     /** returns the damping of the servo*/
    00120     virtual double& damping() {
    00121       return pid.KD;
    00122     };
    00123     /** returns the damping of the servo*/
    00124     virtual double& offsetCanceling() {
    00125       return pid.KI;
    00126     };
    00127
    00128   private:
    00129     OneAxisJoint* joint;
    00130     double min;
    00131     double max;
    00132     PID pid;
    00133     double maxVel;
    00134   };
    00135
    00136   typedef OneAxisServo SliderServo;
    00137   typedef OneAxisServo HingeServo;
    00138   typedef OneAxisServo Hinge2Servo;
    00139
    00140 }
    00141 #endif