VehicleAckermann_ControllerFrontSteerPID.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014-2020 Jose Luis Blanco Claraco |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under 3-clause BSD License |
7  | See COPYING |
8  +-------------------------------------------------------------------------+ */
9 
11 #include "xml_utils.h"
12 
13 using namespace mvsim;
14 using namespace std;
15 
17  DynamicsAckermann& veh)
18  : ControllerBase(veh),
19  setpoint_lin_speed(0),
20  setpoint_steer_ang(0),
21  KP(100),
22  KI(0),
23  KD(0),
24  max_torque(100.0),
25  m_twist_control(veh)
26 {
27  // Get distance between wheels:
28  m_r2f_L = m_veh.m_wheels_info[WHEEL_FL].x - m_veh.m_wheels_info[WHEEL_RL].x;
29  ASSERT_(m_r2f_L > 0.0);
30 }
31 
32 // See base class docs
36 {
37  // Equivalent v/w velocities:
38  const double v = setpoint_lin_speed;
39  double w;
40  if (setpoint_steer_ang == 0.0)
41  {
42  w = 0.0;
43  }
44  else
45  {
46  // ang = atan(r2f_L/R) -> R= r2f_L / tan(ang)
47  // R = v/w -> w=v/R
48  const double R = m_r2f_L / tan(setpoint_steer_ang);
49  w = v / R;
50  }
51 
52  // Let the twist controller do the calculations:
55 
60 
62  co.steer_ang = setpoint_steer_ang; // Mainly for the case of v=0
63 }
64 
66  const rapidxml::xml_node<char>& node)
67 {
69  params["KP"] = TParamEntry("%lf", &KP);
70  params["KI"] = TParamEntry("%lf", &KI);
71  params["KD"] = TParamEntry("%lf", &KD);
72  params["max_torque"] = TParamEntry("%lf", &max_torque);
73 
74  // Initial speed.
75  params["V"] = TParamEntry("%lf", &this->setpoint_lin_speed);
76  params["STEER_ANG"] = TParamEntry("%lf_deg", &this->setpoint_steer_ang);
77 
78  parse_xmlnode_children_as_param(node, params);
79 }
80 
82  const TeleopInput& in, TeleopOutput& out)
83 {
85 
86  switch (in.keycode)
87  {
88  case 'W':
89  case 'w':
90  setpoint_lin_speed += 0.1;
91  break;
92 
93  case 'S':
94  case 's':
95  setpoint_lin_speed -= 0.1;
96  break;
97 
98  case 'A':
99  case 'a':
100  setpoint_steer_ang += 1.0 * M_PI / 180.0;
101  mrpt::keep_min(setpoint_steer_ang, m_veh.getMaxSteeringAngle());
102  break;
103 
104  case 'D':
105  case 'd':
106  setpoint_steer_ang -= 1.0 * M_PI / 180.0;
107  mrpt::keep_max(setpoint_steer_ang, -m_veh.getMaxSteeringAngle());
108  break;
109  case ' ':
110  setpoint_lin_speed = .0;
111  break;
112  };
113  out.append_gui_lines += "[Controller=" + string(class_name()) +
114  "] Teleop keys: w/s=incr/decr lin speed. "
115  "a/d=left/right steering. spacebar=stop.\n";
117  "setpoint: v=%.03f steer=%.03f deg\n", setpoint_lin_speed,
118  setpoint_steer_ang * 180.0 / M_PI);
119 }
const GLdouble * v
virtual void control_step(const DynamicsAckermann::TControllerInput &ci, DynamicsAckermann::TControllerOutput &co) override
std::map< std::string, TParamEntry > TParameterDefinitions
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const TParameterDefinitions &params, const std::map< std::string, std::string > &variableNamesValues={}, const char *functionNameContext="")
Definition: xml_utils.cpp:179
double max_torque
Maximum abs. value torque (for clamp) [Nm].
GLubyte GLubyte GLubyte GLubyte w
#define M_PI
double setpoint_ang_speed
desired velocities (m/s) and (rad/s)
GLuint in
virtual void teleop_interface(const TeleopInput &in, TeleopOutput &out) override
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
virtual void control_step(const DynamicsAckermann::TControllerInput &ci, DynamicsAckermann::TControllerOutput &co) override
GLsizei const GLcharARB ** string
virtual void teleop_interface(const TeleopInput &in, TeleopOutput &out) override
#define ASSERT_(f)
double steer_ang
Equivalent ackerman steering angle.
GLfloat * params
virtual void load_config(const rapidxml::xml_node< char > &node) override
double max_torque
Maximum abs. value torque (for clamp) [Nm].
const float R


mvsim
Author(s):
autogenerated on Fri May 7 2021 03:05:51