Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
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
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);
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
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 }