Vehicleackermann_Drivetrain_ControllerTwistFrontSteerPID.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::ControllerTwistFrontSteerPID::
00017 	ControllerTwistFrontSteerPID(DynamicsAckermannDrivetrain& veh)
00018         : ControllerBase(veh),
00019           setpoint_lin_speed(0),
00020           setpoint_ang_speed(0),
00021           KP(100),
00022           KI(0),
00023           KD(0),
00024           max_torque(400.0)
00025 {
00026         // Get distance between wheels:
00027         m_dist_fWheels =
00028                 m_veh.m_wheels_info[WHEEL_FL].y - m_veh.m_wheels_info[WHEEL_FR].y;
00029         m_r2f_L = m_veh.m_wheels_info[WHEEL_FL].x - m_veh.m_wheels_info[WHEEL_RL].x;
00030 
00031         ASSERT_(m_dist_fWheels > 0.0);
00032         ASSERT_(m_r2f_L > 0.0);
00033 }
00034 
00035 void DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID::control_step(
00036         const DynamicsAckermannDrivetrain::TControllerInput& ci,
00037         DynamicsAckermannDrivetrain::TControllerOutput& co)
00038 {
00039         // 1st: desired steering angle:
00040         // --------------------------------
00041         if (setpoint_ang_speed == 0)
00042         {
00043                 co.steer_ang = 0.0;
00044         }
00045         else
00046         {
00047                 const double R = setpoint_lin_speed / setpoint_ang_speed;
00048                 co.steer_ang = atan(m_r2f_L / R);
00049         }
00050 
00051         m_PID.KP = KP;
00052         m_PID.KI = KI;
00053         m_PID.KD = KD;
00054         m_PID.max_out = max_torque;
00055 
00056         const double vel_act = m_veh.getVelocityLocalOdoEstimate().vals[0];
00057         const double vel_des = setpoint_lin_speed;
00058 
00059         co.drive_torque = -m_PID.compute(
00060                 vel_des - vel_act,
00061                 ci.context.dt);  // "-" because \tau<0 makes robot moves forwards.
00062 }
00063 
00064 void DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID::load_config(
00065         const rapidxml::xml_node<char>& node)
00066 {
00067         std::map<std::string, TParamEntry> params;
00068         params["KP"] = TParamEntry("%lf", &KP);
00069         params["KI"] = TParamEntry("%lf", &KI);
00070         params["KD"] = TParamEntry("%lf", &KD);
00071         params["max_torque"] = TParamEntry("%lf", &max_torque);
00072 
00073         // Initial speed.
00074         params["V"] = TParamEntry("%lf", &this->setpoint_lin_speed);
00075         params["W"] = TParamEntry("%lf_deg", &this->setpoint_ang_speed);
00076 
00077         parse_xmlnode_children_as_param(node, params);
00078 }
00079 
00080 void DynamicsAckermannDrivetrain::ControllerTwistFrontSteerPID::
00081 	teleop_interface(const TeleopInput& in, TeleopOutput& out)
00082 {
00083         ControllerBase::teleop_interface(in, out);
00084 
00085         switch (in.keycode)
00086         {
00087                 case 'W':
00088                 case 'w':
00089                         setpoint_lin_speed += 0.1;
00090                         break;
00091 
00092                 case 'S':
00093                 case 's':
00094                         setpoint_lin_speed -= 0.1;
00095                         break;
00096 
00097                 case 'A':
00098                 case 'a':
00099                         setpoint_ang_speed += 1.0 * M_PI / 180.0;
00100                         break;
00101 
00102                 case 'D':
00103                 case 'd':
00104                         setpoint_ang_speed -= 1.0 * M_PI / 180.0;
00105                         break;
00106 
00107                 case ' ':
00108                         setpoint_lin_speed = .0;
00109                         setpoint_ang_speed = .0;
00110                         break;
00111         };
00112         out.append_gui_lines += "[Controller=" + string(class_name()) +
00113                                                         "] Teleop keys: w/s=incr/decr lin speed. "
00114                                                         "a/d=left/right steering. spacebar=stop.\n";
00115         out.append_gui_lines += mrpt::format(
00116                 "setpoint: v=%.03f w=%.03f deg/s\n", setpoint_lin_speed,
00117                 setpoint_ang_speed * 180.0 / M_PI);
00118 }


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