Robotsystem from Robot Group Leipzig: hinge2servo.cpp Source File

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

- File List
- File Members

# hinge2servo.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: hinge2servo.cpp,v $
    00023  *   Revision 1.1.2.2  2006/01/03 10:35:52  fhesse
    00024  *   getAngle1() -> getPosition1(); the same with getAngle1Rate()
    00025  *
    00026  *   Revision 1.1.2.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.1  2005/09/12 00:08:45  martius
    00031  *   servo for hinges
    00032  *
    00033  *                                                                 *
    00034  ***************************************************************************/
    00035 #include "hinge2servo.h"
    00036 #include <assert.h>
    00037
    00038 namespace lpzrobots {
    00039
    00040 Hinge2Servo::Hinge2Servo(Hinge2Joint* joint, double min, double max, double power)
    00041   : pid(power, 2.0, 0.3 ), joint(joint)
    00042 {
    00043   assert(min <= 0 && min <= max);
    00044   this->min = min;
    00045   this->max = max;
    00046 }
    00047
    00048 void Hinge2Servo::set(double pos){
    00049   if(pos > 0){
    00050     pos *= max;
    00051   }else{
    00052     pos *= -min;
    00053   }
    00054   pid.setTargetPosition(pos);
    00055   double force = pid.stepWithD(joint->getPosition1(), joint->getPosition1Rate());
    00056   joint->addTorques(force, 0);
    00057 }
    00058
    00059 double Hinge2Servo::get(){
    00060   double pos = joint->getPosition1();
    00061   if(pos > 0){
    00062     pos /= max;
    00063   }else{
    00064     pos /= -min;
    00065   }
    00066   return pos;
    00067 }
    00068
    00069 }