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::ControllerFrontSteerPID::ControllerFrontSteerPID(
00017 DynamicsAckermann& 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
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
00033 void DynamicsAckermann::ControllerFrontSteerPID::control_step(
00034 const DynamicsAckermann::TControllerInput& ci,
00035 DynamicsAckermann::TControllerOutput& co)
00036 {
00037
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
00047
00048 const double R = m_r2f_L / tan(setpoint_steer_ang);
00049 w = v / R;
00050 }
00051
00052
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;
00063 }
00064
00065 void DynamicsAckermann::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
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 DynamicsAckermann::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 }