VehicleDifferential_ControllerTwistPID.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/VehicleDifferential.h>
00011 #include "xml_utils.h"
00012 
00013 using namespace mvsim;
00014 using namespace std;
00015 
00016 DynamicsDifferential::ControllerTwistPID::ControllerTwistPID(
00017         DynamicsDifferential& 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(100.0)
00025 {
00026         // Get distance between wheels:
00027         // Warning: the controller *assumes* that both wheels are parallel (as it's
00028         // a rule in differential robots!!)
00029         m_distWheels = m_veh.m_wheels_info[0].y - m_veh.m_wheels_info[1].y;
00030 }
00031 
00032 // See base class docs
00033 void DynamicsDifferential::ControllerTwistPID::control_step(
00034         const DynamicsDifferential::TControllerInput& ci,
00035         DynamicsDifferential::TControllerOutput& co)
00036 {
00037         // For each wheel:
00038         // 1) Compute desired velocity set-point (in m/s)
00039         // 2) Run the PI/PID for that wheel independently (in newtons)
00040         const double vel_l =
00041                 setpoint_lin_speed - 0.5 * setpoint_ang_speed * m_distWheels;
00042         const double vel_r =
00043                 setpoint_lin_speed + 0.5 * setpoint_ang_speed * m_distWheels;
00044 
00045         // Compute each wheel actual velocity (from an "odometry" estimation of
00046         // velocity, not ground-truth!):
00047         const vec3 vehVelOdo = m_veh.getVelocityLocalOdoEstimate();
00048         const double act_vel_l =
00049                 vehVelOdo.vals[0] - 0.5 * vehVelOdo.vals[2] * m_distWheels;
00050         const double act_vel_r =
00051                 vehVelOdo.vals[0] + 0.5 * vehVelOdo.vals[2] * m_distWheels;
00052 
00053         // Apply controller:
00054         for (int i = 0; i < 2; i++)
00055         {
00056                 m_PID[i].KP = KP;
00057                 m_PID[i].KI = KI;
00058                 m_PID[i].KD = KD;
00059                 m_PID[i].max_out = max_torque;
00060         }
00061 
00062         co.wheel_torque_l = -m_PID[0].compute(
00063                 vel_l - act_vel_l,
00064                 ci.context.dt);  // "-" because \tau<0 makes robot moves forwards.
00065         co.wheel_torque_r = -m_PID[1].compute(vel_r - act_vel_r, ci.context.dt);
00066 }
00067 
00068 void DynamicsDifferential::ControllerTwistPID::load_config(
00069         const rapidxml::xml_node<char>& node)
00070 {
00071         std::map<std::string, TParamEntry> params;
00072         params["KP"] = TParamEntry("%lf", &KP);
00073         params["KI"] = TParamEntry("%lf", &KI);
00074         params["KD"] = TParamEntry("%lf", &KD);
00075         params["max_torque"] = TParamEntry("%lf", &max_torque);
00076 
00077         // Initial speed.
00078         params["V"] = TParamEntry("%lf", &this->setpoint_lin_speed);
00079         params["W"] = TParamEntry("%lf_deg", &this->setpoint_ang_speed);
00080 
00081         parse_xmlnode_children_as_param(node, params);
00082 }
00083 
00084 void DynamicsDifferential::ControllerTwistPID::teleop_interface(
00085         const TeleopInput& in, TeleopOutput& out)
00086 {
00087         ControllerBase::teleop_interface(in, out);
00088 
00089         switch (in.keycode)
00090         {
00091                 case 'W':
00092                 case 'w':
00093                         setpoint_lin_speed += 0.1;
00094                         break;
00095 
00096                 case 'S':
00097                 case 's':
00098                         setpoint_lin_speed -= 0.1;
00099                         break;
00100 
00101                 case 'A':
00102                 case 'a':
00103                         setpoint_ang_speed += 2.0 * M_PI / 180;
00104                         break;
00105 
00106                 case 'D':
00107                 case 'd':
00108                         setpoint_ang_speed -= 2.0 * M_PI / 180;
00109                         break;
00110 
00111                 case ' ':
00112                         setpoint_lin_speed = 0.0;
00113                         setpoint_ang_speed = 0.0;
00114                         break;
00115         };
00116         out.append_gui_lines +=
00117                 "[Controller=" + string(class_name()) +
00118                 "] Teleop keys: w/s=forward/backward. a/d=left/right. spacebar=stop.\n";
00119         out.append_gui_lines += mrpt::format(
00120                 "setpoint: lin=%.03f ang=%.03f deg/s\n", setpoint_lin_speed,
00121                 180.0 / M_PI * setpoint_ang_speed);
00122 }


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