VehicleAckermann_Drivetrain_ControllerRaw.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::ControllerRawForces::ControllerRawForces(
00017         DynamicsAckermannDrivetrain& veh)
00018         : ControllerBase(veh), setpoint_wheel_torque(0), setpoint_steer_ang(0)
00019 {
00020         MRPT_TODO("I see this file!")
00021 }
00022 
00023 // See base class docs
00024 void DynamicsAckermannDrivetrain::ControllerRawForces::control_step(
00025         const DynamicsAckermannDrivetrain::TControllerInput& ci,
00026         DynamicsAckermannDrivetrain::TControllerOutput& co)
00027 {
00028         co.drive_torque = this->setpoint_wheel_torque;
00029         co.steer_ang = this->setpoint_steer_ang;
00030 }
00031 
00032 void DynamicsAckermannDrivetrain::ControllerRawForces::load_config(
00033         const rapidxml::xml_node<char>& node)
00034 {
00035         std::map<std::string, TParamEntry> params;
00036         params["torque"] = TParamEntry("%lf", &setpoint_wheel_torque);
00037 
00038         // Initial speed.
00039         params["steer_ang_deg"] = TParamEntry("%lf_deg", &this->setpoint_steer_ang);
00040 
00041         parse_xmlnode_children_as_param(node, params);
00042 }
00043 
00044 void DynamicsAckermannDrivetrain::ControllerRawForces::teleop_interface(
00045         const TeleopInput& in, TeleopOutput& out)
00046 {
00047         ControllerBase::teleop_interface(in, out);
00048 
00049         switch (in.keycode)
00050         {
00051                 case 'W':
00052                 case 'w':
00053                         setpoint_wheel_torque -= 1.0;
00054                         break;
00055 
00056                 case 'S':
00057                 case 's':
00058                         setpoint_wheel_torque += 1.0;
00059                         break;
00060 
00061                 case 'A':
00062                 case 'a':
00063                         setpoint_steer_ang += 1.0 * M_PI / 180.0;
00064                         keep_min(
00065                                 setpoint_steer_ang, m_veh.getMaxSteeringAngle());
00066                         break;
00067 
00068                 case 'D':
00069                 case 'd':
00070                         setpoint_steer_ang -= 1.0 * M_PI / 180.0;
00071                         keep_max(
00072                                 setpoint_steer_ang, -m_veh.getMaxSteeringAngle());
00073                         break;
00074 
00075                 case ' ':
00076                         setpoint_wheel_torque = .0;
00077                         break;
00078         };
00079 
00080         out.append_gui_lines += "[Controller=" + string(class_name()) +
00081                                                         "] Teleop keys: w/s=incr/decr torques. "
00082                                                         "a/d=left/right steering. spacebar=stop.\n";
00083         out.append_gui_lines += mrpt::format(
00084                 "setpoint: t=%.03f steer=%.03f deg\n", setpoint_wheel_torque,
00085                 setpoint_steer_ang * 180.0 / M_PI);
00086 }


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