cob_base_drive_chain: CanCtrlPltfCOb3.h Source File

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: cob_drivers
00012  * ROS package name: cob_base_drive_chain
00013  * Description: This is a sample implementation of a can-bus with several nodes. In this case it implements the drive-chain of the Care-O-bot3 mobile base. yet, this can be used as template for using the generic_can and canopen_motor packages to implement arbitrary can-setups.
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: - Check whether motor status request in "setVelGearRadS" "setMotorTorque" make sense (maybe remove in "CanDriveHarmonica").
00022  *               - move implementational details (can cmds) of configureElmoRecorder to CanDriveHarmonica (check whether CanDriveItf has to be adapted then)
00023  *               - Check: what is the iRecordingGap, what is its unit
00024  *               - Remove Mutex.h search for a Boost lib
00025  *
00026  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00027  *
00028  * Redistribution and use in source and binary forms, with or without
00029  * modification, are permitted provided that the following conditions are met:
00030  *
00031  *     * Redistributions of source code must retain the above copyright
00032  *       notice, this list of conditions and the following disclaimer.
00033  *     * Redistributions in binary form must reproduce the above copyright
00034  *       notice, this list of conditions and the following disclaimer in the
00035  *       documentation and/or other materials provided with the distribution.
00036  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00037  *       Engineering and Automation (IPA) nor the names of its
00038  *       contributors may be used to endorse or promote products derived from
00039  *       this software without specific prior written permission.
00040  *
00041  * This program is free software: you can redistribute it and/or modify
00042  * it under the terms of the GNU Lesser General Public License LGPL as 
00043  * published by the Free Software Foundation, either version 3 of the 
00044  * License, or (at your option) any later version.
00045  * 
00046  * This program is distributed in the hope that it will be useful,
00047  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00048  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00049  * GNU Lesser General Public License LGPL for more details.
00050  * 
00051  * You should have received a copy of the GNU Lesser General Public 
00052  * License LGPL along with this program. 
00053  * If not, see <http://www.gnu.org/licenses/>.
00054  *
00055  ****************************************************************/
00056 
00057 #ifndef CANCTRLPLTFCOB3_INCLUDEDEF_H
00058 #define CANCTRLPLTFCOB3_INCLUDEDEF_H
00059 
00060 //-----------------------------------------------
00061 
00062 // general includes
00063 
00064 // Headers provided by other cob-packages
00065 #include <cob_canopen_motor/CanDriveItf.h>
00066 #include <cob_canopen_motor/CanDriveHarmonica.h>
00067 #include <cob_generic_can/CanItf.h>
00068 
00069 // Headers provided by cob-packages which should be avoided/removed
00070 #include <cob_utilities/IniFile.h>
00071 #include <cob_utilities/Mutex.h>
00072 
00073 // remove (not supported)
00074 //#include "stdafx.h"
00075 
00076 
00077 //-----------------------------------------------
00078 
00082 class CanCtrlPltfCOb3 // : public CanCtrlPltfItf
00083 {
00084 public:
00085 
00086         //--------------------------------- Basic procedures
00087 
00091         CanCtrlPltfCOb3(std::string iniDirectory);
00092 
00096         ~CanCtrlPltfCOb3();
00097 
00098 
00099         //--------------------------------- Hardware Specification
00100 
00105         enum MotorCANNode
00106         {
00107                 CANNODE_WHEEL1DRIVEMOTOR,
00108                 CANNODE_WHEEL1STEERMOTOR,
00109                 CANNODE_WHEEL2DRIVEMOTOR,
00110                 CANNODE_WHEEL2STEERMOTOR,
00111                 CANNODE_WHEEL3DRIVEMOTOR,
00112                 CANNODE_WHEEL3STEERMOTOR,
00113                 CANNODE_WHEEL4DRIVEMOTOR,
00114                 CANNODE_WHEEL4STEERMOTOR
00115         };
00116 
00117 
00118         //--------------------------------- Commands for all nodes on the bus
00119 
00125         bool initPltf();
00126 
00131         bool resetPltf();
00132 
00137         bool isPltfError();
00138 
00143         bool shutdownPltf();
00144 
00148         bool startWatchdog(bool bStarted);
00149 
00153         int evalCanBuffer();
00154 
00155 
00156         //--------------------------------- Commands specific for motor controller nodes
00157 
00164         int setVelGearRadS(int iCanIdent, double dVelGearRadS);
00165 
00172         void setMotorTorque(int iCanIdent, double dTorqueNm);
00173 
00180         void requestDriveStatus();
00181 
00189         int requestMotPosVel(int iCanIdent);
00190 
00195         void requestMotorTorque();
00196 
00203         int getGearPosVelRadS(int iCanIdent, double* pdAngleGearRad, double* pdVelGearRadS);
00204 
00211         int getGearDeltaPosVelRadS(int iCanIdent, double* pdDeltaAngleGearRad, double* pdVelGearRadS);
00212 
00218         void getStatus(int iCanIdent, int* piStatus, int* piTempCel);
00219 
00225         void getMotorTorque(int iCanIdent, double* pdTorqueNm);
00226 
00227 
00228 
00229         //--------------------------------- Commands specific for a certain motor controller
00230         // have to be implemented here, to keep the CanDriveItf generic
00231 
00242         int ElmoRecordings(int iFlag, int iParam, std::string sString);
00243 
00244         //--------------------------------- Commands for other nodes
00245 
00246 
00247 
00248 
00249 protected:
00250 
00251         //--------------------------------- internal functions
00252         
00257         std::string sIniDirectory;
00258         std::string sComposed;
00259         void readConfiguration();
00260 
00264         void sendNetStartCanOpen();
00265 
00266 
00267         //--------------------------------- Types
00268         
00272         struct ParamType
00273         {
00274                 // Platform config
00275 
00276                 int iHasWheel1DriveMotor;
00277                 int iHasWheel1SteerMotor;
00278                 int iHasWheel2DriveMotor;
00279                 int iHasWheel2SteerMotor;
00280                 int iHasWheel3DriveMotor;
00281                 int iHasWheel3SteerMotor;
00282                 int iHasWheel4DriveMotor;
00283                 int iHasWheel4SteerMotor;
00284 
00285                 double dWheel1SteerDriveCoupling;
00286                 double dWheel2SteerDriveCoupling;
00287                 double dWheel3SteerDriveCoupling;
00288                 double dWheel4SteerDriveCoupling;
00289 
00290                 int iRadiusWheelMM;
00291                 int iDistSteerAxisToDriveWheelMM;
00292 
00293                 int iHasRelayBoard;
00294                 int iHasIOBoard;
00295                 int iHasUSBoard;
00296                 int iHasGyroBoard;
00297                 int iHasRadarBoard;
00298 
00299                 double dCanTimeout;
00300         };
00301 
00305         struct GearMotorParamType
00306         {
00307                 int             iEncIncrPerRevMot;
00308                 double  dVelMeasFrqHz;
00309                 double  dGearRatio;
00310                 double  dBeltRatio;
00311                 int             iSign;
00312                 double  dVelMaxEncIncrS;
00313                 double  dAccIncrS2;
00314                 double  dDecIncrS2;
00315                 double  dScaleToMM;
00316                 int     iEncOffsetIncr;
00317                 bool    bIsSteer;
00318                 double  dCurrentToTorque;
00319                 double  dCurrMax;
00320                 int     iHomingDigIn;
00321         };
00322 
00326         struct CanNeoIDType
00327         {
00328                 int IOBoard_rx_ID;
00329                 int IOBoard_tx_ID;
00330 
00331                 int DriveNeo_W1Drive_rx_ID;
00332                 int DriveNeo_W1Drive_tx_ID;
00333                 int DriveNeo_W1Steer_rx_ID;
00334                 int DriveNeo_W1Steer_tx_ID;
00335 
00336                 int DriveNeo_W2Drive_rx_ID;
00337                 int DriveNeo_W2Drive_tx_ID;
00338                 int DriveNeo_W2Steer_rx_ID;
00339                 int DriveNeo_W2Steer_tx_ID;
00340 
00341                 int DriveNeo_W3Drive_rx_ID;
00342                 int DriveNeo_W3Drive_tx_ID;
00343                 int DriveNeo_W3Steer_rx_ID;
00344                 int DriveNeo_W3Steer_tx_ID;
00345 
00346                 int DriveNeo_W4Drive_rx_ID;
00347                 int DriveNeo_W4Drive_tx_ID;
00348                 int DriveNeo_W4Steer_rx_ID;
00349                 int DriveNeo_W4Steer_tx_ID;
00350 
00351                 int USBoard_rx_ID;
00352                 int USBoard_tx_ID;
00353                 int GyroBoard_rx_ID;
00354                 int GyroBoard_tx_ID;
00355                 int RadarBoard_rx_ID;
00356                 int RadarBoard_tx_ID;
00357         };
00358 
00363         struct CanOpenIDType
00364         {       
00365                 // Wheel 1
00366                 // can adresse motor 1
00367                 int TxPDO1_W1Drive;
00368                 int TxPDO2_W1Drive;
00369                 int RxPDO2_W1Drive;
00370                 int TxSDO_W1Drive;
00371                 int RxSDO_W1Drive;
00372                 // can adresse motor 2
00373                 int TxPDO1_W1Steer;
00374                 int TxPDO2_W1Steer;
00375                 int RxPDO2_W1Steer;
00376                 int TxSDO_W1Steer;
00377                 int RxSDO_W1Steer;
00378 
00379                 // Wheel 2
00380                 // can adresse motor 7
00381                 int TxPDO1_W2Drive;
00382                 int TxPDO2_W2Drive;
00383                 int RxPDO2_W2Drive;
00384                 int TxSDO_W2Drive;
00385                 int RxSDO_W2Drive;
00386                 // can adresse motor 8
00387                 int TxPDO1_W2Steer;
00388                 int TxPDO2_W2Steer;
00389                 int RxPDO2_W2Steer;
00390                 int TxSDO_W2Steer;
00391                 int RxSDO_W2Steer;      
00392                 
00393                 // Wheel 3
00394                 // can adresse motor 5
00395                 int TxPDO1_W3Drive;
00396                 int TxPDO2_W3Drive;
00397                 int RxPDO2_W3Drive;
00398                 int TxSDO_W3Drive;
00399                 int RxSDO_W3Drive;
00400                 // can adresse motor 6
00401                 int TxPDO1_W3Steer;
00402                 int TxPDO2_W3Steer;
00403                 int RxPDO2_W3Steer;
00404                 int TxSDO_W3Steer;
00405                 int RxSDO_W3Steer;
00406                 
00407                 // Wheel 4
00408                 // can adresse motor 3
00409                 int TxPDO1_W4Drive;
00410                 int TxPDO2_W4Drive;
00411                 int RxPDO2_W4Drive;
00412                 int TxSDO_W4Drive;
00413                 int RxSDO_W4Drive;
00414                 // can adresse motor 4
00415                 int TxPDO1_W4Steer;
00416                 int TxPDO2_W4Steer;
00417                 int RxPDO2_W4Steer;
00418                 int TxSDO_W4Steer;
00419                 int RxSDO_W4Steer;      
00420         };
00421 
00422         //--------------------------------- Parameter
00423         ParamType m_Param;
00424 //      CanNeoIDType m_CanNeoIDParam;
00425         CanOpenIDType m_CanOpenIDParam;
00426 
00427         // Prms for all Motor/Gear combos
00428         GearMotorParamType m_GearMotDrive1;
00429         GearMotorParamType m_GearMotDrive2;
00430         GearMotorParamType m_GearMotDrive3;
00431         GearMotorParamType m_GearMotDrive4;     
00432         GearMotorParamType m_GearMotSteer1;
00433         GearMotorParamType m_GearMotSteer2;
00434         GearMotorParamType m_GearMotSteer3;
00435         GearMotorParamType m_GearMotSteer4;
00436 
00437         //--------------------------------- Variables
00438         CanMsg m_CanMsgRec;
00439         Mutex m_Mutex;
00440         bool m_bWatchdogErr;
00441 
00442         //--------------------------------- Components
00443         // Can-Interface
00444         CanItf* m_pCanCtrl;
00445         IniFile m_IniFile;
00446 
00447         int m_iNumMotors;
00448         int m_iNumDrives;
00449 
00450         // Motor-Controllers
00451 /*      CanDriveItf* m_pW1DriveMotor;
00452         CanDriveItf* m_pW1SteerMotor;
00453         CanDriveItf* m_pW2DriveMotor;
00454         CanDriveItf* m_pW2SteerMotor;
00455         CanDriveItf* m_pW3DriveMotor;
00456         CanDriveItf* m_pW3SteerMotor;
00457         CanDriveItf* m_pW4DriveMotor;
00458         CanDriveItf* m_pW4SteerMotor;*/
00459         // pointer to each motors Can-Itf
00460         std::vector<CanDriveItf*> m_vpMotor;
00461         // vector with enums (specifying hardware-structure) -> simplifies cmd-check
00462         // this has to be adapted in c++ file to your hardware
00463         std::vector<int> m_viMotorID;
00464 
00465         // other
00466 
00467 
00468 };
00469 
00470 
00471 //-----------------------------------------------
00472 #endif