TwoDofController.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00002 
00011 #include "TwoDofController.h"
00012 #include <iostream>
00013 
00014 void TwoDofControllerInterface::setErrorPrefix(const std::string& _error_prefix) {
00015   error_prefix = _error_prefix;
00016 }
00017 
00018 TwoDofController::TwoDofController() {
00019   param = TwoDofController::TwoDofControllerParam(); // use default constructor
00020   integrator = Integrator(0.0, 0.0);
00021   integrator.reset();
00022   error_prefix = "";
00023 }
00024 
00025 TwoDofController::TwoDofController(TwoDofController::TwoDofControllerParam &_param, unsigned int _range) {
00026   param.ke = _param.ke; param.tc = _param.tc; param.dt = _param.dt;
00027   integrator = Integrator(_param.dt, _range);
00028   integrator.reset();
00029   error_prefix = "";
00030 }
00031 
00032 TwoDofController::~TwoDofController() {
00033 }
00034 
00035 void TwoDofController::setup() {
00036   param.ke = 0; param.tc = 0; param.dt = 0;
00037   integrator = Integrator(0, 0);
00038   reset();
00039 }
00040 
00041 void TwoDofController::setup(TwoDofController::TwoDofControllerParam &_param, unsigned int _range) {
00042   param.ke = _param.ke; param.tc = _param.tc; param.dt = _param.dt;
00043   integrator = Integrator(_param.dt, _range);
00044   reset();
00045 }
00046 
00047 void TwoDofController::reset() {
00048   integrator.reset();
00049 }
00050 
00051 bool TwoDofController::getParameter() {
00052   return false;
00053 }
00054 
00055 bool TwoDofController::getParameter(TwoDofController::TwoDofControllerParam &_p) {
00056   _p.ke = param.ke;
00057   _p.tc = param.tc;
00058   _p.dt = param.dt;
00059   return true;
00060 }
00061 
00062 double TwoDofController::update (double _x, double _xd) {
00063   // Ca = 1/P * Q / (1-Q)
00064   // Cb = Gr / (1-Gr) * 1/P * 1 / (1-Q)
00065   // P = - ke/s
00066   // Gr = Q = 1 / (tc*s + 1)
00067 
00068   double velocity; // velocity calcurated by 2 dof controller
00069 
00070   // check parameters
00071   if (!param.ke || !param.tc || !param.dt){
00072     std::cerr << "[" << error_prefix << "]" << "TwoDofController parameters are not set." << std::endl;
00073     return 0;
00074   }
00075   
00076   // integrate (xd - x)
00077   // integrated_diff += (_xd - _x) * dt;
00078   integrator.update(_xd - _x);
00079 
00080   // 2 dof controller
00081   velocity = (-_x + (_xd - _x) + (integrator.calculate() / param.tc)) / (-param.ke * param.tc);
00082 
00083   return -velocity * param.dt;
00084   
00085 }
00086 
00087 // for compatiblity of Stabilizer 
00088 TwoDofController::TwoDofController(double _ke, double _tc, double _dt, unsigned int _range) {
00089   param.ke = _ke; param.tc = _tc; param.dt = _dt;
00090   integrator = Integrator(_dt, _range);
00091   integrator.reset();
00092   error_prefix = "";
00093 }
00094 
00095 void TwoDofController::setup(double _ke, double _tc, double _dt, unsigned int _range) {
00096   param.ke = _ke; param.tc = _tc; param.dt = _dt;
00097   integrator = Integrator(_dt, _range);
00098 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56