VehicleAckermann_Drivetrain_ControllerFrontSteerPID.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014 Jose Luis Blanco Claraco (University of Almeria) |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under GNU General Public License version 3 |
7  | See <http://www.gnu.org/licenses/> |
8  +-------------------------------------------------------------------------+ */
9 
11 #include "xml_utils.h"
12 
13 using namespace mvsim;
14 using namespace std;
15 
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 {
68  std::map<std::string, TParamEntry> params;
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  keep_min(
102  setpoint_steer_ang, m_veh.getMaxSteeringAngle());
103  break;
104 
105  case 'D':
106  case 'd':
107  setpoint_steer_ang -= 1.0 * M_PI / 180.0;
108  keep_max(
109  setpoint_steer_ang, -m_veh.getMaxSteeringAngle());
110  break;
111  case ' ':
112  setpoint_lin_speed = .0;
113  break;
114  };
115  out.append_gui_lines += "[Controller=" + string(class_name()) +
116  "] Teleop keys: w/s=incr/decr lin speed. "
117  "a/d=left/right steering. spacebar=stop.\n";
118  out.append_gui_lines += mrpt::format(
119  "setpoint: v=%.03f steer=%.03f deg\n", setpoint_lin_speed,
120  setpoint_steer_ang * 180.0 / M_PI);
121 }
double max_torque
Maximum abs. value torque (for clamp) [Nm].
virtual void teleop_interface(const TeleopInput &in, TeleopOutput &out) override
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const std::map< std::string, TParamEntry > &params, const char *function_name_context="")
Definition: xml_utils.cpp:196
virtual void control_step(const DynamicsAckermannDrivetrain::TControllerInput &ci, DynamicsAckermannDrivetrain::TControllerOutput &co)
double steer_ang
Equivalent ackerman steering angle.
TFSIMD_FORCE_INLINE const tfScalar & w() const
virtual void control_step(const DynamicsAckermannDrivetrain::TControllerInput &ci, DynamicsAckermannDrivetrain::TControllerOutput &co)
double max_torque
Maximum abs. value torque (for clamp) [Nm].


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:40