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

cob_base_drive_chain
Author(s): Christian Connette
autogenerated on Sat Jun 8 2019 21:02:29