footbot_turret_entity.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/utility/math/angles.h>
9 #include <argos3/core/simulator/physics_engine/physics_engine.h>
10 #include <argos3/core/simulator/space/space.h>
11 
12 namespace argos {
13 
14  /****************************************/
15  /****************************************/
16 
18  CEntity(pc_parent),
19  m_psAnchor(nullptr) {
20  Reset();
21  Disable();
22  }
23 
24  /****************************************/
25  /****************************************/
26 
28  const std::string& str_id,
29  SAnchor& s_anchor) :
30  CEntity(pc_parent, str_id),
31  m_psAnchor(&s_anchor) {
32  Reset();
33  Disable();
34  }
35 
36  /****************************************/
37  /****************************************/
38 
40  }
41 
42  /****************************************/
43  /****************************************/
44 
46  m_unMode = MODE_OFF;
47  m_cDesRot = CRadians::ZERO;
48  m_cOldRot = CRadians::ZERO;
49  m_fDesRotSpeed = 0.0;
50  m_fCurRotSpeed = 0.0;
51  if (m_psAnchor) {
52  m_psAnchor->OffsetOrientation = CQuaternion();
53  }
54  }
55 
56  /****************************************/
57  /****************************************/
58 
60  /* Calculate rotation speed */
61  CRadians cZAngle, cYAngle, cXAngle;
62  m_psAnchor->OffsetOrientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
63  m_fCurRotSpeed =
64  NormalizedDifference(cZAngle, m_cOldRot).GetValue() *
66  /* Save rotation for next time */
67  m_cOldRot = cZAngle;
68  }
69 
70  /****************************************/
71  /****************************************/
72 
74  CRadians cZAngle, cYAngle, cXAngle;
75  m_psAnchor->OffsetOrientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
76  return cZAngle;
77  }
78 
79  /****************************************/
80  /****************************************/
81 
83  return m_fCurRotSpeed;
84  }
85 
86  /****************************************/
87  /****************************************/
88 
90  return m_cDesRot;
91  }
92 
93  /****************************************/
94  /****************************************/
95 
97  return m_fDesRotSpeed;
98  }
99 
100  /****************************************/
101  /****************************************/
102 
104  m_cDesRot = c_rotation;
105  m_cDesRot.SignedNormalize();
106  }
107 
108  /****************************************/
109  /****************************************/
110 
112  m_fDesRotSpeed = f_speed;
113  }
114 
115  /****************************************/
116  /****************************************/
117 
119 
120  /****************************************/
121  /****************************************/
122 
123 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
Definition: angles.h:510
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CEntity)
Basic class for an entity that contains other entities.
The basic entity type.
Definition: entity.h:90
void Disable()
Disables the entity.
Definition: entity.h:275
static Real GetInverseSimulationClockTick()
Returns the inverse of GetSimulationClockTick().
An anchor related to the body of an entity.
Definition: physics_model.h:38
CQuaternion OffsetOrientation
The initial orientation of the anchor wrt the body coordinate system.
Definition: physics_model.h:49
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
static const CRadians ZERO
Set to zero radians.
Definition: angles.h:79
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
const CRadians & GetDesiredRotation() const
void SetDesiredRotationSpeed(Real f_speed)
CFootBotTurretEntity(CComposableEntity *pc_parent)
void SetDesiredRotation(const CRadians &c_rotation)
virtual void Update()
Updates the state of this entity.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...