Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <mvsim/VehicleDynamics/VehicleAckermann.h>
00011 #include "xml_utils.h"
00012
00013 using namespace mvsim;
00014 using namespace std;
00015
00016 DynamicsAckermann::ControllerRawForces::ControllerRawForces(
00017 DynamicsAckermann& veh)
00018 : ControllerBase(veh),
00019 setpoint_wheel_torque_l(0),
00020 setpoint_wheel_torque_r(0),
00021 setpoint_steer_ang(0)
00022 {
00023 }
00024
00025
00026 void DynamicsAckermann::ControllerRawForces::control_step(
00027 const DynamicsAckermann::TControllerInput& ci,
00028 DynamicsAckermann::TControllerOutput& co)
00029 {
00030 co.fl_torque = this->setpoint_wheel_torque_l;
00031 co.fr_torque = this->setpoint_wheel_torque_r;
00032 co.steer_ang = this->setpoint_steer_ang;
00033 }
00034
00035 void DynamicsAckermann::ControllerRawForces::load_config(
00036 const rapidxml::xml_node<char>& node)
00037 {
00038 std::map<std::string, TParamEntry> params;
00039 params["fl_torque"] = TParamEntry("%lf", &setpoint_wheel_torque_l);
00040 params["fr_torque"] = TParamEntry("%lf", &setpoint_wheel_torque_r);
00041
00042
00043 params["steer_ang_deg"] = TParamEntry("%lf_deg", &this->setpoint_steer_ang);
00044
00045 parse_xmlnode_children_as_param(node, params);
00046 }
00047
00048 void DynamicsAckermann::ControllerRawForces::teleop_interface(
00049 const TeleopInput& in, TeleopOutput& out)
00050 {
00051 ControllerBase::teleop_interface(in, out);
00052
00053 switch (in.keycode)
00054 {
00055 case 'W':
00056 case 'w':
00057 setpoint_wheel_torque_l -= 1.0;
00058 setpoint_wheel_torque_r -= 1.0;
00059 break;
00060
00061 case 'S':
00062 case 's':
00063 setpoint_wheel_torque_l += 1.0;
00064 setpoint_wheel_torque_r += 1.0;
00065 break;
00066
00067 case 'A':
00068 case 'a':
00069 setpoint_steer_ang += 1.0 * M_PI / 180.0;
00070 keep_min(
00071 setpoint_steer_ang, m_veh.getMaxSteeringAngle());
00072 break;
00073
00074 case 'D':
00075 case 'd':
00076 setpoint_steer_ang -= 1.0 * M_PI / 180.0;
00077 keep_max(
00078 setpoint_steer_ang, -m_veh.getMaxSteeringAngle());
00079 break;
00080
00081 case ' ':
00082 setpoint_wheel_torque_l = .0;
00083 setpoint_wheel_torque_r = .0;
00084 break;
00085 };
00086 out.append_gui_lines += "[Controller=" + string(class_name()) +
00087 "] Teleop keys: w/s=incr/decr torques. "
00088 "a/d=left/right steering. spacebar=stop.\n";
00089 out.append_gui_lines += mrpt::format(
00090 "setpoint: t=%.03f steer=%.03f deg\n", setpoint_wheel_torque_l,
00091 setpoint_steer_ang * 180.0 / M_PI);
00092 }