DriveParam.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Neobotix GmbH
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Neobotix nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 
00036 #ifndef DRIVEPARAM_INCLUDEDEF_H
00037 #define DRIVEPARAM_INCLUDEDEF_H
00038 
00039 //-----------------------------------------------
00040 
00041 const double PI = 3.14159265358979323846;
00042 
00046 class DriveParam
00047 {
00048 public:
00049 
00050         enum TypeEncoder
00051         {
00052                 ENCODER_NO,
00053                 ENCODER_INCREMENTAL,
00054                 ENCODER_ABSOLUTE
00055         };
00056 
00060         DriveParam()
00061         {
00062                 m_iDriveIdent = 0;
00063                 m_iEncIncrPerRevMot = 0;
00064                 m_dVelMeasFrqHz = 0;
00065                 m_dGearRatio = 0;
00066                 m_dBeltRatio = 0;
00067                 m_iSign = 0;
00068                 m_bHoming = 0;
00069                 m_dHomePos = 0;
00070                 m_dHomeVel = 0;
00071                 m_iHomeEvent = 0;
00072                 m_iHomeDigIn = 0;
00073                 m_iHomeTimeOut = 0;
00074                 m_dVelMaxEncIncrS = 0;
00075                 m_dVelPModeEncIncrS = 0;
00076                 m_dAccIncrS2 = 0;
00077                 m_dDecIncrS2 = 0;
00078                 m_iCANId = 0;
00079                 m_iTypeEncoder = ENCODER_INCREMENTAL;
00080                 m_bUsePosMode = false;
00081                 m_bEnabled = false;
00082 
00083                 m_dRadToIncr = 0;
00084         }
00085 
00089         void set(               int iDriveIdent,
00090                                 int iEncIncrPerRevMot,
00091                                 double dVelMeasFrqHz,
00092                                 double dBeltRatio, double dWheelRatio,
00093                                 int iSign,
00094                                 bool bHoming, double dHomePos, double dHomeVel, int iHomeEvent, int iHomeDigIn, int iHomeTimeOut,
00095                                 double dVelMaxEncIncrS, double dVelPModeEncIncrS,
00096                                 double dAccIncrS2, double dDecIncrS2,
00097                                 int iTypeEncoder,
00098                                 int iCANId,
00099                                 bool bUsePosMode,
00100                                 bool bEnabled )
00101         {
00102                 m_iDriveIdent = iDriveIdent;
00103                 m_iEncIncrPerRevMot = iEncIncrPerRevMot;
00104                 m_dVelMeasFrqHz = dVelMeasFrqHz;
00105                 m_dBeltRatio = dBeltRatio;
00106                 m_dGearRatio = dWheelRatio;
00107                 m_iSign = iSign;
00108                 m_bHoming = bHoming;
00109                 m_dHomePos = dHomePos;
00110                 m_dHomeVel = dHomeVel;
00111                 m_iHomeEvent = iHomeEvent;
00112                 m_iHomeDigIn = iHomeDigIn;
00113                 m_iHomeTimeOut = iHomeTimeOut;
00114                 m_dVelMaxEncIncrS = dVelMaxEncIncrS;
00115                 m_dVelPModeEncIncrS = dVelPModeEncIncrS;
00116                 m_dAccIncrS2 = dAccIncrS2;
00117                 m_dDecIncrS2 = dDecIncrS2;
00118                 m_iTypeEncoder = iTypeEncoder;
00119                 m_iCANId = iCANId;
00120                 m_bUsePosMode = bUsePosMode;
00121                 m_bEnabled = bEnabled;
00122                 
00123                 m_dRadToIncr = (m_iEncIncrPerRevMot * m_dGearRatio * m_dBeltRatio) / (2. * PI);
00124 
00125                 
00126                 
00127         }
00128 
00132         int getDriveIdent() { return m_iDriveIdent; }
00133 
00137         int getCANId() { return m_iCANId; }
00138         
00142         int getSign() { return m_iSign; }
00143 
00147         bool isEnabled() { return m_bEnabled; }
00148 
00152         bool usePosMode() { return m_bUsePosMode; }
00153 
00157         bool getHoming() { return m_bHoming; }
00158 
00162         void setHoming(bool b) { m_bHoming = b; }
00163         
00167         double getHomePos() { return m_dHomePos; }
00168         
00172         double getHomeVel() { return m_dHomeVel; }
00173         
00177         int getHomeEvent() { return m_iHomeEvent; }
00178         
00182         int getHomeDigIn() { return m_iHomeDigIn; }
00183         
00187         int getHomeTimeOut() { return m_iHomeTimeOut; }
00188 
00192         double getVelMax() { return m_dVelMaxEncIncrS; }
00193 
00197         double getVelPosMode() { return m_dVelPModeEncIncrS; }
00198 
00202         double getAcc() { return m_dAccIncrS2; }
00203 
00207         double getDec() { return m_dDecIncrS2; }
00208 
00212         int getTypeEncoder() { return m_iTypeEncoder; }
00213 
00217         int getEncoderIncr() { return m_iEncIncrPerRevMot; }
00218 
00224         int TempMeasIncrToGradCel(int iTempIncr)
00225         {
00226                 double dTempMeasGradCel;
00227 
00228                 dTempMeasGradCel = 0.0002 * (iTempIncr * iTempIncr) - 0.2592 * iTempIncr + 105;
00229 
00230                 return (int)dTempMeasGradCel;
00231         }
00232 
00240         void convRadSToIncrPerPeriod(double dPosRad, double dVelRadS, int* piPosIncr, int* piVelIncrPeriod)
00241         {
00242                 *piPosIncr = (int)convRadToIncr(dPosRad);
00243                 *piVelIncrPeriod = (int)convRadSToIncrPerPeriod(dVelRadS);
00244         }
00245 
00247         double convRadToIncr(double dPosWheelRad)
00248         {
00249                 return (dPosWheelRad * m_dRadToIncr);
00250         }
00251         
00253         double convIncrToRad(int iPosIncr)
00254         {
00255                 return ((double)iPosIncr / m_dRadToIncr);
00256         }
00257         
00259         double convRadSToIncrPerPeriod(double dVelWheelRadS)
00260         {
00261                 return ( (dVelWheelRadS * m_dRadToIncr) / m_dVelMeasFrqHz );
00262         }
00263         
00265         double convIncrPerPeriodToRadS(int iVelMotIncrPeriod)
00266         {
00267                 return ( (double)iVelMotIncrPeriod / m_dRadToIncr * m_dVelMeasFrqHz );
00268         }
00269 
00270 private:
00271 
00272         int m_iDriveIdent;                      // drive identifier 0 ... n
00273         int m_iEncIncrPerRevMot;        // encoder increments per revolution of motor shaft 
00274         double m_dVelMeasFrqHz;         // only used for Neo drive = 500, else = 1
00275         double m_dGearRatio;            // gear ratio
00276         double m_dBeltRatio;            // if drive has belt set ratio, else = 1
00277         int m_iSign;                            // direction of motion
00278         bool m_bHoming;                         // if true, motor starts homing sequence on startup
00279         double m_dHomePos;                      // position set if homing switch active
00280         double m_dHomeVel;                      // velocity while homing
00281         int m_iHomeEvent;                       // type of homing input used at amplifier
00282         int m_iHomeDigIn;                       // number of digital input used for homing
00283         int m_iHomeTimeOut;                     // time out for homing
00284         double m_dVelMaxEncIncrS;       // max. veloctiy [encoder increments / s]
00285         double m_dVelPModeEncIncrS;     // velocity in position mode e. g. if amplifier generates trajectory
00286         double m_dAccIncrS2;            // max. acceleration [encoder increments / s�]
00287         double m_dDecIncrS2;            // max. deceleration [encoder increments / s�]
00288         int m_iTypeEncoder;                     // see enum TypeEncoder
00289         int m_iCANId;                           // CAN identifier
00290         bool m_bUsePosMode;                     // if enabled use amplifier position mode if velocity command = 0
00291         bool m_bEnabled;                        // if enabled, drive receives command data
00292 
00293         double m_dRadToIncr;
00294 
00295 public:
00296 
00297 
00298 
00299 };
00300 //-----------------------------------------------
00301 #endif


neo_relayboard
Author(s): Jan-Niklas Nieland
autogenerated on Thu Jun 6 2019 21:37:07