Robotsystem from Robot Group Leipzig: sinecontroller.cpp Source File

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

- File List
- File Members

# sinecontroller.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: sinecontroller.cpp,v $
    00023  *   Revision 1.1.2.3  2005/12/11 23:35:08  martius
    00024  *   *** empty log message ***
    00025  *
    00026  *   Revision 1.1.2.2  2005/11/14 17:37:29  martius
    00027  *   moved to selforg
    00028  *
    00029  *   Revision 1.1.2.1  2005/11/14 15:38:17  martius
    00030  *   moved to selforg
    00031  *
    00032  *   Revision 1.4  2005/11/14 12:46:19  martius
    00033  *   added number of controlled motors
    00034  *
    00035  *   Revision 1.3  2005/10/06 17:06:57  martius
    00036  *   switched to stl lists
    00037  *
    00038  *   Revision 1.2  2005/07/07 10:25:47  martius
    00039  *   added Phaseshift
    00040  *
    00041  *   Revision 1.1  2005/07/06 13:55:33  fhesse
    00042  *   initial version, realising sine and cosine outputs
    00043  *                                            *
    00044  *                                                                         *
    00045  ***************************************************************************/
    00046
    00047 #include <stdio.h>
    00048 #include <math.h>
    00049 #include "sinecontroller.h"
    00050
    00051 SineController::SineController(int number_controlled){
    00052   t=0;
    00053   sineRate=40;
    00054   phaseShift=1;
    00055
    00056   number_sensors=0;
    00057   number_motors=0;
    00058   this->number_controlled=number_controlled;
    00059
    00060
    00061   // prepare name;
    00062   Configurable::insertCVSInfo(name, "$RCSfile: sinecontroller.cpp,v $",
    00063                                     "$Revision: 1.1.2.3 $");
    00064 };
    00065
    00066 /** initialisation of the controller with the given sensor/ motornumber
    00067     Must be called before use.
    00068 */
    00069 void SineController::init(int sensornumber, int motornumber){
    00070   number_sensors=sensornumber;
    00071   number_motors=motornumber;
    00072   if(number_controlled==-1) number_controlled=motornumber;
    00073 };
    00074
    00075 /** performs one step (includes learning).
    00076     Calculates motor commands from sensor inputs.
    00077     @param sensor sensors inputs scaled to [-1,1]
    00078     @param sensornumber length of the sensor array
    00079     @param motor motors outputs. MUST have enough space for motor values!
    00080     @param motornumber length of the provided motor array
    00081 */
    00082 void SineController::step(const sensor* sensors, int sensornumber,
    00083                           motor* motors, int motornumber) {
    00084   stepNoLearning(sensors, sensornumber, motors, motornumber);
    00085 };
    00086
    00087 /** performs one step without learning.
    00088     @see step
    00089 */
    00090 void SineController::stepNoLearning(const sensor* sensors, int number_sensors,
    00091                                     motor* motors, int number_motors) {
    00092
    00093   for (int i=0; i<number_motors; i++){
    00094     if(i < number_controlled){
    00095       motors[i]=sin(t/sineRate + i*phaseShift*M_PI/2);
    00096     }else {
    00097       motors[i]=0;
    00098     }
    00099   }
    00100   t++;
    00101 };
    00102
    00103
    00104
    00105 Configurable::paramval SineController::getParam(const paramkey& key) const{
    00106   if(key == "sinerate") return sineRate;
    00107   else if(key == "phaseshift") return phaseShift;
    00108   else  return AbstractController::getParam(key) ;
    00109 }
    00110
    00111 bool SineController::setParam(const paramkey& key, paramval val){
    00112   if(key == "sinerate") sineRate=val;
    00113   else if(key == "phaseshift") phaseShift=val;
    00114   else return AbstractController::setParam(key, val);
    00115   return true;
    00116 }
    00117
    00118 Configurable::paramlist SineController::getParamList() const{
    00119   paramlist l;
    00120   l += pair<paramkey, paramval> (string("sinerate"), sineRate);
    00121   l += pair<paramkey, paramval> (string("phaseshift"), phaseShift);
    00122   return l;
    00123 }
    00124
    00125
    00126