TwoDofController.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
2 
11 #include "TwoDofController.h"
12 #include <iostream>
13 
14 void TwoDofControllerInterface::setErrorPrefix(const std::string& _error_prefix) {
15  error_prefix = _error_prefix;
16 }
17 
19  param = TwoDofController::TwoDofControllerParam(); // use default constructor
20  integrator = Integrator(0.0, 0.0);
21  integrator.reset();
22  error_prefix = "";
23 }
24 
26  param.ke = _param.ke; param.tc = _param.tc; param.dt = _param.dt;
27  integrator = Integrator(_param.dt, _range);
28  integrator.reset();
29  error_prefix = "";
30 }
31 
33 }
34 
36  param.ke = 0; param.tc = 0; param.dt = 0;
37  integrator = Integrator(0, 0);
38  reset();
39 }
40 
42  param.ke = _param.ke; param.tc = _param.tc; param.dt = _param.dt;
43  integrator = Integrator(_param.dt, _range);
44  reset();
45 }
46 
48  integrator.reset();
49 }
50 
52  return false;
53 }
54 
56  _p.ke = param.ke;
57  _p.tc = param.tc;
58  _p.dt = param.dt;
59  return true;
60 }
61 
62 double TwoDofController::update (double _x, double _xd) {
63  // Ca = 1/P * Q / (1-Q)
64  // Cb = Gr / (1-Gr) * 1/P * 1 / (1-Q)
65  // P = - ke/s
66  // Gr = Q = 1 / (tc*s + 1)
67 
68  double velocity; // velocity calcurated by 2 dof controller
69 
70  // check parameters
71  if (!param.ke || !param.tc || !param.dt){
72  std::cerr << "[" << error_prefix << "]" << "TwoDofController parameters are not set." << std::endl;
73  return 0;
74  }
75 
76  // integrate (xd - x)
77  // integrated_diff += (_xd - _x) * dt;
78  integrator.update(_xd - _x);
79 
80  // 2 dof controller
81  velocity = (-_x + (_xd - _x) + (integrator.calculate() / param.tc)) / (-param.ke * param.tc);
82 
83  return -velocity * param.dt;
84 
85 }
86 
87 // for compatiblity of Stabilizer
88 TwoDofController::TwoDofController(double _ke, double _tc, double _dt, unsigned int _range) {
89  param.ke = _ke; param.tc = _tc; param.dt = _dt;
90  integrator = Integrator(_dt, _range);
91  integrator.reset();
92  error_prefix = "";
93 }
94 
95 void TwoDofController::setup(double _ke, double _tc, double _dt, unsigned int _range) {
96  param.ke = _ke; param.tc = _tc; param.dt = _dt;
97  integrator = Integrator(_dt, _range);
98 }
Feedback and Feedforward Controller.
double update(double _x, double _xd)
void setErrorPrefix(const std::string &_error_prefix)
virtual void reset()=0


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21