Robotsystem from Robot Group Leipzig: sliderservo.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# sliderservo.cpp

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: sliderservo.cpp,v $
    00023  *   Revision 1.5.4.2  2006/01/03 10:42:17  fhesse
    00024  *   get...1() -> getPosition1()
    00025  *
    00026  *   Revision 1.5.4.1  2005/12/20 17:53:42  martius
    00027  *   changed to Joints from joint.h
    00028  *   new servos for universal and hinge2
    00029  *
    00030  *   Revision 1.5  2005/09/11 18:52:46  martius
    00031  *   new constants
    00032  *
    00033  *   Revision 1.4  2005/09/02 17:23:52  martius
    00034  *   parameters adjusted
    00035  *
    00036  *   Revision 1.3  2005/09/02 13:20:14  martius
    00037  *   adjusted parameters and usage of derivative of slider from ode
    00038  *
    00039  *   Revision 1.2  2005/09/01 14:22:00  martius
    00040  *   parameters adjusted
    00041  *
    00042  *   Revision 1.1  2005/08/30 16:55:48  martius
    00043  *   servo motor for sliders
    00044  *
    00045  *                                                                 *
    00046  ***************************************************************************/
    00047 #include "sliderservo.h"
    00048 #include <assert.h>
    00049
    00050 namespace lpzrobots {
    00051
    00052 SliderServo::SliderServo(SliderJoint* joint, double min, double max, double mass)
    00053   : pid(mass * 200.0, 2.0, 0.3 ), joint(joint)
    00054 {
    00055   assert(min <= 0);
    00056   this->joint = joint;
    00057   this->min = min;
    00058   this->max = max;
    00059 }
    00060
    00061 void SliderServo::set(double pos){
    00062   if(pos > 0){
    00063     pos *= max;
    00064   }else{
    00065     pos *= -min;
    00066   }
    00067   pid.setTargetPosition(pos);
    00068   double force = pid.stepWithD(joint->getPosition1(), joint->getPosition1Rate());
    00069   joint->addForce(force);
    00070 }
    00071
    00072 double SliderServo::get(){
    00073   double pos =  joint->getPosition1();
    00074   if(pos > 0){
    00075     pos /= max;
    00076   }else{
    00077     pos /= -min;
    00078   }
    00079   return pos;
    00080 }
    00081
    00082 }