ElmoCtrl.h
Go to the documentation of this 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: cob3_driver
00012  * ROS package name: cob_head_axis
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Ulrich Reiser, email:ulrich.reiser@ipa.fhg.de
00017  *
00018  * Date of creation: Jul 2010
00019  * ToDo:
00020  *
00021  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *     * Redistributions of source code must retain the above copyright
00027  *       notice, this list of conditions and the following disclaimer.
00028  *     * Redistributions in binary form must reproduce the above copyright
00029  *       notice, this list of conditions and the following disclaimer in the
00030  *       documentation and/or other materials provided with the distribution.
00031  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00032  *       Engineering and Automation (IPA) nor the names of its
00033  *       contributors may be used to endorse or promote products derived from
00034  *       this software without specific prior written permission.
00035  *
00036  * This program is free software: you can redistribute it and/or modify
00037  * it under the terms of the GNU Lesser General Public License LGPL as 
00038  * published by the Free Software Foundation, either version 3 of the 
00039  * License, or (at your option) any later version.
00040  * 
00041  * This program is distributed in the hope that it will be useful,
00042  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00043  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00044  * GNU Lesser General Public License LGPL for more details.
00045  * 
00046  * You should have received a copy of the GNU Lesser General Public 
00047  * License LGPL along with this program. 
00048  * If not, see <http://www.gnu.org/licenses/>.
00049  *
00050  ****************************************************************/
00051 
00052 
00053 
00054 #ifndef __ELMO_CTRL_H__
00055 #define __ELMO_CTRL_H__
00056 #define NTCAN_CLEAN_NAMESPACE
00057 #include <cob_canopen_motor/CanDriveItf.h>
00058 #include <cob_canopen_motor/CanDriveHarmonica.h>
00059 #include <cob_generic_can/CanItf.h>
00060 #include <cob_generic_can/CanPeakSys.h>
00061 #include <cob_generic_can/CanPeakSysUSB.h>
00062 #include <cob_generic_can/CanESD.h>
00063 
00064 // Headers provided by cob-packages which should be avoided/removed^M
00065 #include <cob_utilities/IniFile.h>
00066 #include <string>
00067 
00068 
00069 typedef struct _CanOpenAddress
00070 {
00071         int TxPDO1;
00072         int TxPDO2;
00073         int RxPDO2;
00074         int TxSDO;
00075         int RxSDO;
00076 } CanOpenAddress;
00077 
00078 class ElmoCtrl;
00079 
00080 typedef struct
00081 {
00082         ElmoCtrl * pElmoCtrl;
00083 } ElmoThreadArgs;
00084 
00085 
00086 class ElmoCtrlParams
00087 {
00088 
00089         public:
00090                 ElmoCtrlParams(){;}
00091                 
00092                 int Init(std::string CanDevice, int BaudRate, int ModuleID)
00093                 {
00094                         SetCanDevice(CanDevice);
00095                         SetBaudRate(BaudRate);  
00096                         SetModuleID(ModuleID);
00097                         return 0;
00098                 }
00099                                 
00100                 //Can Device
00101                 void SetCanDevice(std::string CanDevice){m_CanDevice = CanDevice;}
00102                 std::string GetCanDevice(){return m_CanDevice;}
00103                         
00104                 //BaudRate
00105                 void SetBaudRate(int BaudRate){m_BaudRate=BaudRate;}
00106                 int GetBaudRate(){return m_BaudRate;}
00107         
00108                 //ModuleIDs
00109                 int GetModuleID(){return m_ModuleID;}
00110                 void SetModuleID(int id){m_ModuleID = id;}
00111                 
00112                 //Angular Constraints
00113                 void SetUpperLimit(double UpperLimit){m_UpperLimit = UpperLimit;}
00114                 void SetLowerLimit(double LowerLimit){m_LowerLimit = LowerLimit;}
00115                 void SetAngleOffset(double AngleOffset){m_Offset = AngleOffset;}
00116                 double GetUpperLimit(){return m_UpperLimit;}
00117                 double GetLowerLimit(){return m_LowerLimit;}
00118                 double GetAngleOffset(){return m_Offset;}
00119 
00120                 //Velocity
00121                 void SetMaxVel(double maxVel){m_MaxVel = maxVel;}
00122                 double GetMaxVel(){return m_MaxVel;}
00123 
00124                 //HomingDir
00125                 void SetHomingDir(int dir){m_HomingDir = dir;}
00126                 int GetHomingDir(){return m_HomingDir;}
00127 
00128                 //HomingDigIn
00129                 void SetHomingDigIn(int dig_in){m_HomingDigIn = dig_in;}
00130                 int GetHomingDigIn(){return m_HomingDigIn;}
00131                 
00132                 //CanIniFile
00133                 void SetCanIniFile(std::string iniFile){m_CanIniFile=iniFile;}
00134                 std::string GetCanIniFile(){return m_CanIniFile;}
00135                 
00136                 //Encoder increments
00137                 void SetEncoderIncrements(int enc_incr){m_EncIncrPerRevMot = enc_incr;}
00138                 int GetEncoderIncrements(){return m_EncIncrPerRevMot;}
00139                 
00140                 //Belt & Gear ratio in one
00141                 void SetGearRatio(double gear_ratio){m_GearRatio = gear_ratio;}
00142                 double GetGearRatio(){return m_GearRatio;}
00143                 
00144                 //Motor direction
00145                 void SetMotorDirection(int motor_dir){
00146                         if(motor_dir<0) m_MotorDirection = -1;
00147                         else m_MotorDirection = 1;
00148                 }
00149                 int GetMotorDirection(){return m_MotorDirection;}
00150                 
00151                 
00152                         
00153                 
00154         private:
00155                 int m_ModuleID;
00156                 std::string  m_CanDevice;
00157                 int m_BaudRate;
00158                 int m_HomingDir;
00159                 int m_HomingDigIn;
00160                 int m_EncIncrPerRevMot;
00161                 int m_MotorDirection;
00162                 double m_GearRatio;
00163                 double m_Offset;
00164                 double m_UpperLimit;
00165                 double m_LowerLimit;
00166                 double m_MaxVel;
00167                 std::string m_CanIniFile;
00168 
00169 };
00170 
00171 class ElmoCtrl
00172 {
00173 
00174 public:
00175         ElmoCtrl();
00176         ~ElmoCtrl();
00177 
00178         bool Init(ElmoCtrlParams* params, bool home = true);
00179 
00180 
00181         double MoveJointSpace (double PosRad);
00182         
00183 
00184         bool Home();
00185 
00186         bool RecoverAfterEmergencyStop();
00187 
00188         bool Stop();
00189 
00190 
00191         void setMaxVelocity(float radpersec)
00192         {
00193                 m_MaxVel = radpersec;
00194         }
00195 
00201         int getGearPosVelRadS(double* pdAngleGearRad, double* pdVelGearRadS);
00202         
00207         int setGearPosVelRadS(double dPosRad, double dVelRadS);
00208 
00212         int evalCanBuffer();
00213 
00214         //_double getConfig();
00215 
00216         double getJointVelocity();
00217 
00218 
00219 
00220         bool SetMotionCtrlType(int type);
00221         int GetMotionCtrlType(); 
00222         bool isError() ;
00223         
00224 
00225         enum {
00226                 POS_CTRL,
00227                 VEL_CTRL
00228         } MOTION_CTRL_TYPE;
00229 
00230 
00231         CANPeakSysUSB* GetCanCtrl(){return m_CanCtrl;}
00232         //CanESD* GetCanCtrl(){return m_CanCtrl;}
00233         //bool m_ElmoCtrlThreadActive;
00235         //pthread_mutex_t   m_cs_elmoCtrlIO;
00236         CanDriveHarmonica * m_Joint;  
00237 
00238 private:
00239 
00240         bool sendNetStartCanOpen(CanItf* canCtrl);
00241 
00242         int m_MotionCtrlType;   
00243 
00244         DriveParam * m_JointParams;
00245 
00246         int  m_CanBaseAddress;
00247         CanOpenAddress  m_CanAddress;
00248 
00249         CANPeakSysUSB * m_CanCtrl;
00250         //CanESD * m_CanCtrl;
00251 
00252         double m_MaxVel;
00253         int m_HomingDir;
00254         int m_HomingDigIn;
00255         int m_EncIncrPerRevMot;
00256         int m_MotorDirection;
00257         double m_GearRatio;
00258 
00259         double  m_UpperLimit;
00260         double  m_LowerLimit;
00261         double  m_JointOffset;
00262         /*      
00263         Jointd* m_CurrentAngularVelocity;
00265         Jointd* m_CurrentJointAngles;
00266         */
00268         pthread_mutex_t   m_Mutex;
00270         //pthread_mutex_t   m_AngularVel_Mutex;
00272         //pthread_t   m_ElmoThreadID;
00274         //ElmoThreadArgs * m_ElmoCtrlThreadArgs;
00275         ElmoCtrlParams * m_Params;
00276         CanMsg m_CanMsgRec;
00277         
00278         
00280 
00281 };
00282 
00283 
00284 
00285 
00286 
00287 
00288 
00289 
00290 
00291 #endif //__ELMO_CTRL_H__


cob_head_axis
Author(s): Ulrich Reiser
autogenerated on Thu Aug 27 2015 12:45:55