Fawkes API  Fawkes Development Version
conversion.h
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