| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (C) 2012 LAAS-CNRS |
||
3 |
// |
||
4 |
// Author: Florent Lamiraux, |
||
5 |
// Mehdi Benallegue <mehdi@benallegue.com> |
||
6 |
// |
||
7 |
|||
8 |
#include "sot/tools/oscillator.hh" |
||
9 |
|||
10 |
#include <limits> |
||
11 |
|||
12 |
namespace dynamicgraph { |
||
13 |
namespace sot { |
||
14 |
using command::docDirectGetter; |
||
15 |
using command::docDirectSetter; |
||
16 |
using command::makeDirectGetter; |
||
17 |
using command::makeDirectSetter; |
||
18 |
namespace tools { |
||
19 |
|||
20 |
✓✗✓✗ |
1 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Oscillator, "Oscillator"); |
21 |
|||
22 |
1 |
Oscillator::Oscillator(const std::string name) |
|
23 |
: Entity(name), |
||
24 |
✓✗ | 2 |
angularFrequencySIN_(0, "Oscillator(" + name + ")::input(double)::omega"), |
25 |
✓✗ | 2 |
magnitudeSIN_(0, "Oscillator(" + name + ")::input(double)::magnitude"), |
26 |
✓✗ | 2 |
phaseSIN_(0, "Oscillator(" + name + ")::input(double)::phase"), |
27 |
✓✗ | 2 |
biasSIN_(0, "Oscillator(" + name + ")::input(double)::bias"), |
28 |
✓✗ | 2 |
soutSOUT_("Oscillator(" + name + ")::output(double)::sout"), |
29 |
✓✗ | 2 |
vectorSoutSOUT_("Oscillator(" + name + ")::output(vector)::vectorSout"), |
30 |
epsilon_(1e-3), |
||
31 |
started_(true), |
||
32 |
continuous_(false), |
||
33 |
dt_(0.), |
||
34 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
12 |
lastValue_(0.0) { |
35 |
✓✗✓✗ |
2 |
signalRegistration(angularFrequencySIN_ << magnitudeSIN_ << phaseSIN_ |
36 |
✓✗✓✗ |
1 |
<< biasSIN_ << soutSOUT_ |
37 |
✓✗✓✗ |
1 |
<< vectorSoutSOUT_); |
38 |
✓✗ | 1 |
angularFrequencySIN_.setConstant(0.); |
39 |
✓✗ | 1 |
magnitudeSIN_.setConstant(0.); |
40 |
✓✗ | 1 |
phaseSIN_.setConstant(0.); |
41 |
✓✗ | 1 |
biasSIN_.setConstant(0.); |
42 |
|||
43 |
✓✗ | 1 |
soutSOUT_.addDependency(angularFrequencySIN_); |
44 |
✓✗ | 1 |
soutSOUT_.addDependency(magnitudeSIN_); |
45 |
✓✗ | 1 |
soutSOUT_.addDependency(biasSIN_); |
46 |
✓✗ | 1 |
soutSOUT_.addDependency(phaseSIN_); |
47 |
✓✗ | 1 |
vectorSoutSOUT_.addDependency(soutSOUT_); |
48 |
✓✗✓✗ ✓✗ |
1 |
soutSOUT_.setFunction(boost::bind(&Oscillator::computeSignal, this, _1, _2)); |
49 |
✓✗✓✗ ✓✗ |
1 |
vectorSoutSOUT_.setFunction( |
50 |
boost::bind(&Oscillator::computeVectorSignal, this, _1, _2)); |
||
51 |
1 |
soutSOUT_.setNeedUpdateFromAllChildren(true); |
|
52 |
1 |
soutSOUT_.setDependencyType(TimeDependency<int>::ALWAYS_READY); |
|
53 |
|||
54 |
✓✗✓✗ |
1 |
addCommand( |
55 |
"setTimePeriod", |
||
56 |
✓✗✓✗ ✓✗✓✗ |
2 |
makeDirectSetter(*this, &dt_, docDirectSetter("time period", "double"))); |
57 |
✓✗✓✗ |
1 |
addCommand( |
58 |
"getTimePeriod", |
||
59 |
✓✗✓✗ ✓✗✓✗ |
2 |
makeDirectGetter(*this, &dt_, docDirectGetter("time period", "double"))); |
60 |
|||
61 |
✓✗✓✗ |
1 |
addCommand( |
62 |
"setActivated", |
||
63 |
✓✗✓✗ ✓✗✓✗ |
2 |
makeDirectSetter(*this, &started_, docDirectSetter("activated", "bool"))); |
64 |
|||
65 |
✓✗✓✗ |
1 |
addCommand( |
66 |
"getActivated", |
||
67 |
✓✗✓✗ ✓✗✓✗ |
2 |
makeDirectGetter(*this, &started_, docDirectGetter("activated", "bool"))); |
68 |
|||
69 |
/// epsilon is used to ensure there is no discontinuity when starting or |
||
70 |
/// stopping the oscillator. It defines the sensitivity to discontinuities |
||
71 |
✓✗✓✗ |
1 |
addCommand("setEpsilon", |
72 |
✓✗ | 1 |
makeDirectSetter( |
73 |
*this, &epsilon_, |
||
74 |
✓✗✓✗ ✓✗ |
2 |
docDirectSetter("ocillator zero-sensitivity", "double"))); |
75 |
|||
76 |
✓✗✓✗ |
1 |
addCommand("getEpsilon", |
77 |
✓✗ | 1 |
makeDirectGetter( |
78 |
*this, &epsilon_, |
||
79 |
✓✗✓✗ ✓✗ |
2 |
docDirectGetter("ocillator zero-sensitivity", "double"))); |
80 |
|||
81 |
✓✗✓✗ |
1 |
addCommand("setValue", |
82 |
✓✗ | 1 |
makeDirectSetter( |
83 |
*this, &lastValue_, |
||
84 |
✓✗✓✗ ✓✗ |
2 |
docDirectSetter("init value of the oscillator", "double"))); |
85 |
|||
86 |
✓✗✓✗ |
1 |
addCommand("getValue", |
87 |
✓✗ | 1 |
makeDirectGetter( |
88 |
*this, &lastValue_, |
||
89 |
✓✗✓✗ ✓✗ |
2 |
docDirectGetter("current value of the oscillator", "double"))); |
90 |
|||
91 |
✓✗✓✗ |
1 |
addCommand("setContinuous", |
92 |
✓✗ | 1 |
makeDirectSetter(*this, &continuous_, |
93 |
✓✗✓✗ ✓✗ |
2 |
docDirectSetter("continuous", "bool"))); |
94 |
|||
95 |
✓✗✓✗ |
1 |
addCommand("getContinuous", |
96 |
✓✗ | 1 |
makeDirectGetter(*this, &continuous_, |
97 |
✓✗✓✗ ✓✗ |
2 |
docDirectGetter("continuous", "bool"))); |
98 |
1 |
} |
|
99 |
|||
100 |
double Oscillator::value(double dt, double t, double omega, double phase, |
||
101 |
double m, double bias) { |
||
102 |
double tau = dt * t; |
||
103 |
return m * sin(omega * tau + phase) + bias; |
||
104 |
} |
||
105 |
|||
106 |
dynamicgraph::Vector& Oscillator::computeVectorSignal( |
||
107 |
dynamicgraph::Vector& vsout, const int& t) { |
||
108 |
vsout.resize(1); |
||
109 |
vsout(0) = soutSOUT_.access(t); |
||
110 |
return vsout; |
||
111 |
} |
||
112 |
|||
113 |
double& Oscillator::computeSignal(double& sout, const int& t) { |
||
114 |
double eps; |
||
115 |
|||
116 |
if (continuous_) |
||
117 |
eps = epsilon_; |
||
118 |
else |
||
119 |
eps = std::numeric_limits<double>::max(); |
||
120 |
|||
121 |
double omega = angularFrequencySIN_.access(t); |
||
122 |
double m = magnitudeSIN_.access(t); |
||
123 |
double phase = phaseSIN_.access(t); |
||
124 |
double bias = biasSIN_.access(t); |
||
125 |
|||
126 |
if (started_) { |
||
127 |
double current = value(dt_, t, omega, phase, m, bias); |
||
128 |
|||
129 |
if (fabs(lastValue_ - current) < eps) |
||
130 |
lastValue_ = sout = current; |
||
131 |
else |
||
132 |
sout = lastValue_; |
||
133 |
} else { |
||
134 |
if (fabs(lastValue_) < eps) |
||
135 |
lastValue_ = sout = 0; |
||
136 |
else |
||
137 |
lastValue_ = sout = value(dt_, t, omega, phase, m, bias); |
||
138 |
} |
||
139 |
|||
140 |
return sout; |
||
141 |
} |
||
142 |
} // namespace tools |
||
143 |
} // namespace sot |
||
144 |
} // namespace dynamicgraph |
| Generated by: GCOVR (Version 4.2) |