cob_base_drive_chain: CanCtrlPltfCOb3.cpp Source File

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