Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * conversion.h - conversion between encoder and radian motor values 00004 * 00005 * Created: Thu Dec 02 13:51:46 2010 00006 * Copyright 2010 Bahram Maleki-Fard 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #ifndef __PLUGINS_KATANA_CONVERSION_H 00025 #define __PLUGINS_KATANA_CONVERSION_H 00026 00027 #include <utils/math/angle.h> 00028 #include <common/MathHelperFunctions.h> 00029 00030 #include <vector> 00031 using namespace KNI_MHF; 00032 00033 namespace fawkes { 00034 00035 /** Convert radian vaulues of katana arm to encoder values 00036 * @param rad vector with radian angle values 00037 * @param enc vector to be filled with encoder values 00038 */ 00039 inline void 00040 radToEnc(std::vector<float>& rad, std::vector<int>& enc) 00041 { 00042 enc.clear(); 00043 00044 enc.push_back(rad2enc(rad[0], deg2rad( 6.65f), 51200, 31000, 1)); 00045 enc.push_back(rad2enc(rad[1], deg2rad(124.25f), 94976, -31000, 1)); 00046 enc.push_back(rad2enc(rad[2], deg2rad( 52.70f), 81408, -31000, -1)); 00047 enc.push_back(rad2enc(rad[3], deg2rad( 63.50f), 51200, 31000, 1)); 00048 enc.push_back(rad2enc(rad[4], deg2rad( 8.50f), 51200, 31000, 1)); 00049 } 00050 00051 /** Convert encoder vaulues of katana arm to radian angles. 00052 * @param enc vector with encoder values, received from CKatana::getRobotEncoders 00053 * @param rad vector to be filled with angle values 00054 */ 00055 inline void 00056 encToRad(std::vector<int>& enc, std::vector<float>& rad) 00057 { 00058 rad.clear(); 00059 00060 rad.push_back(enc2rad(enc[0], deg2rad( 6.65f), 51200, 31000, 1)); 00061 rad.push_back(enc2rad(enc[1], deg2rad(124.25f), 94976, -31000, 1)); 00062 rad.push_back(enc2rad(enc[2], deg2rad( 52.70f), 81408, -31000, -1)); 00063 rad.push_back(enc2rad(enc[3], deg2rad( 63.50f), 51200, 31000, 1)); 00064 rad.push_back(enc2rad(enc[4], deg2rad( 8.50f), 51200, 31000, 1)); 00065 } 00066 00067 } // end namespace fawkes 00068 00069 #endif