Robotsystem from Robot Group Leipzig: schlangeservo2.cpp Source File

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

- File List
- File Members

# schlangeservo2.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: schlangeservo2.cpp,v $
    00023  *   Revision 1.1.2.2  2006/02/23 18:05:05  martius
    00024  *   friction with angularmotor
    00025  *
    00026  *   Revision 1.1.2.1  2006/02/01 18:33:40  martius
    00027  *   use Axis type for Joint axis. very important, since otherwise Vec3 * pose is not the right direction vector anymore
    00028  *
    00029  *
    00030  ***************************************************************************/
    00031
    00032 #include "schlangeservo2.h"
    00033
    00034 namespace lpzrobots {
    00035
    00036 SchlangeServo2::SchlangeServo2 ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00037                                const SchlangeConf& conf, const char* n)
    00038   : Schlange(odeHandle, osgHandle, conf, n)
    00039 {
    00040
    00041   // prepare name;
    00042   Configurable::insertCVSInfo(name, "$RCSfile: schlangeservo2.cpp,v $",
    00043                               "$Revision: 1.1.2.2 $");
    00044 }
    00045
    00046 SchlangeServo2::~SchlangeServo2() { }
    00047
    00048
    00049 /**
    00050  *Reads the actual motor commands from an array, an sets all motors (forces) of the snake to this values.
    00051  *It is an linear allocation.
    00052  *@param motors pointer to the array, motor values are scaled to [-1,1]
    00053  *@param motornumber length of the motor array
    00054  **/
    00055 void SchlangeServo2::setMotors ( const motor* motors, int motornumber )
    00056 {
    00057   assert(created);
    00058   int len = min(motornumber, getMotorNumber())/2;
    00059   // controller output as torques
    00060   for (int i = 0; i < len; i++){
    00061     servos[i]->set(motors[2*i], motors[2*i+1]);
    00062   }
    00063 }
    00064
    00065 /**
    00066  *Writes the sensor values to an array in the memory.
    00067  *@param sensor* pointer to the arrays
    00068
    00069  *@param sensornumber length of the sensor array
    00070  *@return number of actually written sensors
    00071  **/
    00072 int SchlangeServo2::getSensors ( sensor* sensors, int sensornumber )
    00073 {
    00074   assert(created);
    00075   int len = min(sensornumber, getSensorNumber())/2;
    00076
    00077   for (int n = 0; n < len; n++) {
    00078     sensors[2*n] = servos[n]->get1();
    00079     sensors[2*n+1] = servos[n]->get2();
    00080   }
    00081
    00082   return 2*len;
    00083 }
    00084
    00085
    00086   /** creates vehicle at desired position
    00087       @param pos struct Position with desired position
    00088   */
    00089   void SchlangeServo2::create(const osg::Matrix& pose){
    00090     Schlange::create(pose);
    00091
    00092     //*****************joint definition***********
    00093     for ( int n = 0; n < conf.segmNumber-1; n++ ) {
    00094
    00095       const Pos& p1(objects[n]->getPosition());
    00096       const Pos& p2(objects[n+1]->getPosition());
    00097       UniversalJoint* j = new UniversalJoint(objects[n], objects[n+1],
    00098                                              (p1 + p2)/2,
    00099                                              Axis(0,0,1)* pose, Axis(0,1,0)* pose);
    00100       j->init(odeHandle, osgHandle, true, conf.segmDia * 1.02);
    00101
    00102       // setting stops at universal joints
    00103       j->setParam(dParamLoStop, -conf.jointLimit*1.5);
    00104       j->setParam(dParamHiStop,  conf.jointLimit*1.5);
    00105
    00106       // making stops bouncy
    00107       //    j->setParam (dParamBounce, 0.9 );
    00108       //    j->setParam (dParamBounce2, 0.9 ); // universal
    00109
    00110       joints.push_back(j);
    00111
    00112       UniversalServo* servo =  new UniversalServo(j, -conf.jointLimit, conf.jointLimit, conf.motorPower,
    00113                                                   -conf.jointLimit, conf.jointLimit, conf.motorPower);
    00114       servos.push_back(servo);
    00115
    00116       frictionmotors.push_back(new AngularMotor2Axis(odeHandle, j,
    00117                                                      conf.frictionJoint, conf.frictionJoint)
    00118                                );
    00119     }
    00120   }
    00121
    00122
    00123 bool SchlangeServo2::setParam(const paramkey& key, paramval val){
    00124   bool rv = Schlange::setParam(key, val);
    00125   for (vector<UniversalServo*>::iterator i = servos.begin(); i!= servos.end(); i++){
    00126     if(*i) (*i)->setPower(conf.motorPower, conf.motorPower);
    00127   }
    00128   return rv;
    00129 }
    00130
    00131
    00132 /** destroys vehicle and space
    00133  */
    00134 void SchlangeServo2::destroy(){
    00135   if (created){
    00136     Schlange::destroy();
    00137     for (vector<UniversalServo*>::iterator i = servos.begin(); i!= servos.end(); i++){
    00138       if(*i) delete *i;
    00139     }
    00140     servos.clear();
    00141   }
    00142 }
    00143
    00144 }