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 <mvsim/World.h> 00012 //#include <rapidxml.hpp> 00013 00014 using namespace mvsim; 00015 using namespace std; 00016 00017 // See base class docs 00018 void DynamicsDifferential::ControllerRawForces::control_step( 00019 const DynamicsDifferential::TControllerInput& ci, 00020 DynamicsDifferential::TControllerOutput& co) 00021 { 00022 co.wheel_torque_l = this->setpoint_wheel_torque_l; 00023 co.wheel_torque_r = this->setpoint_wheel_torque_r; 00024 } 00025 00026 void DynamicsDifferential::ControllerRawForces::teleop_interface( 00027 const TeleopInput& in, TeleopOutput& out) 00028 { 00029 ControllerBase::teleop_interface(in, out); 00030 00031 switch (in.keycode) 00032 { 00033 case 'W': 00034 case 'w': 00035 setpoint_wheel_torque_l -= 0.5; 00036 setpoint_wheel_torque_r -= 0.5; 00037 break; 00038 00039 case 'S': 00040 case 's': 00041 setpoint_wheel_torque_l += 0.5; 00042 setpoint_wheel_torque_r += 0.5; 00043 break; 00044 00045 case 'A': 00046 case 'a': 00047 setpoint_wheel_torque_l += 0.5; 00048 setpoint_wheel_torque_r -= 0.5; 00049 break; 00050 00051 case 'D': 00052 case 'd': 00053 setpoint_wheel_torque_l -= 0.5; 00054 setpoint_wheel_torque_r += 0.5; 00055 break; 00056 00057 case ' ': 00058 setpoint_wheel_torque_l = setpoint_wheel_torque_r = 0.0; 00059 break; 00060 }; 00061 out.append_gui_lines += "[Controller=" + string(class_name()) + 00062 "] Teleop keys: w/s=incr/decr both torques. " 00063 "a/d=left/right. spacebar=stop.\n"; 00064 out.append_gui_lines += mrpt::format( 00065 "setpoint: tl=%.03f tr=%.03f deg\n", setpoint_wheel_torque_l, 00066 setpoint_wheel_torque_r); 00067 }