VehicleAckermann_Drivetrain_ControllerFrontSteerPID.cpp
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
00008   +-------------------------------------------------------------------------+ */
00009 
00010 #include <mvsim/VehicleDynamics/VehicleAckermann_Drivetrain.h>
00011 #include "xml_utils.h"
00012 
00013 using namespace mvsim;
00014 using namespace std;
00015 
00016 DynamicsAckermannDrivetrain::ControllerFrontSteerPID::ControllerFrontSteerPID(
00017         DynamicsAckermannDrivetrain& veh)
00018         : ControllerBase(veh),
00019           setpoint_lin_speed(0),
00020           setpoint_steer_ang(0),
00021           KP(100),
00022           KI(0),
00023           KD(0),
00024           max_torque(100.0),
00025           m_twist_control(veh)
00026 {
00027         // Get distance between wheels:
00028         m_r2f_L = m_veh.m_wheels_info[WHEEL_FL].x - m_veh.m_wheels_info[WHEEL_RL].x;
00029         ASSERT_(m_r2f_L > 0.0);
00030 }
00031 
00032 // See base class docs
00033 void DynamicsAckermannDrivetrain::ControllerFrontSteerPID::control_step(
00034         const DynamicsAckermannDrivetrain::TControllerInput& ci,
00035         DynamicsAckermannDrivetrain::TControllerOutput& co)
00036 {
00037         // Equivalent v/w velocities:
00038         const double v = setpoint_lin_speed;
00039         double w;
00040         if (setpoint_steer_ang == 0.0)
00041         {
00042                 w = 0.0;
00043         }
00044         else
00045         {
00046                 // ang = atan(r2f_L/R)  ->  R= r2f_L / tan(ang)
00047                 // R = v/w              ->   w=v/R
00048                 const double R = m_r2f_L / tan(setpoint_steer_ang);
00049                 w = v / R;
00050         }
00051 
00052         // Let the twist controller do the calculations:
00053         m_twist_control.setpoint_lin_speed = v;
00054         m_twist_control.setpoint_ang_speed = w;
00055 
00056         m_twist_control.KP = KP;
00057         m_twist_control.KI = KI;
00058         m_twist_control.KD = KD;
00059         m_twist_control.max_torque = max_torque;
00060 
00061         m_twist_control.control_step(ci, co);
00062         co.steer_ang = setpoint_steer_ang;  // Mainly for the case of v=0
00063 }
00064 
00065 void DynamicsAckermannDrivetrain::ControllerFrontSteerPID::load_config(
00066         const rapidxml::xml_node<char>& node)
00067 {
00068         std::map<std::string, TParamEntry> params;
00069         params["KP"] = TParamEntry("%lf", &KP);
00070         params["KI"] = TParamEntry("%lf", &KI);
00071         params["KD"] = TParamEntry("%lf", &KD);
00072         params["max_torque"] = TParamEntry("%lf", &max_torque);
00073 
00074         // Initial speed.
00075         params["V"] = TParamEntry("%lf", &this->setpoint_lin_speed);
00076         params["STEER_ANG"] = TParamEntry("%lf_deg", &this->setpoint_steer_ang);
00077 
00078         parse_xmlnode_children_as_param(node, params);
00079 }
00080 
00081 void DynamicsAckermannDrivetrain::ControllerFrontSteerPID::teleop_interface(
00082         const TeleopInput& in, TeleopOutput& out)
00083 {
00084         ControllerBase::teleop_interface(in, out);
00085 
00086         switch (in.keycode)
00087         {
00088                 case 'W':
00089                 case 'w':
00090                         setpoint_lin_speed += 0.1;
00091                         break;
00092 
00093                 case 'S':
00094                 case 's':
00095                         setpoint_lin_speed -= 0.1;
00096                         break;
00097 
00098                 case 'A':
00099                 case 'a':
00100                         setpoint_steer_ang += 1.0 * M_PI / 180.0;
00101                         keep_min(
00102                                 setpoint_steer_ang, m_veh.getMaxSteeringAngle());
00103                         break;
00104 
00105                 case 'D':
00106                 case 'd':
00107                         setpoint_steer_ang -= 1.0 * M_PI / 180.0;
00108                         keep_max(
00109                                 setpoint_steer_ang, -m_veh.getMaxSteeringAngle());
00110                         break;
00111                 case ' ':
00112                         setpoint_lin_speed = .0;
00113                         break;
00114         };
00115         out.append_gui_lines += "[Controller=" + string(class_name()) +
00116                                                         "] Teleop keys: w/s=incr/decr lin speed. "
00117                                                         "a/d=left/right steering. spacebar=stop.\n";
00118         out.append_gui_lines += mrpt::format(
00119                 "setpoint: v=%.03f steer=%.03f deg\n", setpoint_lin_speed,
00120                 setpoint_steer_ang * 180.0 / M_PI);
00121 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35