Fawkes API Fawkes Development Version
conversion.h
1
2/***************************************************************************
3 * conversion.h - conversion between encoder and radian motor values
4 *
5 * Created: Thu Dec 02 13:51:46 2010
6 * Copyright 2010 Bahram Maleki-Fard
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version. A runtime exception applies to
14 * this software (see LICENSE.GPL_WRE file mentioned below for details).
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22 */
23
24#ifndef PLUGINS_KATANA_CONVERSION_H_
25#define PLUGINS_KATANA_CONVERSION_H_
26
27#include <common/MathHelperFunctions.h>
28#include <utils/math/angle.h>
29
30#include <vector>
31using namespace KNI_MHF;
32
33namespace fawkes {
34
35/** Convert radian vaulues of katana arm to encoder values
36 * @param rad vector with radian angle values
37 * @param enc vector to be filled with encoder values
38 */
39inline void
40radToEnc(std::vector<float> &rad, std::vector<int> &enc)
41{
42 enc.clear();
43
44 enc.push_back(rad2enc(rad[0], deg2rad(6.65f), 51200, 31000, 1));
45 enc.push_back(rad2enc(rad[1], deg2rad(124.25f), 94976, -31000, 1));
46 enc.push_back(rad2enc(rad[2], deg2rad(52.70f), 81408, -31000, -1));
47 enc.push_back(rad2enc(rad[3], deg2rad(63.50f), 51200, 31000, 1));
48 enc.push_back(rad2enc(rad[4], deg2rad(8.50f), 51200, 31000, 1));
49}
50
51/** Convert encoder vaulues of katana arm to radian angles.
52 * @param enc vector with encoder values, received from CKatana::getRobotEncoders
53 * @param rad vector to be filled with angle values
54 */
55inline void
56encToRad(std::vector<int> &enc, std::vector<float> &rad)
57{
58 rad.clear();
59
60 rad.push_back(enc2rad(enc[0], deg2rad(6.65f), 51200, 31000, 1));
61 rad.push_back(enc2rad(enc[1], deg2rad(124.25f), 94976, -31000, 1));
62 rad.push_back(enc2rad(enc[2], deg2rad(52.70f), 81408, -31000, -1));
63 rad.push_back(enc2rad(enc[3], deg2rad(63.50f), 51200, 31000, 1));
64 rad.push_back(enc2rad(enc[4], deg2rad(8.50f), 51200, 31000, 1));
65}
66
67} // end namespace fawkes
68
69#endif
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.
Definition: conversion.h:56
void radToEnc(std::vector< float > &rad, std::vector< int > &enc)
Convert radian vaulues of katana arm to encoder values.
Definition: conversion.h:40