sinecontroller.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                         *
 *   $Log: sinecontroller.cpp,v $
 *   Revision 1.1.2.3  2005/12/11 23:35:08  martius
 *   *** empty log message ***
 *
 *   Revision 1.1.2.2  2005/11/14 17:37:29  martius
 *   moved to selforg
 *
 *   Revision 1.1.2.1  2005/11/14 15:38:17  martius
 *   moved to selforg
 *
 *   Revision 1.4  2005/11/14 12:46:19  martius
 *   added number of controlled motors
 *
 *   Revision 1.3  2005/10/06 17:06:57  martius
 *   switched to stl lists
 *
 *   Revision 1.2  2005/07/07 10:25:47  martius
 *   added Phaseshift
 *
 *   Revision 1.1  2005/07/06 13:55:33  fhesse
 *   initial version, realising sine and cosine outputs
 *                                            * 
 *                                                                         *
 ***************************************************************************/

#include <stdio.h>
#include <math.h>
#include "sinecontroller.h"

SineController::SineController(int number_controlled){
  t=0;
  sineRate=40;
  phaseShift=1;

  number_sensors=0;
  number_motors=0;
  this->number_controlled=number_controlled;
  

  // prepare name;
  Configurable::insertCVSInfo(name, "$RCSfile: sinecontroller.cpp,v $", 
                                    "$Revision: 1.1.2.3 $");
};

/** initialisation of the controller with the given sensor/ motornumber 
    Must be called before use.
*/
void SineController::init(int sensornumber, int motornumber){
  number_sensors=sensornumber;
  number_motors=motornumber;
  if(number_controlled==-1) number_controlled=motornumber;
};
  
/** performs one step (includes learning). 
    Calculates motor commands from sensor inputs.
    @param sensor sensors inputs scaled to [-1,1]
    @param sensornumber length of the sensor array
    @param motor motors outputs. MUST have enough space for motor values!
    @param motornumber length of the provided motor array
*/
void SineController::step(const sensor* sensors, int sensornumber, 
                          motor* motors, int motornumber) {
  stepNoLearning(sensors, sensornumber, motors, motornumber);
};

/** performs one step without learning. 
    @see step
*/
void SineController::stepNoLearning(const sensor* sensors, int number_sensors, 
                                    motor* motors, int number_motors) {
  
  for (int i=0; i<number_motors; i++){
    if(i < number_controlled){
      motors[i]=sin(t/sineRate + i*phaseShift*M_PI/2);
    }else {
      motors[i]=0;
    }
  }  
  t++;
};
  
  

Configurable::paramval SineController::getParam(const paramkey& key) const{
  if(key == "sinerate") return sineRate; 
  else if(key == "phaseshift") return phaseShift; 
  else  return AbstractController::getParam(key) ;
}

bool SineController::setParam(const paramkey& key, paramval val){
  if(key == "sinerate") sineRate=val;
  else if(key == "phaseshift") phaseShift=val; 
  else return AbstractController::setParam(key, val);
  return true;
}

Configurable::paramlist SineController::getParamList() const{
  paramlist l;
  l += pair<paramkey, paramval> (string("sinerate"), sineRate);
  l += pair<paramkey, paramval> (string("phaseshift"), phaseShift);
  return l;
}

---