Robotsystem from Robot Group Leipzig: universalservo.cpp Source File

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

- File List
- File Members

# universalservo.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: universalservo.cpp,v $
    00023  *   Revision 1.1.2.3  2006/02/07 15:51:56  martius
    00024  *   axis, setpower
    00025  *
    00026  *   Revision 1.1.2.2  2006/01/03 10:42:17  fhesse
    00027  *   get...1() -> getPosition1()
    00028  *
    00029  *   Revision 1.1.2.1  2005/12/20 17:53:42  martius
    00030  *   changed to Joints from joint.h
    00031  *   new servos for universal and hinge2
    00032  *
    00033  *   Revision 1.1  2005/09/12 00:08:45  martius
    00034  *   servo for hinges
    00035  *
    00036  *                                                                 *
    00037  ***************************************************************************/
    00038 #include "universalservo.h"
    00039 #include <assert.h>
    00040
    00041 namespace lpzrobots {
    00042
    00043   UniversalServo::UniversalServo(UniversalJoint* joint, double min1, double max1, double power1,
    00044                                  double min2, double max2, double power2)
    00045     : pid1(power1, 2.0, 0.3 ), pid2(power2, 2.0, 0.3 ), joint(joint)
    00046   {
    00047     assert(min1 <= 0 && min1 <= max1);
    00048     assert(min2 <= 0 && min2 <= max2);
    00049     this->min1 = min1;
    00050     this->max1 = max1;
    00051     this->min2 = min2;
    00052     this->max2 = max2;
    00053   }
    00054
    00055   void UniversalServo::set(double pos1, double pos2){
    00056     if(pos1 > 0){
    00057       pos1 *= max1;
    00058     }else{
    00059       pos1 *= -min1;
    00060     }
    00061     pid1.setTargetPosition(pos1);
    00062     double force1 = pid1.stepWithD(joint->getPosition1(), joint->getPosition1Rate());
    00063     if(pos2 > 0){
    00064       pos2 *= max2;
    00065     }else{
    00066       pos2 *= -min2;
    00067     }
    00068     pid2.setTargetPosition(pos2);
    00069     double force2 = pid2.stepWithD(joint->getPosition2(), joint->getPosition2Rate());
    00070
    00071     joint->addTorques(force1, force2);
    00072   }
    00073
    00074   double UniversalServo::get1(){
    00075     double pos = joint->getPosition1();
    00076     if(pos > 0){
    00077       pos /= max1;
    00078     }else{
    00079       pos /= -min1;
    00080     }
    00081     return pos;
    00082   }
    00083
    00084   double UniversalServo::get2(){
    00085     double pos = joint->getPosition2();
    00086     if(pos > 0){
    00087       pos /= max2;
    00088     }else{
    00089       pos /= -min2;
    00090     }
    00091     return pos;
    00092   }
    00093
    00094   void UniversalServo::get(double& p1, double& p2){
    00095     p1 = get1();
    00096     p2 = get2();
    00097   }
    00098
    00099   /* adjusts the power of the two servos*/
    00100   void UniversalServo::setPower(double power1, double power2){
    00101     pid1.setKP(power1);
    00102     pid2.setKP(power2);
    00103   }
    00104
    00105
    00106 }