CanCtrlPltfCOb3.cpp
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 // general includes
00058 #include <math.h>
00059 #include <unistd.h>
00060 
00061 // Headers provided by other cob-packages
00062 #include <cob_generic_can/CanESD.h>
00063 #include <cob_generic_can/CanPeakSys.h>
00064 #include <cob_generic_can/CanPeakSysUSB.h>
00065 #include <cob_base_drive_chain/CanCtrlPltfCOb3.h>
00066 
00067 #include <unistd.h>
00068 
00069 //-----------------------------------------------
00070 
00071 CanCtrlPltfCOb3::CanCtrlPltfCOb3(std::string iniDirectory)
00072 {       
00073         sIniDirectory = iniDirectory;
00074         IniFile iniFile;
00075         iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
00076 
00077         // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
00078         iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true);
00079         iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true);
00080 
00081         if(m_iNumMotors < 2 || m_iNumMotors > 8) {
00082                 m_iNumMotors = 8;
00083                 m_iNumDrives = 4;
00084         }
00085 
00086         // ------------- first of all set used CanItf
00087         m_pCanCtrl = NULL;
00088 
00089         // ------------- init hardware-specific vectors and set default values
00090         m_vpMotor.resize(m_iNumMotors);
00091 
00092         for(int i=0; i<m_iNumMotors; i++)
00093         {
00094                 m_vpMotor[i] = NULL;
00095         }
00096 
00097         m_viMotorID.resize(m_iNumMotors);
00098 
00099 //      m_viMotorID.resize(8);
00100         if(m_iNumMotors >= 1)
00101                 m_viMotorID[0] = CANNODE_WHEEL1DRIVEMOTOR;
00102         if(m_iNumMotors >= 2)
00103                 m_viMotorID[1] = CANNODE_WHEEL1STEERMOTOR;
00104         if(m_iNumMotors >= 3)
00105                 m_viMotorID[2] = CANNODE_WHEEL2DRIVEMOTOR;
00106         if(m_iNumMotors >= 4)
00107                 m_viMotorID[3] = CANNODE_WHEEL2STEERMOTOR;
00108         if(m_iNumMotors >= 5)
00109                 m_viMotorID[4] = CANNODE_WHEEL3DRIVEMOTOR;
00110         if(m_iNumMotors >= 6)
00111                 m_viMotorID[5] = CANNODE_WHEEL3STEERMOTOR;
00112         if(m_iNumMotors >= 7)
00113                 m_viMotorID[6] = CANNODE_WHEEL4DRIVEMOTOR;
00114         if(m_iNumMotors == 8)
00115                 m_viMotorID[7] = CANNODE_WHEEL4STEERMOTOR;
00116 
00117         // ------------- parameters
00118         m_Param.dCanTimeout = 7;
00119 
00120         if(m_iNumMotors >= 1)
00121                 m_Param.iHasWheel1DriveMotor = 0;
00122         if(m_iNumMotors >= 2)
00123                 m_Param.iHasWheel1SteerMotor = 0;
00124         if(m_iNumMotors >= 3)
00125                 m_Param.iHasWheel2DriveMotor = 0;
00126         if(m_iNumMotors >= 4)
00127                 m_Param.iHasWheel2SteerMotor = 0;
00128         if(m_iNumMotors >= 5)
00129                 m_Param.iHasWheel3DriveMotor = 0;
00130         if(m_iNumMotors >= 6)
00131                 m_Param.iHasWheel3SteerMotor = 0;
00132         if(m_iNumMotors >= 7)
00133                 m_Param.iHasWheel4DriveMotor = 0;
00134         if(m_iNumMotors == 8)
00135                 m_Param.iHasWheel4SteerMotor = 0;
00136 
00137         m_Param.iHasRelayBoard = 0;
00138         m_Param.iHasIOBoard = 0;
00139         m_Param.iHasUSBoard = 0;
00140         m_Param.iHasGyroBoard = 0;
00141         m_Param.iHasRadarBoard = 0;     
00142 
00143         m_bWatchdogErr = false;
00144         
00145         // ------------ CanIds
00146         
00147         // ------------ For CanOpen (Harmonica)
00148         // Wheel 1
00149         // can adresse motor 1
00150         if(m_iNumMotors >= 1)
00151         {
00152                 m_CanOpenIDParam.TxPDO1_W1Drive = 0x182;
00153                 m_CanOpenIDParam.TxPDO2_W1Drive = 0x282;
00154                 m_CanOpenIDParam.RxPDO2_W1Drive = 0x302;
00155                 m_CanOpenIDParam.TxSDO_W1Drive = 0x582;
00156                 m_CanOpenIDParam.RxSDO_W1Drive = 0x602;
00157         }
00158         // can adresse motor 2
00159         if(m_iNumMotors >= 2)
00160         {
00161                 m_CanOpenIDParam.TxPDO1_W1Steer = 0x181;
00162                 m_CanOpenIDParam.TxPDO2_W1Steer = 0x281;
00163                 m_CanOpenIDParam.RxPDO2_W1Steer = 0x301;
00164                 m_CanOpenIDParam.TxSDO_W1Steer = 0x581;
00165                 m_CanOpenIDParam.RxSDO_W1Steer = 0x601;
00166         }
00167         // Wheel 2
00168         // can adresse motor 7
00169         if(m_iNumMotors >= 3)
00170         {
00171                 m_CanOpenIDParam.TxPDO1_W2Drive = 0x184;
00172                 m_CanOpenIDParam.TxPDO2_W2Drive = 0x284;
00173                 m_CanOpenIDParam.RxPDO2_W2Drive = 0x304;
00174                 m_CanOpenIDParam.TxSDO_W2Drive = 0x584;
00175                 m_CanOpenIDParam.RxSDO_W2Drive = 0x604;
00176         }
00177         // can adresse motor 8
00178         if(m_iNumMotors >= 4)
00179         {
00180                 m_CanOpenIDParam.TxPDO1_W2Steer = 0x183;
00181                 m_CanOpenIDParam.TxPDO2_W2Steer = 0x283;
00182                 m_CanOpenIDParam.RxPDO2_W2Steer = 0x303;
00183                 m_CanOpenIDParam.TxSDO_W2Steer = 0x583;
00184                 m_CanOpenIDParam.RxSDO_W2Steer = 0x603;
00185         }
00186         // Wheel 3
00187         // can adresse motor 5
00188         if(m_iNumMotors >= 5)
00189         {
00190                 m_CanOpenIDParam.TxPDO1_W3Drive = 0x188;
00191                 m_CanOpenIDParam.TxPDO2_W3Drive = 0x288;
00192                 m_CanOpenIDParam.RxPDO2_W3Drive = 0x308;
00193                 m_CanOpenIDParam.TxSDO_W3Drive = 0x588;
00194                 m_CanOpenIDParam.RxSDO_W3Drive = 0x608;
00195         }
00196         // can adresse motor 6
00197         if(m_iNumMotors >= 6)
00198         {
00199                 m_CanOpenIDParam.TxPDO1_W3Steer = 0x187;
00200                 m_CanOpenIDParam.TxPDO2_W3Steer = 0x287;
00201                 m_CanOpenIDParam.RxPDO2_W3Steer = 0x307;
00202                 m_CanOpenIDParam.TxSDO_W3Steer = 0x587;
00203                 m_CanOpenIDParam.RxSDO_W3Steer = 0x607;
00204         }
00205         // Wheel 4
00206         // can adresse motor 3
00207         if(m_iNumMotors >= 7)
00208         {
00209                 m_CanOpenIDParam.TxPDO1_W4Drive = 0x186;
00210                 m_CanOpenIDParam.TxPDO2_W4Drive = 0x286;
00211                 m_CanOpenIDParam.RxPDO2_W4Drive = 0x306;
00212                 m_CanOpenIDParam.TxSDO_W4Drive = 0x586;
00213                 m_CanOpenIDParam.RxSDO_W4Drive = 0x606;
00214         }
00215         // can adresse motor 4
00216         if(m_iNumMotors == 8)
00217         {
00218                 m_CanOpenIDParam.TxPDO1_W4Steer = 0x185;
00219                 m_CanOpenIDParam.TxPDO2_W4Steer = 0x285;
00220                 m_CanOpenIDParam.RxPDO2_W4Steer = 0x305;
00221                 m_CanOpenIDParam.TxSDO_W4Steer = 0x585;
00222                 m_CanOpenIDParam.RxSDO_W4Steer = 0x605;
00223         }
00224 }
00225 
00226 //-----------------------------------------------
00227 CanCtrlPltfCOb3::~CanCtrlPltfCOb3()
00228 {
00229 
00230         if (m_pCanCtrl != NULL)
00231         {
00232                 delete m_pCanCtrl;
00233         }
00234 
00235         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
00236         {
00237                 if (m_vpMotor[i] != NULL)
00238                 {
00239                         delete m_vpMotor[i];
00240                 }
00241         }
00242 
00243 }
00244 
00245 //-----------------------------------------------
00246 void CanCtrlPltfCOb3::readConfiguration()
00247 {
00248 
00249         int iTypeCan = 0;
00250         int iMaxMessages = 0;
00251 
00252         DriveParam DriveParamW1DriveMotor;
00253         DriveParam DriveParamW1SteerMotor;
00254         DriveParam DriveParamW2DriveMotor;
00255         DriveParam DriveParamW2SteerMotor;
00256         DriveParam DriveParamW3DriveMotor;
00257         DriveParam DriveParamW3SteerMotor;
00258         DriveParam DriveParamW4DriveMotor;
00259         DriveParam DriveParamW4SteerMotor;
00260         
00261         std::string strTypeDrive;
00262         std::string strTypeSteer;
00263 
00264         double dScaleToMM;
00265 
00266         // read Platform.ini (Coupling of Drive/Steer for Homing)
00267         m_IniFile.SetFileName(sIniDirectory + "Platform.ini", "CanCtrlPltfCOb3.cpp");
00268 
00269         m_IniFile.GetKeyInt("Geom", "RadiusWheel", &m_Param.iRadiusWheelMM, true);
00270         m_IniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_Param.iDistSteerAxisToDriveWheelMM, true);
00271 
00272         if(m_iNumDrives >= 1)
00273                 m_IniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_Param.dWheel1SteerDriveCoupling, true);
00274         if(m_iNumDrives >= 2)
00275                 m_IniFile.GetKeyDouble("DrivePrms", "Wheel2SteerDriveCoupling", &m_Param.dWheel2SteerDriveCoupling, true);
00276         if(m_iNumDrives >= 3)
00277                 m_IniFile.GetKeyDouble("DrivePrms", "Wheel3SteerDriveCoupling", &m_Param.dWheel3SteerDriveCoupling, true);
00278         if(m_iNumDrives == 4)
00279                 m_IniFile.GetKeyDouble("DrivePrms", "Wheel4SteerDriveCoupling", &m_Param.dWheel4SteerDriveCoupling, true);
00280 
00281 
00282         // read CanCtrl.ini
00283         m_IniFile.SetFileName(sIniDirectory + "CanCtrl.ini", "CanCtrlPltfCOb3.cpp");
00284 
00285         std::cout << "Can configuration of the platform:" << std::endl;
00286         
00287         // read Configuration of the Can-Network (CanCtrl.ini)
00288         m_IniFile.GetKeyInt("TypeCan", "Can", &iTypeCan, true);
00289         if (iTypeCan == 0)
00290         {
00291                 sComposed = sIniDirectory;
00292                 sComposed += "CanCtrl.ini";
00293                 m_pCanCtrl = new CanPeakSys(sComposed.c_str());
00294                 std::cout << "Uses CAN-Peak-Systems dongle" << std::endl;
00295         }
00296         else if (iTypeCan == 1)
00297         {
00298                 sComposed = sIniDirectory;
00299                 sComposed += "CanCtrl.ini";
00300                 m_pCanCtrl = new CANPeakSysUSB(sComposed.c_str());
00301                 std::cout << "Uses CAN-Peak-USB" << std::endl;
00302         }
00303         else if (iTypeCan == 2)
00304         {
00305                 sComposed = sIniDirectory;
00306                 sComposed += "CanCtrl.ini";
00307                 m_pCanCtrl = new CanESD(sComposed.c_str(), false);
00308                 std::cout << "Uses CAN-ESD-card" << std::endl;
00309         }
00310 
00311         // CanOpenId's ----- Default values (DESIRE)
00312         // Wheel 1
00313         // DriveMotor
00314         if(m_iNumDrives >= 1)
00315         {
00316                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W1Drive", &m_CanOpenIDParam.TxPDO1_W1Drive, true);
00317                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W1Drive", &m_CanOpenIDParam.TxPDO2_W1Drive, true);
00318                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W1Drive", &m_CanOpenIDParam.RxPDO2_W1Drive, true);
00319                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W1Drive", &m_CanOpenIDParam.TxSDO_W1Drive, true);
00320                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W1Drive", &m_CanOpenIDParam.RxSDO_W1Drive, true);
00321         }
00322         // SteerMotor
00323         if(m_iNumDrives >= 1)
00324         {
00325                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W1Steer", &m_CanOpenIDParam.TxPDO1_W1Steer, true);
00326                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W1Steer", &m_CanOpenIDParam.TxPDO2_W1Steer, true);
00327                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W1Steer", &m_CanOpenIDParam.RxPDO2_W1Steer, true);
00328                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W1Steer", &m_CanOpenIDParam.TxSDO_W1Steer, true);
00329                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W1Steer", &m_CanOpenIDParam.RxSDO_W1Steer, true);
00330         }
00331         
00332         // Wheel 2
00333         // DriveMotor
00334         if(m_iNumDrives >= 2)
00335         {
00336                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W2Drive", &m_CanOpenIDParam.TxPDO1_W2Drive, true);
00337                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W2Drive", &m_CanOpenIDParam.TxPDO2_W2Drive, true);
00338                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W2Drive", &m_CanOpenIDParam.RxPDO2_W2Drive, true);
00339                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W2Drive", &m_CanOpenIDParam.TxSDO_W2Drive, true);
00340                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W2Drive", &m_CanOpenIDParam.RxSDO_W2Drive, true);
00341         }
00342         // SteerMotor
00343         if(m_iNumDrives >= 2)
00344         {
00345                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W2Steer", &m_CanOpenIDParam.TxPDO1_W2Steer, true);
00346                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W2Steer", &m_CanOpenIDParam.TxPDO2_W2Steer, true);
00347                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W2Steer", &m_CanOpenIDParam.RxPDO2_W2Steer, true);
00348                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W2Steer", &m_CanOpenIDParam.TxSDO_W2Steer, true);
00349                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W2Steer", &m_CanOpenIDParam.RxSDO_W2Steer, true);
00350         }
00351         
00352         // Wheel 3
00353         // DriveMotor
00354         if(m_iNumDrives >= 3)
00355         {
00356                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W3Drive", &m_CanOpenIDParam.TxPDO1_W3Drive, true);
00357                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W3Drive", &m_CanOpenIDParam.TxPDO2_W3Drive, true);
00358                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W3Drive", &m_CanOpenIDParam.RxPDO2_W3Drive, true);
00359                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W3Drive", &m_CanOpenIDParam.TxSDO_W3Drive, true);
00360                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W3Drive", &m_CanOpenIDParam.RxSDO_W3Drive, true);
00361         }
00362         // SteerMotor
00363         if(m_iNumDrives >= 3)
00364         {
00365                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W3Steer", &m_CanOpenIDParam.TxPDO1_W3Steer, true);
00366                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W3Steer", &m_CanOpenIDParam.TxPDO2_W3Steer, true);
00367                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W3Steer", &m_CanOpenIDParam.RxPDO2_W3Steer, true);
00368                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W3Steer", &m_CanOpenIDParam.TxSDO_W3Steer, true);
00369                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W3Steer", &m_CanOpenIDParam.RxSDO_W3Steer, true);
00370         }
00371         
00372         // Wheel 4
00373         // DriveMotor
00374         if(m_iNumDrives >= 4)
00375         {
00376                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W4Drive", &m_CanOpenIDParam.TxPDO1_W4Drive, true);
00377                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W4Drive", &m_CanOpenIDParam.TxPDO2_W4Drive, true);
00378                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W4Drive", &m_CanOpenIDParam.RxPDO2_W4Drive, true);
00379                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W4Drive", &m_CanOpenIDParam.TxSDO_W4Drive, true);
00380                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W4Drive", &m_CanOpenIDParam.RxSDO_W4Drive, true);
00381         }
00382         // SteerMotor
00383         if(m_iNumDrives == 4)
00384         {
00385                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W4Steer", &m_CanOpenIDParam.TxPDO1_W4Steer, true);
00386                 m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W4Steer", &m_CanOpenIDParam.TxPDO2_W4Steer, true);
00387                 m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W4Steer", &m_CanOpenIDParam.RxPDO2_W4Steer, true);
00388                 m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W4Steer", &m_CanOpenIDParam.TxSDO_W4Steer, true);
00389                 m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W4Steer", &m_CanOpenIDParam.RxSDO_W4Steer, true);
00390         }
00391 
00392         // read configuration of the Drives (CanCtrl.ini)
00393         /* Drivemotor1-Parameters Old
00394         m_IniFile.GetKeyString("TypeDrive", "Drive1", &strTypeDrive, true);     
00395         m_IniFile.GetKeyInt(strTypeDrive.c_str(), "EncIncrPerRevMot", &(m_GearMotDrive1.iEncIncrPerRevMot), true);
00396         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "VelMeasFrqHz", &(m_GearMotDrive1.dVelMeasFrqHz), true);
00397         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "BeltRatio", &(m_GearMotDrive1.dBeltRatio), true);
00398         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "GearRatio", &(m_GearMotDrive1.dGearRatio), true);
00399         m_IniFile.GetKeyInt(strTypeDrive.c_str(), "Sign", &(m_GearMotDrive1.iSign), true);
00400         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "VelMaxEncIncrS", &(m_GearMotDrive1.dVelMaxEncIncrS), true);
00401         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "AccIncrS", &(m_GearMotDrive1.dAccIncrS2), true);
00402         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "DecIncrS", &(m_GearMotDrive1.dDecIncrS2), true);
00403         m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "EncOffsetIncr",&(m_GearMotDrive1.iEncOffsetIncr),true);
00404         */
00405 
00406         // "Drive Motor Type1" drive parameters 
00407         if(m_iNumDrives >= 1)
00408         {
00409                 m_IniFile.GetKeyInt("Drive1", "EncIncrPerRevMot", &(m_GearMotDrive1.iEncIncrPerRevMot), true);
00410                 m_IniFile.GetKeyDouble("Drive1", "VelMeasFrqHz", &(m_GearMotDrive1.dVelMeasFrqHz), true);
00411                 m_IniFile.GetKeyDouble("Drive1", "BeltRatio", &(m_GearMotDrive1.dBeltRatio), true);
00412                 m_IniFile.GetKeyDouble("Drive1", "GearRatio", &(m_GearMotDrive1.dGearRatio), true);
00413                 m_IniFile.GetKeyInt("Drive1", "Sign", &(m_GearMotDrive1.iSign), true);
00414                 m_IniFile.GetKeyDouble("Drive1", "VelMaxEncIncrS", &(m_GearMotDrive1.dVelMaxEncIncrS), true);
00415                 m_IniFile.GetKeyDouble("Drive1", "AccIncrS", &(m_GearMotDrive1.dAccIncrS2), true);
00416                 m_IniFile.GetKeyDouble("Drive1", "DecIncrS", &(m_GearMotDrive1.dDecIncrS2), true);
00417                 m_IniFile.GetKeyInt("Drive1", "EncOffsetIncr",&(m_GearMotDrive1.iEncOffsetIncr),true);
00418                 m_IniFile.GetKeyBool("Drive1", "IsSteering", &(m_GearMotDrive1.bIsSteer), true);
00419                 m_IniFile.GetKeyDouble("Drive1", "CurrentToTorque", &(m_GearMotDrive1.dCurrentToTorque), false);
00420                 m_IniFile.GetKeyDouble("Drive1", "CurrMax", &(m_GearMotDrive1.dCurrMax), false);
00421                 m_IniFile.GetKeyInt("Drive1", "HomingDigIn", &(m_GearMotDrive1.iHomingDigIn), false);
00422         }
00423 
00424         // "Drive Motor Type2" drive parameters 
00425         if(m_iNumDrives >= 2)
00426         {
00427                 m_IniFile.GetKeyInt("Drive2", "EncIncrPerRevMot", &(m_GearMotDrive2.iEncIncrPerRevMot), true);
00428                 m_IniFile.GetKeyDouble("Drive2", "VelMeasFrqHz", &(m_GearMotDrive2.dVelMeasFrqHz), true);
00429                 m_IniFile.GetKeyDouble("Drive2", "BeltRatio", &(m_GearMotDrive2.dBeltRatio), true);
00430                 m_IniFile.GetKeyDouble("Drive2", "GearRatio", &(m_GearMotDrive2.dGearRatio), true);
00431                 m_IniFile.GetKeyInt("Drive2", "Sign", &(m_GearMotDrive2.iSign), true);
00432                 m_IniFile.GetKeyDouble("Drive2", "VelMaxEncIncrS", &(m_GearMotDrive2.dVelMaxEncIncrS), true);
00433                 m_IniFile.GetKeyDouble("Drive2", "AccIncrS", &(m_GearMotDrive2.dAccIncrS2), true);
00434                 m_IniFile.GetKeyDouble("Drive2", "DecIncrS", &(m_GearMotDrive2.dDecIncrS2), true);
00435                 m_IniFile.GetKeyInt("Drive2", "EncOffsetIncr",&(m_GearMotDrive2.iEncOffsetIncr),true);
00436                 m_IniFile.GetKeyBool("Drive2", "IsSteering", &(m_GearMotDrive2.bIsSteer), true);
00437                 m_IniFile.GetKeyDouble("Drive2", "CurrentToTorque", &(m_GearMotDrive2.dCurrentToTorque), false);
00438                 m_IniFile.GetKeyDouble("Drive2", "CurrMax", &(m_GearMotDrive2.dCurrMax), false);
00439                 m_IniFile.GetKeyInt("Drive2", "HomingDigIn", &(m_GearMotDrive2.iHomingDigIn), false);
00440         }
00441 
00442         // "Drive Motor Type3" drive parameters 
00443         if(m_iNumDrives >= 3)
00444         {
00445                 m_IniFile.GetKeyInt("Drive3", "EncIncrPerRevMot", &(m_GearMotDrive3.iEncIncrPerRevMot), true);
00446                 m_IniFile.GetKeyDouble("Drive3", "VelMeasFrqHz", &(m_GearMotDrive3.dVelMeasFrqHz), true);
00447                 m_IniFile.GetKeyDouble("Drive3", "BeltRatio", &(m_GearMotDrive3.dBeltRatio), true);
00448                 m_IniFile.GetKeyDouble("Drive3", "GearRatio", &(m_GearMotDrive3.dGearRatio), true);
00449                 m_IniFile.GetKeyInt("Drive3", "Sign", &(m_GearMotDrive3.iSign), true);
00450                 m_IniFile.GetKeyDouble("Drive3", "VelMaxEncIncrS", &(m_GearMotDrive3.dVelMaxEncIncrS), true);
00451                 m_IniFile.GetKeyDouble("Drive3", "AccIncrS", &(m_GearMotDrive3.dAccIncrS2), true);
00452                 m_IniFile.GetKeyDouble("Drive3", "DecIncrS", &(m_GearMotDrive3.dDecIncrS2), true);
00453                 m_IniFile.GetKeyInt("Drive3", "EncOffsetIncr",&(m_GearMotDrive3.iEncOffsetIncr),true);
00454                 m_IniFile.GetKeyBool("Drive3", "IsSteering", &(m_GearMotDrive3.bIsSteer), true);
00455                 m_IniFile.GetKeyDouble("Drive3", "CurrentToTorque", &(m_GearMotDrive3.dCurrentToTorque), false);
00456                 m_IniFile.GetKeyDouble("Drive3", "CurrMax", &(m_GearMotDrive3.dCurrMax), false);
00457                 m_IniFile.GetKeyInt("Drive3", "HomingDigIn", &(m_GearMotDrive3.iHomingDigIn), false);
00458         }
00459 
00460         // "Drive Motor Type4" drive parameters 
00461         if(m_iNumDrives == 4)
00462         {
00463                 m_IniFile.GetKeyInt("Drive4", "EncIncrPerRevMot", &(m_GearMotDrive4.iEncIncrPerRevMot), true);
00464                 m_IniFile.GetKeyDouble("Drive4", "VelMeasFrqHz", &(m_GearMotDrive4.dVelMeasFrqHz), true);
00465                 m_IniFile.GetKeyDouble("Drive4", "BeltRatio", &(m_GearMotDrive4.dBeltRatio), true);
00466                 m_IniFile.GetKeyDouble("Drive4", "GearRatio", &(m_GearMotDrive4.dGearRatio), true);
00467                 m_IniFile.GetKeyInt("Drive4", "Sign", &(m_GearMotDrive4.iSign), true);
00468                 m_IniFile.GetKeyDouble("Drive4", "VelMaxEncIncrS", &(m_GearMotDrive4.dVelMaxEncIncrS), true);
00469                 m_IniFile.GetKeyDouble("Drive4", "AccIncrS", &(m_GearMotDrive4.dAccIncrS2), true);
00470                 m_IniFile.GetKeyDouble("Drive4", "DecIncrS", &(m_GearMotDrive4.dDecIncrS2), true);
00471                 m_IniFile.GetKeyInt("Drive4", "EncOffsetIncr",&(m_GearMotDrive4.iEncOffsetIncr),true);
00472                 m_IniFile.GetKeyBool("Drive4", "IsSteering", &(m_GearMotDrive4.bIsSteer), true);
00473                 m_IniFile.GetKeyDouble("Drive4", "CurrentToTorque", &(m_GearMotDrive4.dCurrentToTorque), false);
00474                 m_IniFile.GetKeyDouble("Drive4", "CurrMax", &(m_GearMotDrive4.dCurrMax), false);
00475                 m_IniFile.GetKeyInt("Drive4", "HomingDigIn", &(m_GearMotDrive4.iHomingDigIn), false);
00476         }
00477 
00478         // "Steer Motor Type1" drive parameters 
00479         if(m_iNumDrives >= 1)
00480         {
00481                 m_IniFile.GetKeyInt("Steer1", "EncIncrPerRevMot", &(m_GearMotSteer1.iEncIncrPerRevMot), true);
00482                 m_IniFile.GetKeyDouble("Steer1", "VelMeasFrqHz", &(m_GearMotSteer1.dVelMeasFrqHz), true);
00483                 m_IniFile.GetKeyDouble("Steer1", "BeltRatio", &(m_GearMotSteer1.dBeltRatio), true);
00484                 m_IniFile.GetKeyDouble("Steer1", "GearRatio", &(m_GearMotSteer1.dGearRatio), true);
00485                 m_IniFile.GetKeyInt("Steer1", "Sign", &(m_GearMotSteer1.iSign), true);
00486                 m_IniFile.GetKeyDouble("Steer1", "VelMaxEncIncrS", &(m_GearMotSteer1.dVelMaxEncIncrS), true);
00487                 m_IniFile.GetKeyDouble("Steer1", "AccIncrS", &(m_GearMotSteer1.dAccIncrS2), true);
00488                 m_IniFile.GetKeyDouble("Steer1", "DecIncrS", &(m_GearMotSteer1.dDecIncrS2), true);
00489                 m_IniFile.GetKeyInt("Steer1", "EncOffsetIncr",&(m_GearMotSteer1.iEncOffsetIncr),true);
00490                 m_IniFile.GetKeyBool("Steer1", "IsSteering", &(m_GearMotSteer1.bIsSteer), true);
00491                 m_IniFile.GetKeyDouble("Steer1", "CurrentToTorque", &(m_GearMotSteer1.dCurrentToTorque), false);
00492                 m_IniFile.GetKeyDouble("Steer1", "CurrMax", &(m_GearMotSteer1.dCurrMax), false);
00493                 m_IniFile.GetKeyInt("Steer1", "HomingDigIn", &(m_GearMotSteer1.iHomingDigIn), false);
00494         }
00495 
00496         // "Steer Motor Type2" drive parameters 
00497         if(m_iNumDrives >= 2)
00498         {
00499                 m_IniFile.GetKeyInt("Steer2", "EncIncrPerRevMot", &(m_GearMotSteer2.iEncIncrPerRevMot), true);
00500                 m_IniFile.GetKeyDouble("Steer2", "VelMeasFrqHz", &(m_GearMotSteer2.dVelMeasFrqHz), true);
00501                 m_IniFile.GetKeyDouble("Steer2", "BeltRatio", &(m_GearMotSteer2.dBeltRatio), true);
00502                 m_IniFile.GetKeyDouble("Steer2", "GearRatio", &(m_GearMotSteer2.dGearRatio), true);
00503                 m_IniFile.GetKeyInt("Steer2", "Sign", &(m_GearMotSteer2.iSign), true);
00504                 m_IniFile.GetKeyDouble("Steer2", "VelMaxEncIncrS", &(m_GearMotSteer2.dVelMaxEncIncrS), true);
00505                 m_IniFile.GetKeyDouble("Steer2", "AccIncrS", &(m_GearMotSteer2.dAccIncrS2), true);
00506                 m_IniFile.GetKeyDouble("Steer2", "DecIncrS", &(m_GearMotSteer2.dDecIncrS2), true);
00507                 m_IniFile.GetKeyInt("Steer2", "EncOffsetIncr",&(m_GearMotSteer2.iEncOffsetIncr),true);
00508                 m_IniFile.GetKeyBool("Steer2", "IsSteering", &(m_GearMotSteer2.bIsSteer), true);
00509                 m_IniFile.GetKeyDouble("Steer2", "CurrentToTorque", &(m_GearMotSteer2.dCurrentToTorque), false);
00510                 m_IniFile.GetKeyDouble("Steer2", "CurrMax", &(m_GearMotSteer2.dCurrMax), false);
00511                 m_IniFile.GetKeyInt("Steer2", "HomingDigIn", &(m_GearMotSteer2.iHomingDigIn), false);
00512         }
00513 
00514         // "Steer Motor Type3" drive parameters 
00515         if(m_iNumDrives >= 3)
00516         {
00517                 m_IniFile.GetKeyInt("Steer3", "EncIncrPerRevMot", &(m_GearMotSteer3.iEncIncrPerRevMot), true);
00518                 m_IniFile.GetKeyDouble("Steer3", "VelMeasFrqHz", &(m_GearMotSteer3.dVelMeasFrqHz), true);
00519                 m_IniFile.GetKeyDouble("Steer3", "BeltRatio", &(m_GearMotSteer3.dBeltRatio), true);
00520                 m_IniFile.GetKeyDouble("Steer3", "GearRatio", &(m_GearMotSteer3.dGearRatio), true);
00521                 m_IniFile.GetKeyInt("Steer3", "Sign", &(m_GearMotSteer3.iSign), true);
00522                 m_IniFile.GetKeyDouble("Steer3", "VelMaxEncIncrS", &(m_GearMotSteer3.dVelMaxEncIncrS), true);
00523                 m_IniFile.GetKeyDouble("Steer3", "AccIncrS", &(m_GearMotSteer3.dAccIncrS2), true);
00524                 m_IniFile.GetKeyDouble("Steer3", "DecIncrS", &(m_GearMotSteer3.dDecIncrS2), true);
00525                 m_IniFile.GetKeyInt("Steer3", "EncOffsetIncr",&(m_GearMotSteer3.iEncOffsetIncr),true);
00526                 m_IniFile.GetKeyBool("Steer3", "IsSteering", &(m_GearMotSteer3.bIsSteer), true);
00527                 m_IniFile.GetKeyDouble("Steer3", "CurrentToTorque", &(m_GearMotSteer3.dCurrentToTorque), false);
00528                 m_IniFile.GetKeyDouble("Steer3", "CurrMax", &(m_GearMotSteer3.dCurrMax), false);
00529                 m_IniFile.GetKeyInt("Steer3", "HomingDigIn", &(m_GearMotSteer3.iHomingDigIn), false);
00530         }
00531 
00532         // "Steer Motor Type4" drive parameters 
00533         if(m_iNumDrives == 4)
00534         {
00535                 m_IniFile.GetKeyInt("Steer4", "EncIncrPerRevMot", &(m_GearMotSteer4.iEncIncrPerRevMot), true);
00536                 m_IniFile.GetKeyDouble("Steer4", "VelMeasFrqHz", &(m_GearMotSteer4.dVelMeasFrqHz), true);
00537                 m_IniFile.GetKeyDouble("Steer4", "BeltRatio", &(m_GearMotSteer4.dBeltRatio), true);
00538                 m_IniFile.GetKeyDouble("Steer4", "GearRatio", &(m_GearMotSteer4.dGearRatio), true);
00539                 m_IniFile.GetKeyInt("Steer4", "Sign", &(m_GearMotSteer4.iSign), true);
00540                 m_IniFile.GetKeyDouble("Steer4", "VelMaxEncIncrS", &(m_GearMotSteer4.dVelMaxEncIncrS), true);
00541                 m_IniFile.GetKeyDouble("Steer4", "AccIncrS", &(m_GearMotSteer4.dAccIncrS2), true);
00542                 m_IniFile.GetKeyDouble("Steer4", "DecIncrS", &(m_GearMotSteer4.dDecIncrS2), true);
00543                 m_IniFile.GetKeyInt("Steer4", "EncOffsetIncr",&(m_GearMotSteer4.iEncOffsetIncr),true);
00544                 m_IniFile.GetKeyBool("Steer4", "IsSteering", &(m_GearMotSteer4.bIsSteer), true);
00545                 m_IniFile.GetKeyDouble("Steer4", "CurrentToTorque", &(m_GearMotSteer4.dCurrentToTorque), false);
00546                 m_IniFile.GetKeyDouble("Steer4", "CurrMax", &(m_GearMotSteer4.dCurrMax), false);
00547                 m_IniFile.GetKeyInt("Steer4", "HomingDigIn", &(m_GearMotSteer4.iHomingDigIn), false);
00548         }
00549 
00550         if(m_iNumDrives >= 1)
00551         {
00552                 DriveParamW1DriveMotor.setParam(
00553                         0,
00554                         m_GearMotDrive1.iEncIncrPerRevMot,
00555                         m_GearMotDrive1.dVelMeasFrqHz,
00556                         m_GearMotDrive1.dBeltRatio,
00557                         m_GearMotDrive1.dGearRatio,
00558                         m_GearMotDrive1.iSign,
00559                         m_GearMotDrive1.dVelMaxEncIncrS,
00560                         m_GearMotDrive1.dAccIncrS2,
00561                         m_GearMotDrive1.dDecIncrS2,
00562                         m_GearMotDrive1.iEncOffsetIncr,
00563                         m_GearMotDrive1.bIsSteer,
00564                         m_GearMotDrive1.dCurrentToTorque,
00565                         m_GearMotDrive1.dCurrMax,
00566                         m_GearMotDrive1.iHomingDigIn);
00567         }
00568         
00569         if(m_iNumDrives >= 1)
00570         {
00571                 DriveParamW1SteerMotor.setParam(
00572                         1,
00573                         m_GearMotSteer1.iEncIncrPerRevMot,
00574                         m_GearMotSteer1.dVelMeasFrqHz,
00575                         m_GearMotSteer1.dBeltRatio,
00576                         m_GearMotSteer1.dGearRatio,
00577                         m_GearMotSteer1.iSign,
00578                         m_GearMotSteer1.dVelMaxEncIncrS,
00579                         m_GearMotSteer1.dAccIncrS2,
00580                         m_GearMotSteer1.dDecIncrS2,
00581                         m_GearMotSteer1.iEncOffsetIncr,
00582                         m_GearMotSteer1.bIsSteer,
00583                         m_GearMotSteer1.dCurrentToTorque,
00584                         m_GearMotSteer1.dCurrMax,
00585                         m_GearMotSteer1.iHomingDigIn);
00586         }
00587 
00588         if(m_iNumDrives >= 2)
00589         {
00590                 DriveParamW2DriveMotor.setParam(
00591                         2,
00592                         m_GearMotDrive2.iEncIncrPerRevMot,
00593                         m_GearMotDrive2.dVelMeasFrqHz,
00594                         m_GearMotDrive2.dBeltRatio,
00595                         m_GearMotDrive2.dGearRatio,
00596                         m_GearMotDrive2.iSign,
00597                         m_GearMotDrive2.dVelMaxEncIncrS,
00598                         m_GearMotDrive2.dAccIncrS2,
00599                         m_GearMotDrive2.dDecIncrS2,
00600                         m_GearMotDrive2.iEncOffsetIncr,
00601                         m_GearMotDrive2.bIsSteer,
00602                         m_GearMotDrive2.dCurrentToTorque,
00603                         m_GearMotDrive2.dCurrMax,
00604                         m_GearMotDrive2.iHomingDigIn);
00605         }
00606         
00607         if(m_iNumDrives >= 2)
00608         {
00609                 DriveParamW2SteerMotor.setParam(
00610                         3,
00611                         m_GearMotSteer2.iEncIncrPerRevMot,
00612                         m_GearMotSteer2.dVelMeasFrqHz,
00613                         m_GearMotSteer2.dBeltRatio,
00614                         m_GearMotSteer2.dGearRatio,
00615                         m_GearMotSteer2.iSign,
00616                         m_GearMotSteer2.dVelMaxEncIncrS,
00617                         m_GearMotSteer2.dAccIncrS2,
00618                         m_GearMotSteer2.dDecIncrS2,
00619                         m_GearMotSteer2.iEncOffsetIncr,
00620                         m_GearMotSteer2.bIsSteer,
00621                         m_GearMotSteer2.dCurrentToTorque,
00622                         m_GearMotSteer2.dCurrMax,
00623                         m_GearMotSteer2.iHomingDigIn);
00624         }
00625 
00626         if(m_iNumDrives >= 3)
00627         {
00628                 DriveParamW3DriveMotor.setParam(
00629                         4,
00630                         m_GearMotDrive3.iEncIncrPerRevMot,
00631                         m_GearMotDrive3.dVelMeasFrqHz,
00632                         m_GearMotDrive3.dBeltRatio,
00633                         m_GearMotDrive3.dGearRatio,
00634                         m_GearMotDrive3.iSign,
00635                         m_GearMotDrive3.dVelMaxEncIncrS,
00636                         m_GearMotDrive3.dAccIncrS2,
00637                         m_GearMotDrive3.dDecIncrS2,
00638                         m_GearMotDrive3.iEncOffsetIncr,
00639                         m_GearMotDrive3.bIsSteer,
00640                         m_GearMotDrive3.dCurrentToTorque,
00641                         m_GearMotDrive3.dCurrMax,
00642                         m_GearMotDrive3.iHomingDigIn);
00643         }
00644         
00645         if(m_iNumDrives >= 3)
00646         {
00647                 DriveParamW3SteerMotor.setParam(
00648                         5,
00649                         m_GearMotSteer3.iEncIncrPerRevMot,
00650                         m_GearMotSteer3.dVelMeasFrqHz,
00651                         m_GearMotSteer3.dBeltRatio,
00652                         m_GearMotSteer3.dGearRatio,
00653                         m_GearMotSteer3.iSign,
00654                         m_GearMotSteer3.dVelMaxEncIncrS,
00655                         m_GearMotSteer3.dAccIncrS2,
00656                         m_GearMotSteer3.dDecIncrS2,
00657                         m_GearMotSteer3.iEncOffsetIncr,
00658                         m_GearMotSteer3.bIsSteer,
00659                         m_GearMotSteer3.dCurrentToTorque,
00660                         m_GearMotSteer3.dCurrMax,
00661                         m_GearMotSteer3.iHomingDigIn);
00662         }
00663 
00664         if(m_iNumDrives == 4)
00665         {
00666                 DriveParamW4DriveMotor.setParam(
00667                         6,
00668                         m_GearMotDrive4.iEncIncrPerRevMot,
00669                         m_GearMotDrive4.dVelMeasFrqHz,
00670                         m_GearMotDrive4.dBeltRatio,
00671                         m_GearMotDrive4.dGearRatio,
00672                         m_GearMotDrive4.iSign,
00673                         m_GearMotDrive4.dVelMaxEncIncrS,
00674                         m_GearMotDrive4.dAccIncrS2,
00675                         m_GearMotDrive4.dDecIncrS2,
00676                         m_GearMotDrive4.iEncOffsetIncr,
00677                         m_GearMotDrive4.bIsSteer,
00678                         m_GearMotDrive4.dCurrentToTorque,
00679                         m_GearMotDrive4.dCurrMax,
00680                         m_GearMotDrive4.iHomingDigIn);
00681         }
00682         
00683         if(m_iNumDrives == 4)
00684         {
00685                 DriveParamW4SteerMotor.setParam(
00686                         7,
00687                         m_GearMotSteer4.iEncIncrPerRevMot,
00688                         m_GearMotSteer4.dVelMeasFrqHz,
00689                         m_GearMotSteer4.dBeltRatio,
00690                         m_GearMotSteer4.dGearRatio,
00691                         m_GearMotSteer4.iSign,
00692                         m_GearMotSteer4.dVelMaxEncIncrS,
00693                         m_GearMotSteer4.dAccIncrS2,
00694                         m_GearMotSteer4.dDecIncrS2,
00695                         m_GearMotSteer4.iEncOffsetIncr,
00696                         m_GearMotSteer4.bIsSteer,
00697                         m_GearMotSteer4.dCurrentToTorque,
00698                         m_GearMotSteer4.dCurrMax,
00699                         m_GearMotSteer4.iHomingDigIn);
00700         }
00701         
00702         m_IniFile.GetKeyDouble("US", "ScaleToMM", &dScaleToMM, true);
00703 
00704 
00705         // read Platform.ini
00706         m_IniFile.SetFileName(sIniDirectory + "Platform.ini", "CanCtrlPltfCOb3.cpp");
00707 
00708 
00709         // ------ WHEEL 1 ------ //
00710         // --- Motor Wheel 1 Drive
00711         if(m_iNumDrives >= 1)
00712         {
00713                 m_IniFile.GetKeyInt("Config", "Wheel1DriveMotor", &m_Param.iHasWheel1DriveMotor, true);
00714                 if (m_Param.iHasWheel1DriveMotor == 0)
00715                 {
00716                         // No motor
00717                         std::cout << "node Wheel1DriveMotor available = 0" << std::endl;
00718                 }
00719                 else
00720                 {
00721                         // Motor Harmonica
00722                         std::cout << "Wheel1DriveMotor available = type version 2" << std::endl;
00723                         m_vpMotor[0] = new CanDriveHarmonica();
00724                         ((CanDriveHarmonica*) m_vpMotor[0])->setCanOpenParam(
00725                                 m_CanOpenIDParam.TxPDO1_W1Drive, m_CanOpenIDParam.TxPDO2_W1Drive, m_CanOpenIDParam.RxPDO2_W1Drive,
00726                                 m_CanOpenIDParam.TxSDO_W1Drive, m_CanOpenIDParam.RxSDO_W1Drive);
00727                         m_vpMotor[0]->setCanItf(m_pCanCtrl);
00728                         m_vpMotor[0]->setDriveParam(DriveParamW1DriveMotor);
00729                 }
00730         }
00731 
00732         // --- Motor Wheel 1 Steer
00733         if(m_iNumDrives >= 1)
00734         {
00735                 m_IniFile.GetKeyInt("Config", "Wheel1SteerMotor", &m_Param.iHasWheel1SteerMotor, true);
00736                 if (m_Param.iHasWheel1SteerMotor == 0)
00737                 {
00738                         // No motor
00739                         std::cout << "node Wheel1SteerMotor available = 0" << std::endl;
00740                 }
00741                 else
00742                 {
00743                         // Motor Harmonica
00744                         std::cout << "Wheel1SteerMotor available = type version 2" << std::endl;
00745                         m_vpMotor[1] = new CanDriveHarmonica();
00746                         ((CanDriveHarmonica*) m_vpMotor[1])->setCanOpenParam(
00747                                 m_CanOpenIDParam.TxPDO1_W1Steer, m_CanOpenIDParam.TxPDO2_W1Steer, m_CanOpenIDParam.RxPDO2_W1Steer,
00748                                 m_CanOpenIDParam.TxSDO_W1Steer, m_CanOpenIDParam.RxSDO_W1Steer);
00749                         m_vpMotor[1]->setCanItf(m_pCanCtrl);
00750                         m_vpMotor[1]->setDriveParam(DriveParamW1SteerMotor);
00751 
00752                 }
00753         }
00754         
00755         // ------ WHEEL 2 ------ //
00756         // --- Motor Wheel 2 Drive
00757         if(m_iNumDrives >= 2)
00758         {
00759                 m_IniFile.GetKeyInt("Config", "Wheel2DriveMotor", &m_Param.iHasWheel2DriveMotor, true);
00760                 if (m_Param.iHasWheel2DriveMotor == 0)
00761                 {
00762                         // No motor
00763                         std::cout << "node Wheel2DriveMotor available = 0" << std::endl;
00764                 }
00765                 else
00766                 {
00767                         // Motor Harmonica
00768                         std::cout << "Wheel2DriveMotor available = type version 2" << std::endl;
00769                         m_vpMotor[2] = new CanDriveHarmonica();
00770                         ((CanDriveHarmonica*) m_vpMotor[2])->setCanOpenParam(
00771                                 m_CanOpenIDParam.TxPDO1_W2Drive, m_CanOpenIDParam.TxPDO2_W2Drive, m_CanOpenIDParam.RxPDO2_W2Drive,
00772                                 m_CanOpenIDParam.TxSDO_W2Drive, m_CanOpenIDParam.RxSDO_W2Drive);
00773                         m_vpMotor[2]->setCanItf(m_pCanCtrl);
00774                         m_vpMotor[2]->setDriveParam(DriveParamW2DriveMotor);
00775                 }
00776         }
00777 
00778         // --- Motor Wheel 2 Steer
00779         if(m_iNumDrives >= 2)
00780         {
00781                 m_IniFile.GetKeyInt("Config", "Wheel2SteerMotor", &m_Param.iHasWheel2SteerMotor, true);
00782                 if (m_Param.iHasWheel2SteerMotor == 0)
00783                 {
00784                         // No motor
00785                         std::cout << "node Wheel2SteerMotor available = 0" << std::endl;
00786                 }
00787                 else
00788                 {
00789                         // Motor Harmonica
00790                         std::cout << "Wheel2SteerMotor available = type version 2" << std::endl;
00791                         m_vpMotor[3] = new CanDriveHarmonica();
00792                         ((CanDriveHarmonica*) m_vpMotor[3])->setCanOpenParam(
00793                                 m_CanOpenIDParam.TxPDO1_W2Steer, m_CanOpenIDParam.TxPDO2_W2Steer, m_CanOpenIDParam.RxPDO2_W2Steer,
00794                                 m_CanOpenIDParam.TxSDO_W2Steer, m_CanOpenIDParam.RxSDO_W2Steer);
00795                         m_vpMotor[3]->setCanItf(m_pCanCtrl);
00796                         m_vpMotor[3]->setDriveParam(DriveParamW2SteerMotor);
00797 
00798                 }
00799         }
00800         
00801         // ------ WHEEL 3 ------ //
00802         // --- Motor Wheel 3 Drive
00803         if(m_iNumDrives >= 3)
00804         {
00805                 m_IniFile.GetKeyInt("Config", "Wheel3DriveMotor", &m_Param.iHasWheel3DriveMotor, true);
00806                 if (m_Param.iHasWheel3DriveMotor == 0)
00807                 {
00808                         // No motor
00809                         std::cout << "node Wheel3DriveMotor available = 0" << std::endl;
00810                 }
00811                 else
00812                 {
00813                         // Motor Harmonica
00814                         std::cout << "Wheel3DriveMotor available = type version 2" << std::endl;
00815                         m_vpMotor[4] = new CanDriveHarmonica();
00816                         ((CanDriveHarmonica*) m_vpMotor[4])->setCanOpenParam(
00817                                 m_CanOpenIDParam.TxPDO1_W3Drive, m_CanOpenIDParam.TxPDO2_W3Drive, m_CanOpenIDParam.RxPDO2_W3Drive,
00818                                 m_CanOpenIDParam.TxSDO_W3Drive, m_CanOpenIDParam.RxSDO_W3Drive);
00819                         m_vpMotor[4]->setCanItf(m_pCanCtrl);
00820                         m_vpMotor[4]->setDriveParam(DriveParamW3DriveMotor);
00821                 }
00822         }
00823 
00824         // --- Motor Wheel 3 Steer
00825         if(m_iNumDrives >= 3)
00826         {
00827                 m_IniFile.GetKeyInt("Config", "Wheel3SteerMotor", &m_Param.iHasWheel3SteerMotor, true);
00828                 if (m_Param.iHasWheel3SteerMotor == 0)
00829                 {
00830                         // No motor
00831                         std::cout << "node Wheel3SteerMotor available = 0" << std::endl;
00832                 }
00833                 else
00834                 {
00835                         // Motor Harmonica
00836                         std::cout << "Wheel3SteerMotor available = type version 2" << std::endl;
00837                         m_vpMotor[5] = new CanDriveHarmonica();
00838                         ((CanDriveHarmonica*) m_vpMotor[5])->setCanOpenParam(
00839                                 m_CanOpenIDParam.TxPDO1_W3Steer, m_CanOpenIDParam.TxPDO2_W3Steer, m_CanOpenIDParam.RxPDO2_W3Steer,
00840                                 m_CanOpenIDParam.TxSDO_W3Steer, m_CanOpenIDParam.RxSDO_W3Steer);
00841                         m_vpMotor[5]->setCanItf(m_pCanCtrl);
00842                         m_vpMotor[5]->setDriveParam(DriveParamW3SteerMotor);
00843 
00844                 }
00845         }
00846         
00847         // ------ WHEEL 4 ------ //
00848         // --- Motor Wheel 4 Drive
00849         if(m_iNumDrives == 4)
00850         {
00851                 m_IniFile.GetKeyInt("Config", "Wheel4DriveMotor", &m_Param.iHasWheel4DriveMotor, true);
00852                 if (m_Param.iHasWheel4DriveMotor == 0)
00853                 {
00854                         // No motor
00855                         std::cout << "node Wheel4DriveMotor available = 0" << std::endl;
00856                 }
00857                 else
00858                 {
00859                         // Motor Harmonica
00860                         std::cout << "Wheel4DriveMotor available = type version 2" << std::endl;
00861                         m_vpMotor[6] = new CanDriveHarmonica();
00862                         ((CanDriveHarmonica*) m_vpMotor[6])->setCanOpenParam(
00863                                 m_CanOpenIDParam.TxPDO1_W4Drive, m_CanOpenIDParam.TxPDO2_W4Drive, m_CanOpenIDParam.RxPDO2_W4Drive,
00864                                 m_CanOpenIDParam.TxSDO_W4Drive, m_CanOpenIDParam.RxSDO_W4Drive);
00865                         m_vpMotor[6]->setCanItf(m_pCanCtrl);
00866                         m_vpMotor[6]->setDriveParam(DriveParamW4DriveMotor);
00867                 }
00868         }
00869 
00870         // --- Motor Wheel 4 Steer
00871         if(m_iNumDrives == 4)
00872         {
00873                 m_IniFile.GetKeyInt("Config", "Wheel4SteerMotor", &m_Param.iHasWheel4SteerMotor, true);
00874                 if (m_Param.iHasWheel4SteerMotor == 0)
00875                 {
00876                         // No motor
00877                         std::cout << "node Wheel4SteerMotor available = 0" << std::endl;
00878                 }
00879                 else
00880                 {
00881                         // Motor Harmonica
00882                         std::cout << "Wheel4SteerMotor available = type version 2" << std::endl;
00883                         m_vpMotor[7] = new CanDriveHarmonica();
00884                         ((CanDriveHarmonica*) m_vpMotor[7])->setCanOpenParam(
00885                                 m_CanOpenIDParam.TxPDO1_W4Steer, m_CanOpenIDParam.TxPDO2_W4Steer, m_CanOpenIDParam.RxPDO2_W4Steer,
00886                                 m_CanOpenIDParam.TxSDO_W4Steer, m_CanOpenIDParam.RxSDO_W4Steer);
00887                         m_vpMotor[7]->setCanItf(m_pCanCtrl);
00888                         m_vpMotor[7]->setDriveParam(DriveParamW4SteerMotor);
00889 
00890                 }
00891         }
00892 
00893         m_IniFile.GetKeyInt("Config", "GenericBufferLen", &iMaxMessages, true);
00894 
00895 
00896 }
00897 
00898 //-----------------------------------------------
00899 int CanCtrlPltfCOb3::evalCanBuffer()
00900 {
00901         bool bRet;
00902 //      char cBuf[200];
00903         
00904         m_Mutex.lock();
00905 
00906         // as long as there is something in the can buffer -> read out next message
00907         while(m_pCanCtrl->receiveMsg(&m_CanMsgRec) == true)
00908         {
00909                 bRet = false;
00910                 // check for every motor if message belongs to it
00911                 for (unsigned int i = 0; i < m_vpMotor.size(); i++)
00912                 {
00913                         // if message belongs to this motor write data (Pos, Vel, ...) to internal member vars
00914                         bRet |= m_vpMotor[i]->evalReceivedMsg(m_CanMsgRec);
00915                 }
00916 
00917                 if (bRet == false)
00918                 {
00919                         std::cout << "evalCanBuffer(): Received CAN_Message with unknown identifier " << m_CanMsgRec.m_iID << std::endl;
00920                 }               
00921         };
00922         
00923 
00924         m_Mutex.unlock();
00925 
00926         return 0;
00927 }
00928 
00929 //-----------------------------------------------
00930 bool CanCtrlPltfCOb3::initPltf()
00931 {       
00932         // read Configuration parameters from Inifile
00933         readConfiguration();
00934                 
00935         // Vectors for drive objects and return values
00936         std::vector<bool> vbRetDriveMotor;
00937         std::vector<bool> vbRetSteerMotor;
00938         std::vector<CanDriveItf*> vpDriveMotor;
00939         std::vector<CanDriveItf*> vpSteerMotor;
00940         bool bHomingOk;
00941 
00942 //      vbRetDriveMotor.assign(4,0);
00943 //      vbRetSteerMotor.assign(4,0);
00944         vbRetDriveMotor.assign(m_iNumDrives,0);
00945         vbRetSteerMotor.assign(m_iNumDrives,0);
00946 
00947         // Homing is done on a wheel-module base (steering and driving needs to be synchronized) 
00948         // copy Motor-Pointer into Steer/Drive vector for more insight
00949         for(int i=0; i<=m_iNumMotors; i+=2)
00950                 vpDriveMotor.push_back(m_vpMotor[i]);
00951 //      vpDriveMotor.push_back(m_vpMotor[2]);
00952 //      vpDriveMotor.push_back(m_vpMotor[4]);
00953 //      vpDriveMotor.push_back(m_vpMotor[6]);
00954         for(int i=1; i<=m_iNumMotors; i+=2)
00955                 vpSteerMotor.push_back(m_vpMotor[i]);
00956 //      vpSteerMotor.push_back(m_vpMotor[3]);
00957 //      vpSteerMotor.push_back(m_vpMotor[5]);
00958 //      vpSteerMotor.push_back(m_vpMotor[7]);
00959 
00960         std::vector<double> vdFactorVel;
00961 //      vdFactorVel.assign(4,0);
00962         vdFactorVel.assign(m_iNumDrives,0);
00963         double dhomeVeloRadS = -1.0;
00964 
00965 
00966         // Start can open network
00967         std::cout << "StartCanOpen" << std::endl;
00968         sendNetStartCanOpen();
00969         
00970 
00971         // initialize drives
00972         
00973         // 1st init watchdogs
00974         std::cout << "Initialization of Watchdogs" << std::endl;
00975         for(int i=0; i<m_iNumMotors; i++)
00976         {
00977                 m_vpMotor[i]->startWatchdog(true);
00978         }
00979 /*      m_vpMotor[0]->startWatchdog(true);
00980         m_vpMotor[1]->startWatchdog(true);
00981         m_vpMotor[2]->startWatchdog(true);
00982         m_vpMotor[3]->startWatchdog(true);
00983         m_vpMotor[4]->startWatchdog(true);
00984         m_vpMotor[5]->startWatchdog(true);
00985         m_vpMotor[6]->startWatchdog(true);
00986         m_vpMotor[7]->startWatchdog(true);
00987 */
00988         usleep(10000);
00989         
00990         // 2nd send watchdogs to bed while initializing drives
00991         for(int i=0; i<m_iNumMotors; i++)
00992         {
00993                 m_vpMotor[i]->startWatchdog(false);
00994         }
00995 /*      m_vpMotor[0]->startWatchdog(false);
00996         m_vpMotor[1]->startWatchdog(false);
00997         m_vpMotor[2]->startWatchdog(false);
00998         m_vpMotor[3]->startWatchdog(false);
00999         m_vpMotor[4]->startWatchdog(false);
01000         m_vpMotor[5]->startWatchdog(false);
01001         m_vpMotor[6]->startWatchdog(false);
01002         m_vpMotor[7]->startWatchdog(false);
01003 */
01004         usleep(100000);
01005 
01006         std::cout << "Initialization of Watchdogs done" << std::endl;
01007 
01008 
01009         // ---------------------- start homing procedurs
01010         // Perform homing of all wheels simultaneously
01011         // o.k. to avoid crashing hardware -> lets check that we have at least the 8 motors, like we have on cob
01012         if( (int)m_vpMotor.size() == m_iNumMotors )
01013         {
01014                 // Initialize and start all motors
01015                 for (int i = 0; i<m_iNumDrives; i++)
01016                 {
01017                         // init + start
01018                         vbRetDriveMotor[i] = vpDriveMotor[i]->init();
01019                         vbRetSteerMotor[i] = vpSteerMotor[i]->init();
01020                         usleep(10000);
01021                         vbRetDriveMotor[i] = vbRetDriveMotor[i] && vpDriveMotor[i]->start();
01022                         vbRetSteerMotor[i] = vbRetSteerMotor[i] && vpSteerMotor[i]->start();
01023                         usleep(10000);
01024                         // output State / Errors
01025                         if (vbRetDriveMotor[i] && vbRetSteerMotor[i])
01026                                 std::cout << "Initialization of Wheel "<< (i+1) << " OK" << std::endl;
01027                         else if (!vbRetDriveMotor[i] && vbRetSteerMotor[i])
01028                                 std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing DRIVE-Motor" << std::endl;
01029                         else if (vbRetDriveMotor[i] && !vbRetSteerMotor[i])
01030                                 std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing STEER-Motor" << std::endl;
01031                         else
01032                                 std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing STEER- and DRIVE-Motor" << std::endl;
01033                         // Just to be sure: Set vel to zero
01034                         vpDriveMotor[i]->setGearVelRadS(0);
01035                         vpSteerMotor[i]->setGearVelRadS(0);
01036                 }       
01037                 
01038                 // perform homing only when ALL drives are ERROR-Free
01039 //              if (vbRetDriveMotor[0] && vbRetDriveMotor[1] && vbRetDriveMotor[2] && vbRetDriveMotor[3] &&
01040 //                      vbRetSteerMotor[0] && vbRetSteerMotor[1] && vbRetSteerMotor[2] && vbRetSteerMotor[3])
01041                 bHomingOk = true;
01042                 for(int i=0; i<m_iNumDrives; i++)
01043                 {
01044                         if((vbRetDriveMotor[i] && vbRetSteerMotor[i]) == false)
01045                                 bHomingOk = false;
01046                 }
01047                 if(bHomingOk)
01048                 {
01049                         // Calc Compensation factor for Velocity:
01050                         if(m_iNumDrives >= 1)
01051                                 vdFactorVel[0] = - m_Param.dWheel1SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
01052                         if(m_iNumDrives >= 2)
01053                                 vdFactorVel[1] = - m_Param.dWheel2SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
01054                         if(m_iNumDrives >= 3)
01055                                 vdFactorVel[2] = - m_Param.dWheel3SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
01056                         if(m_iNumDrives == 4)
01057                                 vdFactorVel[3] = - m_Param.dWheel4SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
01058 
01059                         // initialize homing procedure
01060                         for (int i = 0; i<m_iNumDrives; i++)
01061                                 vpSteerMotor[i]->initHoming();                                          
01062                         
01063                         // make motors move
01064                         for (int i = 0; i<m_iNumDrives; i++)
01065                         {
01066                                 vpSteerMotor[i]->setGearVelRadS(dhomeVeloRadS);
01067                                 vpDriveMotor[i]->setGearVelRadS(dhomeVeloRadS * vdFactorVel[i]);
01068                         }
01069 
01070                         // wait at least 0.5 sec.
01071                         usleep(500000);
01072 
01073                         // Get rid of unnecessary can messages
01074                         bool bRet;
01075                         do
01076                                 bRet = m_pCanCtrl->receiveMsg(&m_CanMsgRec);
01077                         while(bRet == true);
01078 
01079                         // arm homing procedure
01080                         for (int i = 0; i<m_iNumDrives; i++)
01081                                 vpSteerMotor[i]->IntprtSetInt(8, 'H', 'M', 1, 1);
01082 
01083                         // wait until all steers are homed                      
01084                         bool bAllDone, bTimeOut=false;
01085                         int iCnt = 0;
01086                         do
01087                         {
01088                                 // send request for homing status
01089                                 for (int i = 0; i<m_iNumDrives; i++)
01090                                         vpSteerMotor[i]->IntprtSetInt(4, 'H', 'M', 1, 0);
01091                                 
01092                                 // eval Can Messages
01093                                 evalCanBuffer();
01094 
01095                                 // set already homed wheels to zero velocity
01096                                 bAllDone = true;
01097                                 for (int i = 0; i<m_iNumDrives; i++)
01098                                 {
01099                                         if (vpSteerMotor[i]->getStatusLimitSwitch())
01100                                         {
01101                                                 vpSteerMotor[i]->setGearVelRadS(0);
01102                                                 vpDriveMotor[i]->setGearVelRadS(0);
01103                                         }
01104                                         bAllDone = bAllDone && vpSteerMotor[i]->getStatusLimitSwitch();
01105                                 }       
01106 
01107                                 // increment timeout counter
01108                                 if (iCnt++ > 1000) //cpc-ck has 500 for Cob 3.5 here
01109                                         bTimeOut = true;
01110 
01111                                 // Sleep: To avoid can overload
01112                         usleep(20000);                          
01113                         }
01114                         while(!bAllDone && !bTimeOut);
01115 
01116                         // Output State
01117                         if (bTimeOut)
01118                         {
01119                                 for (int i=0;i<m_iNumDrives;i++)
01120                                 {
01121                                         vpSteerMotor[i]->setGearVelRadS(0);
01122                                         vpDriveMotor[i]->setGearVelRadS(0);
01123                                 }
01124                                 std::cout << "Error while Homing: Timeout while waiting for homing signal" << std::endl;
01125                         }
01126 
01127                         // update Can Buffer once more
01128                         // read current can buffer and update position and velocity measurements
01129                         evalCanBuffer();
01130 
01131                         // Now make steers move to position: zero
01132                         // this could be handled also by the elmos themselve
01133                         // however this way synchronization of steering and driving is better
01134                         double m_d0 = 2.5;
01135                         if (bTimeOut == false)
01136                         {
01137                                 do
01138                                 {       
01139                                         bAllDone = true;
01140                                         for (int i = 0; i<m_iNumDrives; i++)
01141                                         {
01142                                                 // get current position of steer
01143                                                 double dCurrentPosRad;
01144                                                 vpSteerMotor[i]->getGearPosRad(&dCurrentPosRad);
01145                                                 // P-Ctrl                                       
01146                                                 double dDeltaPhi = 0.0 - dCurrentPosRad; 
01147                                                 // check if steer is at pos zero
01148                                                 if (fabs(dDeltaPhi) < 0.03)     //alter Wert=0.03
01149                                                 {
01150                                                         dDeltaPhi = 0.0;
01151                                                 }
01152                                                 else
01153                                                 {
01154                                                         bAllDone = false;       
01155                                                 }
01156                                                 double dVelCmd = m_d0 * dDeltaPhi;
01157                                                 // set Outputs
01158                                                 vpSteerMotor[i]->setGearVelRadS(dVelCmd);
01159                                                 vpDriveMotor[i]->setGearVelRadS(dVelCmd*vdFactorVel[i]);
01160                                         }
01161                                         usleep(20000);
01162                                         evalCanBuffer();
01163                                 } while(!bAllDone);
01164 
01165 
01166                                 // Homing done. Wheels at position zero (+/- 0.5°)
01167                                 std::cout << "Wheels homed" << std::endl;
01168                                 for (int i=0;i<m_iNumDrives;i++)
01169                                 {
01170                                         vpSteerMotor[i]->setGearVelRadS(0);
01171                                         vpDriveMotor[i]->setGearVelRadS(0);
01172                                 }
01173                         }
01174                 }
01175         }
01176         // ---------------------- end homing procedure
01177 
01178         // homing done -> wake up watchdogs
01179         for(int i=0; i<m_iNumMotors; i++)
01180         {
01181                 m_vpMotor[i]->startWatchdog(true);
01182         }
01183 /*      m_vpMotor[0]->startWatchdog(true);
01184         m_vpMotor[1]->startWatchdog(true);
01185         m_vpMotor[2]->startWatchdog(true);
01186         m_vpMotor[3]->startWatchdog(true);
01187         m_vpMotor[4]->startWatchdog(true);
01188         m_vpMotor[5]->startWatchdog(true);
01189         m_vpMotor[6]->startWatchdog(true);
01190         m_vpMotor[7]->startWatchdog(true);
01191 */
01192 //      return  (
01193 //              vbRetDriveMotor[0] && vbRetDriveMotor[1] && vbRetDriveMotor[2] && vbRetDriveMotor[3] &&
01194 //              vbRetSteerMotor[0] && vbRetSteerMotor[1] && vbRetSteerMotor[2] && vbRetSteerMotor[3]);
01195         bHomingOk = true;
01196         for(int i=0; i<m_iNumDrives; i++)
01197         {
01198                 if((vbRetDriveMotor[i] && vbRetSteerMotor[i]) == false)
01199                 {
01200                         bHomingOk = false;
01201                 }
01202         }
01203         return (bHomingOk);
01204 }
01205 
01206 //-----------------------------------------------
01207 bool CanCtrlPltfCOb3::resetPltf()
01208 {
01209         bool bRetMotor = true;
01210         bool bRet = true;
01211         
01212         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01213         {
01214                 bRetMotor = m_vpMotor[i]->start();
01215                 if (bRetMotor == true)
01216                 {
01217                         m_vpMotor[i]->setGearVelRadS(0);
01218                 }
01219                 else
01220                 {
01221                         std::cout << "Resetting of Motor " << i << " failed" << std::endl;
01222                 }
01223 
01224                 bRet &= bRetMotor;
01225         }
01226         return(bRet);
01227 }
01228 
01229 //-----------------------------------------------
01230 bool CanCtrlPltfCOb3::shutdownPltf()
01231 {
01232 
01233         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01234         {
01235                 m_vpMotor[i]->shutdown();
01236         }
01237 
01238         return true;
01239 }
01240 
01241 //-----------------------------------------------
01242 bool CanCtrlPltfCOb3::isPltfError()
01243 {
01244 
01245         bool bErrMotor = false;
01246         std::vector<bool> vbErrMotor;
01247         vbErrMotor.resize(m_vpMotor.size());
01248 
01249         // check all motors for errors
01250         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01251         {
01252                 vbErrMotor[i] = m_vpMotor[i]->isError();
01253                 // if watchdog is not yet in error state but one motor is errorneous Log Out Error-Msg
01254                 if( (m_bWatchdogErr == false) && (vbErrMotor[i] == true) )
01255                 {
01256                         std::cout << "Motor " << i << " error" << std::endl;
01257                 }
01258                 // check whether no motor was errorneous
01259                 bErrMotor |= vbErrMotor[i];
01260         }
01261 
01262         // if no motor is in error state force Watchdog to "No Error" state
01263         if(bErrMotor == false)
01264                 m_bWatchdogErr = false;
01265         else
01266                 m_bWatchdogErr = true;
01267 
01268         if (m_bWatchdogErr) return true;
01269 
01270 
01271         // Check communication
01272         double dWatchTime = 0;
01273 
01274         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01275         {
01276                 dWatchTime = m_vpMotor[i]->getTimeToLastMsg();
01277 
01278                 if(dWatchTime <  m_Param.dCanTimeout)
01279                 {
01280                         m_bWatchdogErr = false;
01281                 }
01282                 if( (dWatchTime > m_Param.dCanTimeout) && (m_bWatchdogErr == false) )
01283                 {
01284                         std::cout << "timeout CAN motor " << i << std::endl;
01285                         m_bWatchdogErr = true;
01286                         return true;
01287                 }
01288         }
01289 
01290         return false;
01291 }
01292 
01293 //-----------------------------------------------
01294 bool CanCtrlPltfCOb3::startWatchdog(bool bStarted)
01295 {
01296 
01297         bool bRet = true;
01298 
01299         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01300         {
01301                 bRet = m_vpMotor[i]->startWatchdog(bStarted);
01302         }
01303 
01304         return (bRet);
01305 }
01306 
01307 //-----------------------------------------------
01308 void CanCtrlPltfCOb3::sendNetStartCanOpen()
01309 {
01310         CanMsg msg;
01311 
01312         msg.m_iID  = 0;
01313         msg.m_iLen = 2;
01314         msg.set(1,0,0,0,0,0,0,0);
01315         m_pCanCtrl->transmitMsg(msg, false);
01316 
01317         usleep(100000);
01318 }
01319 
01320 
01321 //-----------------------------------------------
01322 // Motor Controlers
01323 //-----------------------------------------------
01324 
01325 
01326 //-----------------------------------------------
01327 int CanCtrlPltfCOb3::setVelGearRadS(int iCanIdent, double dVelGearRadS)
01328 {               
01329         m_Mutex.lock();
01330 
01331         // If an error was detected and processed in isPltfErr().
01332         if (m_bWatchdogErr == true)
01333         {
01334                 // Error -> Stop motor driving
01335                 dVelGearRadS = 0;
01336                 // The error is detected and quitted in the funtion isPltfErr().
01337         }
01338 
01339         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01340         {
01341                 // check if Identifier fits to availlable hardware
01342                 if(iCanIdent == m_viMotorID[i])
01343                 {
01344                         m_vpMotor[i]->setGearVelRadS(dVelGearRadS);
01345                 }
01346         }
01347         
01348         m_Mutex.unlock();
01349         
01350         return 0;
01351 }
01352 
01353 //-----------------------------------------------
01354 int CanCtrlPltfCOb3::requestMotPosVel(int iCanIdent)
01355 {
01356         m_Mutex.lock();
01357 
01358         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01359         {
01360                 // check if Identifier fits to availlable hardware
01361                 if(iCanIdent == m_viMotorID[i])
01362                 {
01363                         m_vpMotor[i]->requestPosVel();
01364                 }
01365         }
01366         
01367         m_Mutex.unlock();
01368 
01369         return 0;
01370 }
01371 
01372 //-----------------------------------------------
01373 void CanCtrlPltfCOb3::requestDriveStatus()
01374 {
01375         m_Mutex.lock();
01376 
01377         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01378         {
01379                 m_vpMotor[i]->requestStatus();
01380         }
01381 
01382         m_Mutex.unlock();
01383 }
01384 
01385 //-----------------------------------------------
01386 int CanCtrlPltfCOb3::getGearPosVelRadS(int iCanIdent, double* pdAngleGearRad, double* pdVelGearRadS)
01387 {
01388 
01389         // init default outputs
01390         *pdAngleGearRad = 0;
01391         *pdVelGearRadS = 0;
01392 
01393         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01394         {
01395                 // check if Identifier fits to availlable hardware
01396                 if(iCanIdent == m_viMotorID[i])
01397                 {
01398                         m_vpMotor[i]->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS);
01399                 }
01400         }
01401         
01402         return 0;
01403 }
01404 
01405 //-----------------------------------------------
01406 int CanCtrlPltfCOb3::getGearDeltaPosVelRadS(int iCanIdent, double* pdAngleGearRad,
01407                                                                                    double* pdVelGearRadS)
01408 {
01409 
01410         // init default outputs
01411         *pdAngleGearRad = 0;
01412         *pdVelGearRadS = 0;
01413 
01414         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01415         {
01416                 // check if Identifier fits to availlable hardware
01417                 if(iCanIdent == m_viMotorID[i])
01418                 {
01419                         m_vpMotor[i]->getGearDeltaPosVelRadS(pdAngleGearRad, pdVelGearRadS);
01420                 }
01421         }
01422 
01423         return 0;
01424 }
01425 
01426 //-----------------------------------------------
01427 void CanCtrlPltfCOb3::getStatus(int iCanIdent, int* piStatus, int* piTempCel)
01428 {
01429         // init default outputs
01430         *piStatus = 0;
01431         *piTempCel = 0;
01432 
01433         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01434         {
01435                 // check if Identifier fits to availlable hardware
01436                 if(iCanIdent == m_viMotorID[i])
01437                 {
01438                         m_vpMotor[i]->getStatus(piStatus, piTempCel);
01439                 }
01440         }
01441 
01442 }
01443 //-----------------------------------------------
01444 void CanCtrlPltfCOb3::requestMotorTorque()
01445 {
01446         m_Mutex.lock();
01447 
01448         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01449         {
01450                 m_vpMotor[i]->requestMotorTorque();
01451         }
01452 
01453         m_Mutex.unlock();
01454 }       
01455 
01456 //-----------------------------------------------
01457 void CanCtrlPltfCOb3::getMotorTorque(int iCanIdent, double* pdTorqueNm)
01458 {
01459         // init default outputs
01460         *pdTorqueNm = 0;
01461 
01462         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01463         {
01464                 // check if Identifier fits to availlable hardware
01465                 if(iCanIdent == m_viMotorID[i])
01466                 {
01467                         m_vpMotor[i]->getMotorTorque(pdTorqueNm);
01468                 }
01469         }
01470 
01471 }
01472 //-----------------------------------------------
01473 void CanCtrlPltfCOb3::setMotorTorque(int iCanIdent, double dTorqueNm)
01474 {
01475         m_Mutex.lock();
01476 
01477         for(unsigned int i = 0; i < m_vpMotor.size(); i++)
01478         {
01479                 // check if Identifier fits to availlable hardware
01480                 if(iCanIdent == m_viMotorID[i])
01481                 {
01482                         m_vpMotor[i]->setMotorTorque(dTorqueNm);
01483                 }
01484         }
01485 
01486         m_Mutex.unlock();
01487 }
01488 
01489 //-----------------------------------------------
01490 // Read out Elmo-Recorder via CAN (cpc-pk)
01491 //-----------------------------------------------
01492 
01493 //-----------------------------------------------
01494 int CanCtrlPltfCOb3::ElmoRecordings(int iFlag, int iParam, std::string sString) {
01495         int tempRet = 0;
01496         int bRet = 0;
01497         
01498         switch(iFlag) {
01499                 case 0: //Flag = 0 means reset recorder and configure it
01500                         for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
01501                                 m_vpMotor[i]->setRecorder(0, iParam); //Configure Elmo Recorder with RecordingGap and start immediately
01502                         }
01503                         return 0;
01504 
01505                 case 1: //Flag = 1 means start readout process, mustn't be called too early (while Rec is in process..)
01506                         for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
01507                                 if((tempRet = m_vpMotor[i]->setRecorder(1, iParam, sString)) > bRet) { 
01508                                         bRet = tempRet; //Query Readout of Index to Log Directory
01509                                 }
01510                         }
01511                         return bRet;
01512                 
01513                 case 99:
01514                         for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
01515                                 m_vpMotor[i]->setRecorder(99, 0); //Stop any ongoing SDO transfer and clear corresponding data.
01516                         }
01517                         return 0;
01518                         
01519                 case 100:
01520                         for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
01521                                 bRet += m_vpMotor[i]->setRecorder(2, 0); //Request state of transmission
01522                         }
01523                         return bRet;
01524                 
01525                 default:
01526                         return -1;
01527         }
01528 }


cob_base_drive_chain
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:48