cob_canopen_motor: cob_canopen_motor: DriveParam.h Source File

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_canopen_motor
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Feb 2010
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 
00055 #ifndef DRIVEPARAM_INCLUDEDEF_H
00056 #define DRIVEPARAM_INCLUDEDEF_H
00057 
00058 //-----------------------------------------------
00059 
00060 
00065 class DriveParam
00066 {
00067 
00068 private:
00069 
00070         int m_iDriveIdent;
00071         int m_iEncIncrPerRevMot;        // encoder increments per revolution motor shaft 
00072         double m_dVelMeasFrqHz;         // only used for Neo drive, else = 1
00073         double m_dGearRatio;            //
00074         double m_dBeltRatio;            // if drive has belt set ratio, else = 1
00075         int m_iSign;                            // direction of motion
00076         double m_dVelMaxEncIncrS;       // max. veloctiy
00077         double m_dAccIncrS2;            // max. acceleration
00078         double m_dDecIncrS2;            // max. decelration
00079         double m_dPosGearRadToPosMotIncr;
00080         int m_iEncOffsetIncr;           // Position in Increments of Steerwheel when Homingposition
00081                                         //  is reached (only needed forCoB3)
00082         int m_iHomingDigIn; // specifies which digital input is used for homing signal, standart 11 is good for COB3, 19 for Cob3_5
00083         bool m_bIsSteer;                // needed to distinguish motors for initializing
00084     double m_dCurrToTorque;             // factor to convert motor active current [A] into torque [Nm]
00085         double m_dCurrMax;              // max. current allowed
00086 
00087 public:
00088 
00092         DriveParam()
00093         {
00094         
00095                 m_bIsSteer = true; //has to be set, because it is checked for absolute / relative positioning
00096         
00097         }
00098 
00111         void setParam(
00112                 int iDriveIdent,
00113                 int iEncIncrPerRevMot,
00114                 double dVelMeasFrqHz,
00115                 double dBeltRatio,
00116                 double dGearRatio,
00117                 int iSign,
00118                 double dVelMaxEncIncrS,
00119                 double dAccIncrS2,
00120                 double dDecIncrS2)
00121         {
00122 
00123                 m_iDriveIdent = iDriveIdent;
00124                 m_iEncIncrPerRevMot = iEncIncrPerRevMot;
00125                 m_dVelMeasFrqHz = dVelMeasFrqHz;
00126                 m_dBeltRatio = dBeltRatio;
00127                 m_dGearRatio = dGearRatio;
00128                 m_iSign = iSign;
00129                 m_dVelMaxEncIncrS = dVelMaxEncIncrS;
00130                 m_dAccIncrS2 = dAccIncrS2;
00131                 m_dDecIncrS2 = dDecIncrS2;
00132                 
00133                 m_iHomingDigIn = 11; //for Cob3
00134                 
00135                 double dPI = 3.14159265358979323846;
00136 
00137                 m_dPosGearRadToPosMotIncr = m_iEncIncrPerRevMot * m_dGearRatio
00138                         * m_dBeltRatio / (2. * dPI);
00139         }
00140 
00141         //Overloaded Method for CoB3
00142         void setParam(
00143                 int iDriveIdent,
00144                 int iEncIncrPerRevMot,
00145                 double dVelMeasFrqHz,
00146                 double dBeltRatio,
00147                 double dGearRatio,
00148                 int iSign,
00149                 double dVelMaxEncIncrS,
00150                 double dAccIncrS2,
00151                 double dDecIncrS2,
00152                 int iEncOffsetIncr,
00153                 bool bIsSteer,
00154         double dCurrToTorque,
00155                 double dCurrMax)
00156         {
00157 
00158                 m_iDriveIdent = iDriveIdent;
00159                 m_iEncIncrPerRevMot = iEncIncrPerRevMot;
00160                 m_dVelMeasFrqHz = dVelMeasFrqHz;
00161                 m_dBeltRatio = dBeltRatio;
00162                 m_dGearRatio = dGearRatio;
00163                 m_iSign = iSign;
00164                 m_dVelMaxEncIncrS = dVelMaxEncIncrS;
00165                 m_dAccIncrS2 = dAccIncrS2;
00166                 m_dDecIncrS2 = dDecIncrS2;
00167                 m_iEncOffsetIncr = iEncOffsetIncr;
00168                 m_bIsSteer = bIsSteer;
00169                 
00170                 m_iHomingDigIn = 11; //for Cob3
00171 
00172                 double dPI = 3.14159265358979323846;
00173 
00174                 m_dPosGearRadToPosMotIncr = m_iEncIncrPerRevMot * m_dGearRatio
00175                         * m_dBeltRatio / (2. * dPI);
00176 
00177         m_dCurrToTorque = dCurrToTorque;
00178                 m_dCurrMax = dCurrMax;
00179         }
00180         
00181         //Overloaded Method for CoB3 including new feature HomingDigIn, for compatibility reasons overloaded
00182         void setParam(
00183                 int iDriveIdent,
00184                 int iEncIncrPerRevMot,
00185                 double dVelMeasFrqHz,
00186                 double dBeltRatio,
00187                 double dGearRatio,
00188                 int iSign,
00189                 double dVelMaxEncIncrS,
00190                 double dAccIncrS2,
00191                 double dDecIncrS2,
00192                 int iEncOffsetIncr,
00193                 bool bIsSteer,
00194         double dCurrToTorque,
00195                 double dCurrMax,
00196                 int iHomingDigIn)
00197         {
00198 
00199                 m_iDriveIdent = iDriveIdent;
00200                 m_iEncIncrPerRevMot = iEncIncrPerRevMot;
00201                 m_dVelMeasFrqHz = dVelMeasFrqHz;
00202                 m_dBeltRatio = dBeltRatio;
00203                 m_dGearRatio = dGearRatio;
00204                 m_iSign = iSign;
00205                 m_dVelMaxEncIncrS = dVelMaxEncIncrS;
00206                 m_dAccIncrS2 = dAccIncrS2;
00207                 m_dDecIncrS2 = dDecIncrS2;
00208                 m_iEncOffsetIncr = iEncOffsetIncr;
00209                 m_bIsSteer = bIsSteer;
00210 
00211                 double dPI = 3.14159265358979323846;
00212 
00213                 m_dPosGearRadToPosMotIncr = m_iEncIncrPerRevMot * m_dGearRatio
00214                         * m_dBeltRatio / (2. * dPI);
00215 
00216         m_dCurrToTorque = dCurrToTorque;
00217                 m_dCurrMax = dCurrMax;
00218                 m_iHomingDigIn = iHomingDigIn;
00219         }
00220 
00224         int getDriveIdent()
00225         {
00226                 return m_iDriveIdent;
00227         }
00228         
00232         int getSign()
00233         {
00234                 return m_iSign;
00235         }
00236         
00240         double getVelMax()
00241         {
00242                 return m_dVelMaxEncIncrS;
00243         }
00244         
00252         void PosVelRadToIncr(double dPosRad, double dVelRadS, int* piPosIncr, int* piVelIncrPeriod)
00253         {
00254                 *piPosIncr = PosGearRadToPosMotIncr(dPosRad);
00255                 *piVelIncrPeriod = VelGearRadSToVelMotIncrPeriod(dVelRadS);
00256         }
00257 
00263         int TempMeasIncrToGradCel(int iTempIncr)
00264         {
00265                 double dTempMeasGradCel;
00266 
00267                 dTempMeasGradCel = 0.0002 * (iTempIncr * iTempIncr) - 0.2592 * iTempIncr + 105;
00268 
00269                 return (int)dTempMeasGradCel;
00270         }
00271 
00276         int PosGearRadToPosMotIncr(double dPosGearRad)
00277         {
00278                 return ((int)(dPosGearRad * m_dPosGearRadToPosMotIncr));
00279         }
00280         
00282         double PosMotIncrToPosGearRad(int iPosIncr)
00283         {
00284                 return ((double)iPosIncr / m_dPosGearRadToPosMotIncr);
00285         }
00286         
00288         int VelGearRadSToVelMotIncrPeriod(double dVelGearRadS)
00289         {
00290                 return ((int)(dVelGearRadS * m_dPosGearRadToPosMotIncr / m_dVelMeasFrqHz));
00291         }
00292         
00294         double VelMotIncrPeriodToVelGearRadS(int iVelMotIncrPeriod)
00295         {
00296                 return ((double)iVelMotIncrPeriod / m_dPosGearRadToPosMotIncr * m_dVelMeasFrqHz);
00297         }
00298         
00303         void setMaxAcc(double dMaxAcc)
00304         {
00305                 m_dAccIncrS2 = dMaxAcc;
00306         }
00307 
00312         double getMaxAcc()
00313         {
00314                 return m_dAccIncrS2;
00315         }
00316 
00321         void setMaxDec(double dMaxDec)
00322         {
00323                 m_dDecIncrS2 = dMaxDec;
00324         }
00325 
00330         double getMaxDec()
00331         {
00332                 return m_dDecIncrS2;
00333         }
00334 
00339         void setMaxVel(double dMaxVel)
00340         {
00341                 m_dVelMaxEncIncrS = dMaxVel;
00342         }
00343 
00348         double getMaxVel()
00349         {
00350                 return m_dVelMaxEncIncrS;
00351         }
00352         
00357         double getGearRatio()
00358         {
00359                 return m_dGearRatio;
00360         }
00365         double getBeltRatio()
00366         {
00367                 return m_dBeltRatio;
00368         }
00369         
00374         int getEncOffset()
00375         {
00376                 return m_iEncOffsetIncr;
00377         }
00378         
00383         bool getIsSteer()
00384         {
00385                 return m_bIsSteer;
00386         }
00391         int getEncIncrPerRevMot()
00392         {
00393                 return m_iEncIncrPerRevMot;
00394         }
00398         double getCurrToTorque()
00399         {
00400                 return m_dCurrToTorque;
00401         }
00405         double getCurrMax()
00406         {
00407                 return m_dCurrMax;
00408         }
00412         int getHomingDigIn()
00413         {
00414                 return m_iHomingDigIn;
00415         }
00419         void setHomingDigIn(int HomingDigIn)
00420         {
00421                 m_iHomingDigIn = HomingDigIn;
00422         }
00423 };
00424 //-----------------------------------------------
00425 #endif