dynamics2d_differentialsteering_control.cpp
Go to the documentation of this file.
1 
8 
9 namespace argos {
10 
11  /****************************************/
12  /****************************************/
13 
15  Real f_max_force,
16  Real f_max_torque,
17  Real f_interwheel_distance,
18  TConfigurationNode* t_node) :
19  CDynamics2DVelocityControl(c_engine, f_max_force, f_max_torque, t_node),
20  m_fInterwheelDistance(f_interwheel_distance) {}
21 
22  /****************************************/
23  /****************************************/
24 
26  Real f_right_wheel) {
27  /*
28  * THE DIFFERENTIAL STEERING SYSTEM
29  *
30  * check http://rossum.sourceforge.net/papers/DiffSteer/
31  * for details
32  *
33  * Equations:
34  *
35  * w = (Vr - Vl) / b
36  * v = [ ((Vr + Vl) / 2) cos(a),
37  * ((Vr + Vl) / 2) sin(a) ]
38  *
39  * where:
40  * a = body orientation
41  * w = body angular velocity
42  * v = body center linear velocity
43  * Vr = right wheel velocity
44  * Vl = left wheel velocity
45  * b = length of wheel axis
46  */
47  SetAngularVelocity((f_right_wheel - f_left_wheel) / m_fInterwheelDistance);
48  Real fVelocity = (f_right_wheel + f_left_wheel) * 0.5f;
49  CVector2 cVelocity(fVelocity * ::cos(m_ptControlledBody->a),
50  fVelocity * ::sin(m_ptControlledBody->a));
51  SetLinearVelocity(cVelocity);
52  }
53 
54  /****************************************/
55  /****************************************/
56 
57 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
A 2D vector class.
Definition: vector2.h:27
CDynamics2DDifferentialSteeringControl(CDynamics2DEngine &c_engine, Real f_max_force, Real f_max_torque, Real f_interwheel_distance, TConfigurationNode *t_node=NULL)
void SetLinearVelocity(const CVector2 &c_velocity)