CanCtrlPltfCOb3.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 // general includes
19 #include <math.h>
20 #include <unistd.h>
21 
22 // Headers provided by other cob-packages
23 #include <cob_generic_can/CanESD.h>
24 #include <cob_generic_can/CanPeakSys.h>
25 #include <cob_generic_can/CanPeakSysUSB.h>
26 #include <cob_base_drive_chain/CanCtrlPltfCOb3.h>
27 
28 #include <unistd.h>
29 
30 //-----------------------------------------------
31 
32 CanCtrlPltfCOb3::CanCtrlPltfCOb3(std::string iniDirectory)
33 {
34  sIniDirectory = iniDirectory;
35  IniFile iniFile;
36  iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
37 
38  // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
39  iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true);
40  iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true);
41 
42  if(m_iNumMotors < 2 || m_iNumMotors > 8) {
43  m_iNumMotors = 8;
44  m_iNumDrives = 4;
45  }
46 
47  // ------------- first of all set used CanItf
48  m_pCanCtrl = NULL;
49 
50  // ------------- init hardware-specific vectors and set default values
51  m_vpMotor.resize(m_iNumMotors);
52 
53  for(int i=0; i<m_iNumMotors; i++)
54  {
55  m_vpMotor[i] = NULL;
56  }
57 
58  m_viMotorID.resize(m_iNumMotors);
59 
60 // m_viMotorID.resize(8);
61  if(m_iNumMotors >= 1)
62  m_viMotorID[0] = CANNODE_WHEEL1DRIVEMOTOR;
63  if(m_iNumMotors >= 2)
64  m_viMotorID[1] = CANNODE_WHEEL1STEERMOTOR;
65  if(m_iNumMotors >= 3)
66  m_viMotorID[2] = CANNODE_WHEEL2DRIVEMOTOR;
67  if(m_iNumMotors >= 4)
68  m_viMotorID[3] = CANNODE_WHEEL2STEERMOTOR;
69  if(m_iNumMotors >= 5)
70  m_viMotorID[4] = CANNODE_WHEEL3DRIVEMOTOR;
71  if(m_iNumMotors >= 6)
72  m_viMotorID[5] = CANNODE_WHEEL3STEERMOTOR;
73  if(m_iNumMotors >= 7)
74  m_viMotorID[6] = CANNODE_WHEEL4DRIVEMOTOR;
75  if(m_iNumMotors == 8)
76  m_viMotorID[7] = CANNODE_WHEEL4STEERMOTOR;
77 
78  // ------------- parameters
79  m_Param.dCanTimeout = 7;
80 
81  if(m_iNumMotors >= 1)
82  m_Param.iHasWheel1DriveMotor = 0;
83  if(m_iNumMotors >= 2)
84  m_Param.iHasWheel1SteerMotor = 0;
85  if(m_iNumMotors >= 3)
86  m_Param.iHasWheel2DriveMotor = 0;
87  if(m_iNumMotors >= 4)
88  m_Param.iHasWheel2SteerMotor = 0;
89  if(m_iNumMotors >= 5)
90  m_Param.iHasWheel3DriveMotor = 0;
91  if(m_iNumMotors >= 6)
92  m_Param.iHasWheel3SteerMotor = 0;
93  if(m_iNumMotors >= 7)
94  m_Param.iHasWheel4DriveMotor = 0;
95  if(m_iNumMotors == 8)
96  m_Param.iHasWheel4SteerMotor = 0;
97 
98  m_Param.iHasRelayBoard = 0;
99  m_Param.iHasIOBoard = 0;
100  m_Param.iHasUSBoard = 0;
101  m_Param.iHasGyroBoard = 0;
102  m_Param.iHasRadarBoard = 0;
103 
104  m_bWatchdogErr = false;
105 
106  // ------------ CanIds
107 
108  // ------------ For CanOpen (Harmonica)
109  // Wheel 1
110  // can adresse motor 1
111  if(m_iNumMotors >= 1)
112  {
113  m_CanOpenIDParam.TxPDO1_W1Drive = 0x182;
114  m_CanOpenIDParam.TxPDO2_W1Drive = 0x282;
115  m_CanOpenIDParam.RxPDO2_W1Drive = 0x302;
116  m_CanOpenIDParam.TxSDO_W1Drive = 0x582;
117  m_CanOpenIDParam.RxSDO_W1Drive = 0x602;
118  }
119  // can adresse motor 2
120  if(m_iNumMotors >= 2)
121  {
122  m_CanOpenIDParam.TxPDO1_W1Steer = 0x181;
123  m_CanOpenIDParam.TxPDO2_W1Steer = 0x281;
124  m_CanOpenIDParam.RxPDO2_W1Steer = 0x301;
125  m_CanOpenIDParam.TxSDO_W1Steer = 0x581;
126  m_CanOpenIDParam.RxSDO_W1Steer = 0x601;
127  }
128  // Wheel 2
129  // can adresse motor 7
130  if(m_iNumMotors >= 3)
131  {
132  m_CanOpenIDParam.TxPDO1_W2Drive = 0x184;
133  m_CanOpenIDParam.TxPDO2_W2Drive = 0x284;
134  m_CanOpenIDParam.RxPDO2_W2Drive = 0x304;
135  m_CanOpenIDParam.TxSDO_W2Drive = 0x584;
136  m_CanOpenIDParam.RxSDO_W2Drive = 0x604;
137  }
138  // can adresse motor 8
139  if(m_iNumMotors >= 4)
140  {
141  m_CanOpenIDParam.TxPDO1_W2Steer = 0x183;
142  m_CanOpenIDParam.TxPDO2_W2Steer = 0x283;
143  m_CanOpenIDParam.RxPDO2_W2Steer = 0x303;
144  m_CanOpenIDParam.TxSDO_W2Steer = 0x583;
145  m_CanOpenIDParam.RxSDO_W2Steer = 0x603;
146  }
147  // Wheel 3
148  // can adresse motor 5
149  if(m_iNumMotors >= 5)
150  {
151  m_CanOpenIDParam.TxPDO1_W3Drive = 0x188;
152  m_CanOpenIDParam.TxPDO2_W3Drive = 0x288;
153  m_CanOpenIDParam.RxPDO2_W3Drive = 0x308;
154  m_CanOpenIDParam.TxSDO_W3Drive = 0x588;
155  m_CanOpenIDParam.RxSDO_W3Drive = 0x608;
156  }
157  // can adresse motor 6
158  if(m_iNumMotors >= 6)
159  {
160  m_CanOpenIDParam.TxPDO1_W3Steer = 0x187;
161  m_CanOpenIDParam.TxPDO2_W3Steer = 0x287;
162  m_CanOpenIDParam.RxPDO2_W3Steer = 0x307;
163  m_CanOpenIDParam.TxSDO_W3Steer = 0x587;
164  m_CanOpenIDParam.RxSDO_W3Steer = 0x607;
165  }
166  // Wheel 4
167  // can adresse motor 3
168  if(m_iNumMotors >= 7)
169  {
170  m_CanOpenIDParam.TxPDO1_W4Drive = 0x186;
171  m_CanOpenIDParam.TxPDO2_W4Drive = 0x286;
172  m_CanOpenIDParam.RxPDO2_W4Drive = 0x306;
173  m_CanOpenIDParam.TxSDO_W4Drive = 0x586;
174  m_CanOpenIDParam.RxSDO_W4Drive = 0x606;
175  }
176  // can adresse motor 4
177  if(m_iNumMotors == 8)
178  {
179  m_CanOpenIDParam.TxPDO1_W4Steer = 0x185;
180  m_CanOpenIDParam.TxPDO2_W4Steer = 0x285;
181  m_CanOpenIDParam.RxPDO2_W4Steer = 0x305;
182  m_CanOpenIDParam.TxSDO_W4Steer = 0x585;
183  m_CanOpenIDParam.RxSDO_W4Steer = 0x605;
184  }
185 }
186 
187 //-----------------------------------------------
188 CanCtrlPltfCOb3::~CanCtrlPltfCOb3()
189 {
190 
191  if (m_pCanCtrl != NULL)
192  {
193  delete m_pCanCtrl;
194  }
195 
196  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
197  {
198  if (m_vpMotor[i] != NULL)
199  {
200  delete m_vpMotor[i];
201  }
202  }
203 
204 }
205 
206 //-----------------------------------------------
207 void CanCtrlPltfCOb3::readConfiguration()
208 {
209 
210  int iTypeCan = 0;
211  int iMaxMessages = 0;
212 
213  DriveParam DriveParamW1DriveMotor;
214  DriveParam DriveParamW1SteerMotor;
215  DriveParam DriveParamW2DriveMotor;
216  DriveParam DriveParamW2SteerMotor;
217  DriveParam DriveParamW3DriveMotor;
218  DriveParam DriveParamW3SteerMotor;
219  DriveParam DriveParamW4DriveMotor;
220  DriveParam DriveParamW4SteerMotor;
221 
222  std::string strTypeDrive;
223  std::string strTypeSteer;
224 
225  double dScaleToMM;
226 
227  // read Platform.ini (Coupling of Drive/Steer for Homing)
228  m_IniFile.SetFileName(sIniDirectory + "Platform.ini", "CanCtrlPltfCOb3.cpp");
229 
230  m_IniFile.GetKeyInt("Geom", "RadiusWheel", &m_Param.iRadiusWheelMM, true);
231  m_IniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_Param.iDistSteerAxisToDriveWheelMM, true);
232 
233  m_IniFile.GetKeyDouble("DrivePrms","HomingVelocityRadS", &m_Param.dHomeVeloRadS, true);
234 
235  if(m_iNumDrives >= 1)
236  m_IniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_Param.dWheel1SteerDriveCoupling, true);
237  if(m_iNumDrives >= 2)
238  m_IniFile.GetKeyDouble("DrivePrms", "Wheel2SteerDriveCoupling", &m_Param.dWheel2SteerDriveCoupling, true);
239  if(m_iNumDrives >= 3)
240  m_IniFile.GetKeyDouble("DrivePrms", "Wheel3SteerDriveCoupling", &m_Param.dWheel3SteerDriveCoupling, true);
241  if(m_iNumDrives == 4)
242  m_IniFile.GetKeyDouble("DrivePrms", "Wheel4SteerDriveCoupling", &m_Param.dWheel4SteerDriveCoupling, true);
243 
244 
245  // read CanCtrl.ini
246  m_IniFile.SetFileName(sIniDirectory + "CanCtrl.ini", "CanCtrlPltfCOb3.cpp");
247 
248  std::cout << "Can configuration of the platform:" << std::endl;
249 
250  // read Configuration of the Can-Network (CanCtrl.ini)
251  m_IniFile.GetKeyInt("TypeCan", "Can", &iTypeCan, true);
252  if (iTypeCan == 0)
253  {
254  sComposed = sIniDirectory;
255  sComposed += "CanCtrl.ini";
256  m_pCanCtrl = new CanPeakSys(sComposed.c_str());
257  std::cout << "Uses CAN-Peak-Systems dongle" << std::endl;
258  }
259  else if (iTypeCan == 1)
260  {
261  sComposed = sIniDirectory;
262  sComposed += "CanCtrl.ini";
263  m_pCanCtrl = new CANPeakSysUSB(sComposed.c_str());
264  std::cout << "Uses CAN-Peak-USB" << std::endl;
265  }
266  else if (iTypeCan == 2)
267  {
268  sComposed = sIniDirectory;
269  sComposed += "CanCtrl.ini";
270  m_pCanCtrl = new CanESD(sComposed.c_str(), false);
271  std::cout << "Uses CAN-ESD-card" << std::endl;
272  }
273 
274  // CanOpenId's ----- Default values (DESIRE)
275  // Wheel 1
276  // DriveMotor
277  if(m_iNumDrives >= 1)
278  {
279  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W1Drive", &m_CanOpenIDParam.TxPDO1_W1Drive, true);
280  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W1Drive", &m_CanOpenIDParam.TxPDO2_W1Drive, true);
281  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W1Drive", &m_CanOpenIDParam.RxPDO2_W1Drive, true);
282  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W1Drive", &m_CanOpenIDParam.TxSDO_W1Drive, true);
283  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W1Drive", &m_CanOpenIDParam.RxSDO_W1Drive, true);
284  }
285  // SteerMotor
286  if(m_iNumDrives >= 1)
287  {
288  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W1Steer", &m_CanOpenIDParam.TxPDO1_W1Steer, true);
289  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W1Steer", &m_CanOpenIDParam.TxPDO2_W1Steer, true);
290  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W1Steer", &m_CanOpenIDParam.RxPDO2_W1Steer, true);
291  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W1Steer", &m_CanOpenIDParam.TxSDO_W1Steer, true);
292  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W1Steer", &m_CanOpenIDParam.RxSDO_W1Steer, true);
293  }
294 
295  // Wheel 2
296  // DriveMotor
297  if(m_iNumDrives >= 2)
298  {
299  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W2Drive", &m_CanOpenIDParam.TxPDO1_W2Drive, true);
300  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W2Drive", &m_CanOpenIDParam.TxPDO2_W2Drive, true);
301  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W2Drive", &m_CanOpenIDParam.RxPDO2_W2Drive, true);
302  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W2Drive", &m_CanOpenIDParam.TxSDO_W2Drive, true);
303  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W2Drive", &m_CanOpenIDParam.RxSDO_W2Drive, true);
304  }
305  // SteerMotor
306  if(m_iNumDrives >= 2)
307  {
308  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W2Steer", &m_CanOpenIDParam.TxPDO1_W2Steer, true);
309  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W2Steer", &m_CanOpenIDParam.TxPDO2_W2Steer, true);
310  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W2Steer", &m_CanOpenIDParam.RxPDO2_W2Steer, true);
311  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W2Steer", &m_CanOpenIDParam.TxSDO_W2Steer, true);
312  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W2Steer", &m_CanOpenIDParam.RxSDO_W2Steer, true);
313  }
314 
315  // Wheel 3
316  // DriveMotor
317  if(m_iNumDrives >= 3)
318  {
319  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W3Drive", &m_CanOpenIDParam.TxPDO1_W3Drive, true);
320  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W3Drive", &m_CanOpenIDParam.TxPDO2_W3Drive, true);
321  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W3Drive", &m_CanOpenIDParam.RxPDO2_W3Drive, true);
322  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W3Drive", &m_CanOpenIDParam.TxSDO_W3Drive, true);
323  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W3Drive", &m_CanOpenIDParam.RxSDO_W3Drive, true);
324  }
325  // SteerMotor
326  if(m_iNumDrives >= 3)
327  {
328  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W3Steer", &m_CanOpenIDParam.TxPDO1_W3Steer, true);
329  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W3Steer", &m_CanOpenIDParam.TxPDO2_W3Steer, true);
330  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W3Steer", &m_CanOpenIDParam.RxPDO2_W3Steer, true);
331  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W3Steer", &m_CanOpenIDParam.TxSDO_W3Steer, true);
332  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W3Steer", &m_CanOpenIDParam.RxSDO_W3Steer, true);
333  }
334 
335  // Wheel 4
336  // DriveMotor
337  if(m_iNumDrives >= 4)
338  {
339  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W4Drive", &m_CanOpenIDParam.TxPDO1_W4Drive, true);
340  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W4Drive", &m_CanOpenIDParam.TxPDO2_W4Drive, true);
341  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W4Drive", &m_CanOpenIDParam.RxPDO2_W4Drive, true);
342  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W4Drive", &m_CanOpenIDParam.TxSDO_W4Drive, true);
343  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W4Drive", &m_CanOpenIDParam.RxSDO_W4Drive, true);
344  }
345  // SteerMotor
346  if(m_iNumDrives == 4)
347  {
348  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO1_W4Steer", &m_CanOpenIDParam.TxPDO1_W4Steer, true);
349  m_IniFile.GetKeyInt("CanOpenIDs", "TxPDO2_W4Steer", &m_CanOpenIDParam.TxPDO2_W4Steer, true);
350  m_IniFile.GetKeyInt("CanOpenIDs", "RxPDO2_W4Steer", &m_CanOpenIDParam.RxPDO2_W4Steer, true);
351  m_IniFile.GetKeyInt("CanOpenIDs", "TxSDO_W4Steer", &m_CanOpenIDParam.TxSDO_W4Steer, true);
352  m_IniFile.GetKeyInt("CanOpenIDs", "RxSDO_W4Steer", &m_CanOpenIDParam.RxSDO_W4Steer, true);
353  }
354 
355  // read configuration of the Drives (CanCtrl.ini)
356  /* Drivemotor1-Parameters Old
357  m_IniFile.GetKeyString("TypeDrive", "Drive1", &strTypeDrive, true);
358  m_IniFile.GetKeyInt(strTypeDrive.c_str(), "EncIncrPerRevMot", &(m_GearMotDrive1.iEncIncrPerRevMot), true);
359  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "VelMeasFrqHz", &(m_GearMotDrive1.dVelMeasFrqHz), true);
360  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "BeltRatio", &(m_GearMotDrive1.dBeltRatio), true);
361  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "GearRatio", &(m_GearMotDrive1.dGearRatio), true);
362  m_IniFile.GetKeyInt(strTypeDrive.c_str(), "Sign", &(m_GearMotDrive1.iSign), true);
363  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "VelMaxEncIncrS", &(m_GearMotDrive1.dVelMaxEncIncrS), true);
364  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "AccIncrS", &(m_GearMotDrive1.dAccIncrS2), true);
365  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "DecIncrS", &(m_GearMotDrive1.dDecIncrS2), true);
366  m_IniFile.GetKeyDouble(strTypeDrive.c_str(), "EncOffsetIncr",&(m_GearMotDrive1.iEncOffsetIncr),true);
367  */
368 
369  // "Drive Motor Type1" drive parameters
370  if(m_iNumDrives >= 1)
371  {
372  m_IniFile.GetKeyInt("Drive1", "EncIncrPerRevMot", &(m_GearMotDrive1.iEncIncrPerRevMot), true);
373  m_IniFile.GetKeyDouble("Drive1", "VelMeasFrqHz", &(m_GearMotDrive1.dVelMeasFrqHz), true);
374  m_IniFile.GetKeyDouble("Drive1", "BeltRatio", &(m_GearMotDrive1.dBeltRatio), true);
375  m_IniFile.GetKeyDouble("Drive1", "GearRatio", &(m_GearMotDrive1.dGearRatio), true);
376  m_IniFile.GetKeyInt("Drive1", "Sign", &(m_GearMotDrive1.iSign), true);
377  m_IniFile.GetKeyDouble("Drive1", "VelMaxEncIncrS", &(m_GearMotDrive1.dVelMaxEncIncrS), true);
378  m_IniFile.GetKeyDouble("Drive1", "AccIncrS", &(m_GearMotDrive1.dAccIncrS2), true);
379  m_IniFile.GetKeyDouble("Drive1", "DecIncrS", &(m_GearMotDrive1.dDecIncrS2), true);
380  m_IniFile.GetKeyInt("Drive1", "EncOffsetIncr",&(m_GearMotDrive1.iEncOffsetIncr),true);
381  m_IniFile.GetKeyBool("Drive1", "IsSteering", &(m_GearMotDrive1.bIsSteer), true);
382  m_IniFile.GetKeyDouble("Drive1", "CurrentToTorque", &(m_GearMotDrive1.dCurrentToTorque), false);
383  m_IniFile.GetKeyDouble("Drive1", "CurrMax", &(m_GearMotDrive1.dCurrMax), false);
384  m_IniFile.GetKeyInt("Drive1", "HomingDigIn", &(m_GearMotDrive1.iHomingDigIn), false);
385  }
386 
387  // "Drive Motor Type2" drive parameters
388  if(m_iNumDrives >= 2)
389  {
390  m_IniFile.GetKeyInt("Drive2", "EncIncrPerRevMot", &(m_GearMotDrive2.iEncIncrPerRevMot), true);
391  m_IniFile.GetKeyDouble("Drive2", "VelMeasFrqHz", &(m_GearMotDrive2.dVelMeasFrqHz), true);
392  m_IniFile.GetKeyDouble("Drive2", "BeltRatio", &(m_GearMotDrive2.dBeltRatio), true);
393  m_IniFile.GetKeyDouble("Drive2", "GearRatio", &(m_GearMotDrive2.dGearRatio), true);
394  m_IniFile.GetKeyInt("Drive2", "Sign", &(m_GearMotDrive2.iSign), true);
395  m_IniFile.GetKeyDouble("Drive2", "VelMaxEncIncrS", &(m_GearMotDrive2.dVelMaxEncIncrS), true);
396  m_IniFile.GetKeyDouble("Drive2", "AccIncrS", &(m_GearMotDrive2.dAccIncrS2), true);
397  m_IniFile.GetKeyDouble("Drive2", "DecIncrS", &(m_GearMotDrive2.dDecIncrS2), true);
398  m_IniFile.GetKeyInt("Drive2", "EncOffsetIncr",&(m_GearMotDrive2.iEncOffsetIncr),true);
399  m_IniFile.GetKeyBool("Drive2", "IsSteering", &(m_GearMotDrive2.bIsSteer), true);
400  m_IniFile.GetKeyDouble("Drive2", "CurrentToTorque", &(m_GearMotDrive2.dCurrentToTorque), false);
401  m_IniFile.GetKeyDouble("Drive2", "CurrMax", &(m_GearMotDrive2.dCurrMax), false);
402  m_IniFile.GetKeyInt("Drive2", "HomingDigIn", &(m_GearMotDrive2.iHomingDigIn), false);
403  }
404 
405  // "Drive Motor Type3" drive parameters
406  if(m_iNumDrives >= 3)
407  {
408  m_IniFile.GetKeyInt("Drive3", "EncIncrPerRevMot", &(m_GearMotDrive3.iEncIncrPerRevMot), true);
409  m_IniFile.GetKeyDouble("Drive3", "VelMeasFrqHz", &(m_GearMotDrive3.dVelMeasFrqHz), true);
410  m_IniFile.GetKeyDouble("Drive3", "BeltRatio", &(m_GearMotDrive3.dBeltRatio), true);
411  m_IniFile.GetKeyDouble("Drive3", "GearRatio", &(m_GearMotDrive3.dGearRatio), true);
412  m_IniFile.GetKeyInt("Drive3", "Sign", &(m_GearMotDrive3.iSign), true);
413  m_IniFile.GetKeyDouble("Drive3", "VelMaxEncIncrS", &(m_GearMotDrive3.dVelMaxEncIncrS), true);
414  m_IniFile.GetKeyDouble("Drive3", "AccIncrS", &(m_GearMotDrive3.dAccIncrS2), true);
415  m_IniFile.GetKeyDouble("Drive3", "DecIncrS", &(m_GearMotDrive3.dDecIncrS2), true);
416  m_IniFile.GetKeyInt("Drive3", "EncOffsetIncr",&(m_GearMotDrive3.iEncOffsetIncr),true);
417  m_IniFile.GetKeyBool("Drive3", "IsSteering", &(m_GearMotDrive3.bIsSteer), true);
418  m_IniFile.GetKeyDouble("Drive3", "CurrentToTorque", &(m_GearMotDrive3.dCurrentToTorque), false);
419  m_IniFile.GetKeyDouble("Drive3", "CurrMax", &(m_GearMotDrive3.dCurrMax), false);
420  m_IniFile.GetKeyInt("Drive3", "HomingDigIn", &(m_GearMotDrive3.iHomingDigIn), false);
421  }
422 
423  // "Drive Motor Type4" drive parameters
424  if(m_iNumDrives == 4)
425  {
426  m_IniFile.GetKeyInt("Drive4", "EncIncrPerRevMot", &(m_GearMotDrive4.iEncIncrPerRevMot), true);
427  m_IniFile.GetKeyDouble("Drive4", "VelMeasFrqHz", &(m_GearMotDrive4.dVelMeasFrqHz), true);
428  m_IniFile.GetKeyDouble("Drive4", "BeltRatio", &(m_GearMotDrive4.dBeltRatio), true);
429  m_IniFile.GetKeyDouble("Drive4", "GearRatio", &(m_GearMotDrive4.dGearRatio), true);
430  m_IniFile.GetKeyInt("Drive4", "Sign", &(m_GearMotDrive4.iSign), true);
431  m_IniFile.GetKeyDouble("Drive4", "VelMaxEncIncrS", &(m_GearMotDrive4.dVelMaxEncIncrS), true);
432  m_IniFile.GetKeyDouble("Drive4", "AccIncrS", &(m_GearMotDrive4.dAccIncrS2), true);
433  m_IniFile.GetKeyDouble("Drive4", "DecIncrS", &(m_GearMotDrive4.dDecIncrS2), true);
434  m_IniFile.GetKeyInt("Drive4", "EncOffsetIncr",&(m_GearMotDrive4.iEncOffsetIncr),true);
435  m_IniFile.GetKeyBool("Drive4", "IsSteering", &(m_GearMotDrive4.bIsSteer), true);
436  m_IniFile.GetKeyDouble("Drive4", "CurrentToTorque", &(m_GearMotDrive4.dCurrentToTorque), false);
437  m_IniFile.GetKeyDouble("Drive4", "CurrMax", &(m_GearMotDrive4.dCurrMax), false);
438  m_IniFile.GetKeyInt("Drive4", "HomingDigIn", &(m_GearMotDrive4.iHomingDigIn), false);
439  }
440 
441  // "Steer Motor Type1" drive parameters
442  if(m_iNumDrives >= 1)
443  {
444  m_IniFile.GetKeyInt("Steer1", "EncIncrPerRevMot", &(m_GearMotSteer1.iEncIncrPerRevMot), true);
445  m_IniFile.GetKeyDouble("Steer1", "VelMeasFrqHz", &(m_GearMotSteer1.dVelMeasFrqHz), true);
446  m_IniFile.GetKeyDouble("Steer1", "BeltRatio", &(m_GearMotSteer1.dBeltRatio), true);
447  m_IniFile.GetKeyDouble("Steer1", "GearRatio", &(m_GearMotSteer1.dGearRatio), true);
448  m_IniFile.GetKeyInt("Steer1", "Sign", &(m_GearMotSteer1.iSign), true);
449  m_IniFile.GetKeyDouble("Steer1", "VelMaxEncIncrS", &(m_GearMotSteer1.dVelMaxEncIncrS), true);
450  m_IniFile.GetKeyDouble("Steer1", "AccIncrS", &(m_GearMotSteer1.dAccIncrS2), true);
451  m_IniFile.GetKeyDouble("Steer1", "DecIncrS", &(m_GearMotSteer1.dDecIncrS2), true);
452  m_IniFile.GetKeyInt("Steer1", "EncOffsetIncr",&(m_GearMotSteer1.iEncOffsetIncr),true);
453  m_IniFile.GetKeyBool("Steer1", "IsSteering", &(m_GearMotSteer1.bIsSteer), true);
454  m_IniFile.GetKeyDouble("Steer1", "CurrentToTorque", &(m_GearMotSteer1.dCurrentToTorque), false);
455  m_IniFile.GetKeyDouble("Steer1", "CurrMax", &(m_GearMotSteer1.dCurrMax), false);
456  m_IniFile.GetKeyInt("Steer1", "HomingDigIn", &(m_GearMotSteer1.iHomingDigIn), false);
457  }
458 
459  // "Steer Motor Type2" drive parameters
460  if(m_iNumDrives >= 2)
461  {
462  m_IniFile.GetKeyInt("Steer2", "EncIncrPerRevMot", &(m_GearMotSteer2.iEncIncrPerRevMot), true);
463  m_IniFile.GetKeyDouble("Steer2", "VelMeasFrqHz", &(m_GearMotSteer2.dVelMeasFrqHz), true);
464  m_IniFile.GetKeyDouble("Steer2", "BeltRatio", &(m_GearMotSteer2.dBeltRatio), true);
465  m_IniFile.GetKeyDouble("Steer2", "GearRatio", &(m_GearMotSteer2.dGearRatio), true);
466  m_IniFile.GetKeyInt("Steer2", "Sign", &(m_GearMotSteer2.iSign), true);
467  m_IniFile.GetKeyDouble("Steer2", "VelMaxEncIncrS", &(m_GearMotSteer2.dVelMaxEncIncrS), true);
468  m_IniFile.GetKeyDouble("Steer2", "AccIncrS", &(m_GearMotSteer2.dAccIncrS2), true);
469  m_IniFile.GetKeyDouble("Steer2", "DecIncrS", &(m_GearMotSteer2.dDecIncrS2), true);
470  m_IniFile.GetKeyInt("Steer2", "EncOffsetIncr",&(m_GearMotSteer2.iEncOffsetIncr),true);
471  m_IniFile.GetKeyBool("Steer2", "IsSteering", &(m_GearMotSteer2.bIsSteer), true);
472  m_IniFile.GetKeyDouble("Steer2", "CurrentToTorque", &(m_GearMotSteer2.dCurrentToTorque), false);
473  m_IniFile.GetKeyDouble("Steer2", "CurrMax", &(m_GearMotSteer2.dCurrMax), false);
474  m_IniFile.GetKeyInt("Steer2", "HomingDigIn", &(m_GearMotSteer2.iHomingDigIn), false);
475  }
476 
477  // "Steer Motor Type3" drive parameters
478  if(m_iNumDrives >= 3)
479  {
480  m_IniFile.GetKeyInt("Steer3", "EncIncrPerRevMot", &(m_GearMotSteer3.iEncIncrPerRevMot), true);
481  m_IniFile.GetKeyDouble("Steer3", "VelMeasFrqHz", &(m_GearMotSteer3.dVelMeasFrqHz), true);
482  m_IniFile.GetKeyDouble("Steer3", "BeltRatio", &(m_GearMotSteer3.dBeltRatio), true);
483  m_IniFile.GetKeyDouble("Steer3", "GearRatio", &(m_GearMotSteer3.dGearRatio), true);
484  m_IniFile.GetKeyInt("Steer3", "Sign", &(m_GearMotSteer3.iSign), true);
485  m_IniFile.GetKeyDouble("Steer3", "VelMaxEncIncrS", &(m_GearMotSteer3.dVelMaxEncIncrS), true);
486  m_IniFile.GetKeyDouble("Steer3", "AccIncrS", &(m_GearMotSteer3.dAccIncrS2), true);
487  m_IniFile.GetKeyDouble("Steer3", "DecIncrS", &(m_GearMotSteer3.dDecIncrS2), true);
488  m_IniFile.GetKeyInt("Steer3", "EncOffsetIncr",&(m_GearMotSteer3.iEncOffsetIncr),true);
489  m_IniFile.GetKeyBool("Steer3", "IsSteering", &(m_GearMotSteer3.bIsSteer), true);
490  m_IniFile.GetKeyDouble("Steer3", "CurrentToTorque", &(m_GearMotSteer3.dCurrentToTorque), false);
491  m_IniFile.GetKeyDouble("Steer3", "CurrMax", &(m_GearMotSteer3.dCurrMax), false);
492  m_IniFile.GetKeyInt("Steer3", "HomingDigIn", &(m_GearMotSteer3.iHomingDigIn), false);
493  }
494 
495  // "Steer Motor Type4" drive parameters
496  if(m_iNumDrives == 4)
497  {
498  m_IniFile.GetKeyInt("Steer4", "EncIncrPerRevMot", &(m_GearMotSteer4.iEncIncrPerRevMot), true);
499  m_IniFile.GetKeyDouble("Steer4", "VelMeasFrqHz", &(m_GearMotSteer4.dVelMeasFrqHz), true);
500  m_IniFile.GetKeyDouble("Steer4", "BeltRatio", &(m_GearMotSteer4.dBeltRatio), true);
501  m_IniFile.GetKeyDouble("Steer4", "GearRatio", &(m_GearMotSteer4.dGearRatio), true);
502  m_IniFile.GetKeyInt("Steer4", "Sign", &(m_GearMotSteer4.iSign), true);
503  m_IniFile.GetKeyDouble("Steer4", "VelMaxEncIncrS", &(m_GearMotSteer4.dVelMaxEncIncrS), true);
504  m_IniFile.GetKeyDouble("Steer4", "AccIncrS", &(m_GearMotSteer4.dAccIncrS2), true);
505  m_IniFile.GetKeyDouble("Steer4", "DecIncrS", &(m_GearMotSteer4.dDecIncrS2), true);
506  m_IniFile.GetKeyInt("Steer4", "EncOffsetIncr",&(m_GearMotSteer4.iEncOffsetIncr),true);
507  m_IniFile.GetKeyBool("Steer4", "IsSteering", &(m_GearMotSteer4.bIsSteer), true);
508  m_IniFile.GetKeyDouble("Steer4", "CurrentToTorque", &(m_GearMotSteer4.dCurrentToTorque), false);
509  m_IniFile.GetKeyDouble("Steer4", "CurrMax", &(m_GearMotSteer4.dCurrMax), false);
510  m_IniFile.GetKeyInt("Steer4", "HomingDigIn", &(m_GearMotSteer4.iHomingDigIn), false);
511  }
512 
513  if(m_iNumDrives >= 1)
514  {
515  DriveParamW1DriveMotor.setParam(
516  0,
517  m_GearMotDrive1.iEncIncrPerRevMot,
518  m_GearMotDrive1.dVelMeasFrqHz,
519  m_GearMotDrive1.dBeltRatio,
520  m_GearMotDrive1.dGearRatio,
521  m_GearMotDrive1.iSign,
522  m_GearMotDrive1.dVelMaxEncIncrS,
523  m_GearMotDrive1.dAccIncrS2,
524  m_GearMotDrive1.dDecIncrS2,
525  m_GearMotDrive1.iEncOffsetIncr,
526  m_GearMotDrive1.bIsSteer,
527  m_GearMotDrive1.dCurrentToTorque,
528  m_GearMotDrive1.dCurrMax,
529  m_GearMotDrive1.iHomingDigIn);
530  }
531 
532  if(m_iNumDrives >= 1)
533  {
534  DriveParamW1SteerMotor.setParam(
535  1,
536  m_GearMotSteer1.iEncIncrPerRevMot,
537  m_GearMotSteer1.dVelMeasFrqHz,
538  m_GearMotSteer1.dBeltRatio,
539  m_GearMotSteer1.dGearRatio,
540  m_GearMotSteer1.iSign,
541  m_GearMotSteer1.dVelMaxEncIncrS,
542  m_GearMotSteer1.dAccIncrS2,
543  m_GearMotSteer1.dDecIncrS2,
544  m_GearMotSteer1.iEncOffsetIncr,
545  m_GearMotSteer1.bIsSteer,
546  m_GearMotSteer1.dCurrentToTorque,
547  m_GearMotSteer1.dCurrMax,
548  m_GearMotSteer1.iHomingDigIn);
549  }
550 
551  if(m_iNumDrives >= 2)
552  {
553  DriveParamW2DriveMotor.setParam(
554  2,
555  m_GearMotDrive2.iEncIncrPerRevMot,
556  m_GearMotDrive2.dVelMeasFrqHz,
557  m_GearMotDrive2.dBeltRatio,
558  m_GearMotDrive2.dGearRatio,
559  m_GearMotDrive2.iSign,
560  m_GearMotDrive2.dVelMaxEncIncrS,
561  m_GearMotDrive2.dAccIncrS2,
562  m_GearMotDrive2.dDecIncrS2,
563  m_GearMotDrive2.iEncOffsetIncr,
564  m_GearMotDrive2.bIsSteer,
565  m_GearMotDrive2.dCurrentToTorque,
566  m_GearMotDrive2.dCurrMax,
567  m_GearMotDrive2.iHomingDigIn);
568  }
569 
570  if(m_iNumDrives >= 2)
571  {
572  DriveParamW2SteerMotor.setParam(
573  3,
574  m_GearMotSteer2.iEncIncrPerRevMot,
575  m_GearMotSteer2.dVelMeasFrqHz,
576  m_GearMotSteer2.dBeltRatio,
577  m_GearMotSteer2.dGearRatio,
578  m_GearMotSteer2.iSign,
579  m_GearMotSteer2.dVelMaxEncIncrS,
580  m_GearMotSteer2.dAccIncrS2,
581  m_GearMotSteer2.dDecIncrS2,
582  m_GearMotSteer2.iEncOffsetIncr,
583  m_GearMotSteer2.bIsSteer,
584  m_GearMotSteer2.dCurrentToTorque,
585  m_GearMotSteer2.dCurrMax,
586  m_GearMotSteer2.iHomingDigIn);
587  }
588 
589  if(m_iNumDrives >= 3)
590  {
591  DriveParamW3DriveMotor.setParam(
592  4,
593  m_GearMotDrive3.iEncIncrPerRevMot,
594  m_GearMotDrive3.dVelMeasFrqHz,
595  m_GearMotDrive3.dBeltRatio,
596  m_GearMotDrive3.dGearRatio,
597  m_GearMotDrive3.iSign,
598  m_GearMotDrive3.dVelMaxEncIncrS,
599  m_GearMotDrive3.dAccIncrS2,
600  m_GearMotDrive3.dDecIncrS2,
601  m_GearMotDrive3.iEncOffsetIncr,
602  m_GearMotDrive3.bIsSteer,
603  m_GearMotDrive3.dCurrentToTorque,
604  m_GearMotDrive3.dCurrMax,
605  m_GearMotDrive3.iHomingDigIn);
606  }
607 
608  if(m_iNumDrives >= 3)
609  {
610  DriveParamW3SteerMotor.setParam(
611  5,
612  m_GearMotSteer3.iEncIncrPerRevMot,
613  m_GearMotSteer3.dVelMeasFrqHz,
614  m_GearMotSteer3.dBeltRatio,
615  m_GearMotSteer3.dGearRatio,
616  m_GearMotSteer3.iSign,
617  m_GearMotSteer3.dVelMaxEncIncrS,
618  m_GearMotSteer3.dAccIncrS2,
619  m_GearMotSteer3.dDecIncrS2,
620  m_GearMotSteer3.iEncOffsetIncr,
621  m_GearMotSteer3.bIsSteer,
622  m_GearMotSteer3.dCurrentToTorque,
623  m_GearMotSteer3.dCurrMax,
624  m_GearMotSteer3.iHomingDigIn);
625  }
626 
627  if(m_iNumDrives == 4)
628  {
629  DriveParamW4DriveMotor.setParam(
630  6,
631  m_GearMotDrive4.iEncIncrPerRevMot,
632  m_GearMotDrive4.dVelMeasFrqHz,
633  m_GearMotDrive4.dBeltRatio,
634  m_GearMotDrive4.dGearRatio,
635  m_GearMotDrive4.iSign,
636  m_GearMotDrive4.dVelMaxEncIncrS,
637  m_GearMotDrive4.dAccIncrS2,
638  m_GearMotDrive4.dDecIncrS2,
639  m_GearMotDrive4.iEncOffsetIncr,
640  m_GearMotDrive4.bIsSteer,
641  m_GearMotDrive4.dCurrentToTorque,
642  m_GearMotDrive4.dCurrMax,
643  m_GearMotDrive4.iHomingDigIn);
644  }
645 
646  if(m_iNumDrives == 4)
647  {
648  DriveParamW4SteerMotor.setParam(
649  7,
650  m_GearMotSteer4.iEncIncrPerRevMot,
651  m_GearMotSteer4.dVelMeasFrqHz,
652  m_GearMotSteer4.dBeltRatio,
653  m_GearMotSteer4.dGearRatio,
654  m_GearMotSteer4.iSign,
655  m_GearMotSteer4.dVelMaxEncIncrS,
656  m_GearMotSteer4.dAccIncrS2,
657  m_GearMotSteer4.dDecIncrS2,
658  m_GearMotSteer4.iEncOffsetIncr,
659  m_GearMotSteer4.bIsSteer,
660  m_GearMotSteer4.dCurrentToTorque,
661  m_GearMotSteer4.dCurrMax,
662  m_GearMotSteer4.iHomingDigIn);
663  }
664 
665  m_IniFile.GetKeyDouble("US", "ScaleToMM", &dScaleToMM, true);
666 
667 
668  // read Platform.ini
669  m_IniFile.SetFileName(sIniDirectory + "Platform.ini", "CanCtrlPltfCOb3.cpp");
670 
671 
672  // ------ WHEEL 1 ------ //
673  // --- Motor Wheel 1 Drive
674  if(m_iNumDrives >= 1)
675  {
676  m_IniFile.GetKeyInt("Config", "Wheel1DriveMotor", &m_Param.iHasWheel1DriveMotor, true);
677  if (m_Param.iHasWheel1DriveMotor == 0)
678  {
679  // No motor
680  std::cout << "node Wheel1DriveMotor available = 0" << std::endl;
681  }
682  else
683  {
684  // Motor Harmonica
685  std::cout << "Wheel1DriveMotor available = type version 2" << std::endl;
686  m_vpMotor[0] = new CanDriveHarmonica();
687  ((CanDriveHarmonica*) m_vpMotor[0])->setCanOpenParam(
688  m_CanOpenIDParam.TxPDO1_W1Drive, m_CanOpenIDParam.TxPDO2_W1Drive, m_CanOpenIDParam.RxPDO2_W1Drive,
689  m_CanOpenIDParam.TxSDO_W1Drive, m_CanOpenIDParam.RxSDO_W1Drive);
690  m_vpMotor[0]->setCanItf(m_pCanCtrl);
691  m_vpMotor[0]->setDriveParam(DriveParamW1DriveMotor);
692  }
693  }
694 
695  // --- Motor Wheel 1 Steer
696  if(m_iNumDrives >= 1)
697  {
698  m_IniFile.GetKeyInt("Config", "Wheel1SteerMotor", &m_Param.iHasWheel1SteerMotor, true);
699  if (m_Param.iHasWheel1SteerMotor == 0)
700  {
701  // No motor
702  std::cout << "node Wheel1SteerMotor available = 0" << std::endl;
703  }
704  else
705  {
706  // Motor Harmonica
707  std::cout << "Wheel1SteerMotor available = type version 2" << std::endl;
708  m_vpMotor[1] = new CanDriveHarmonica();
709  ((CanDriveHarmonica*) m_vpMotor[1])->setCanOpenParam(
710  m_CanOpenIDParam.TxPDO1_W1Steer, m_CanOpenIDParam.TxPDO2_W1Steer, m_CanOpenIDParam.RxPDO2_W1Steer,
711  m_CanOpenIDParam.TxSDO_W1Steer, m_CanOpenIDParam.RxSDO_W1Steer);
712  m_vpMotor[1]->setCanItf(m_pCanCtrl);
713  m_vpMotor[1]->setDriveParam(DriveParamW1SteerMotor);
714 
715  }
716  }
717 
718  // ------ WHEEL 2 ------ //
719  // --- Motor Wheel 2 Drive
720  if(m_iNumDrives >= 2)
721  {
722  m_IniFile.GetKeyInt("Config", "Wheel2DriveMotor", &m_Param.iHasWheel2DriveMotor, true);
723  if (m_Param.iHasWheel2DriveMotor == 0)
724  {
725  // No motor
726  std::cout << "node Wheel2DriveMotor available = 0" << std::endl;
727  }
728  else
729  {
730  // Motor Harmonica
731  std::cout << "Wheel2DriveMotor available = type version 2" << std::endl;
732  m_vpMotor[2] = new CanDriveHarmonica();
733  ((CanDriveHarmonica*) m_vpMotor[2])->setCanOpenParam(
734  m_CanOpenIDParam.TxPDO1_W2Drive, m_CanOpenIDParam.TxPDO2_W2Drive, m_CanOpenIDParam.RxPDO2_W2Drive,
735  m_CanOpenIDParam.TxSDO_W2Drive, m_CanOpenIDParam.RxSDO_W2Drive);
736  m_vpMotor[2]->setCanItf(m_pCanCtrl);
737  m_vpMotor[2]->setDriveParam(DriveParamW2DriveMotor);
738  }
739  }
740 
741  // --- Motor Wheel 2 Steer
742  if(m_iNumDrives >= 2)
743  {
744  m_IniFile.GetKeyInt("Config", "Wheel2SteerMotor", &m_Param.iHasWheel2SteerMotor, true);
745  if (m_Param.iHasWheel2SteerMotor == 0)
746  {
747  // No motor
748  std::cout << "node Wheel2SteerMotor available = 0" << std::endl;
749  }
750  else
751  {
752  // Motor Harmonica
753  std::cout << "Wheel2SteerMotor available = type version 2" << std::endl;
754  m_vpMotor[3] = new CanDriveHarmonica();
755  ((CanDriveHarmonica*) m_vpMotor[3])->setCanOpenParam(
756  m_CanOpenIDParam.TxPDO1_W2Steer, m_CanOpenIDParam.TxPDO2_W2Steer, m_CanOpenIDParam.RxPDO2_W2Steer,
757  m_CanOpenIDParam.TxSDO_W2Steer, m_CanOpenIDParam.RxSDO_W2Steer);
758  m_vpMotor[3]->setCanItf(m_pCanCtrl);
759  m_vpMotor[3]->setDriveParam(DriveParamW2SteerMotor);
760 
761  }
762  }
763 
764  // ------ WHEEL 3 ------ //
765  // --- Motor Wheel 3 Drive
766  if(m_iNumDrives >= 3)
767  {
768  m_IniFile.GetKeyInt("Config", "Wheel3DriveMotor", &m_Param.iHasWheel3DriveMotor, true);
769  if (m_Param.iHasWheel3DriveMotor == 0)
770  {
771  // No motor
772  std::cout << "node Wheel3DriveMotor available = 0" << std::endl;
773  }
774  else
775  {
776  // Motor Harmonica
777  std::cout << "Wheel3DriveMotor available = type version 2" << std::endl;
778  m_vpMotor[4] = new CanDriveHarmonica();
779  ((CanDriveHarmonica*) m_vpMotor[4])->setCanOpenParam(
780  m_CanOpenIDParam.TxPDO1_W3Drive, m_CanOpenIDParam.TxPDO2_W3Drive, m_CanOpenIDParam.RxPDO2_W3Drive,
781  m_CanOpenIDParam.TxSDO_W3Drive, m_CanOpenIDParam.RxSDO_W3Drive);
782  m_vpMotor[4]->setCanItf(m_pCanCtrl);
783  m_vpMotor[4]->setDriveParam(DriveParamW3DriveMotor);
784  }
785  }
786 
787  // --- Motor Wheel 3 Steer
788  if(m_iNumDrives >= 3)
789  {
790  m_IniFile.GetKeyInt("Config", "Wheel3SteerMotor", &m_Param.iHasWheel3SteerMotor, true);
791  if (m_Param.iHasWheel3SteerMotor == 0)
792  {
793  // No motor
794  std::cout << "node Wheel3SteerMotor available = 0" << std::endl;
795  }
796  else
797  {
798  // Motor Harmonica
799  std::cout << "Wheel3SteerMotor available = type version 2" << std::endl;
800  m_vpMotor[5] = new CanDriveHarmonica();
801  ((CanDriveHarmonica*) m_vpMotor[5])->setCanOpenParam(
802  m_CanOpenIDParam.TxPDO1_W3Steer, m_CanOpenIDParam.TxPDO2_W3Steer, m_CanOpenIDParam.RxPDO2_W3Steer,
803  m_CanOpenIDParam.TxSDO_W3Steer, m_CanOpenIDParam.RxSDO_W3Steer);
804  m_vpMotor[5]->setCanItf(m_pCanCtrl);
805  m_vpMotor[5]->setDriveParam(DriveParamW3SteerMotor);
806 
807  }
808  }
809 
810  // ------ WHEEL 4 ------ //
811  // --- Motor Wheel 4 Drive
812  if(m_iNumDrives == 4)
813  {
814  m_IniFile.GetKeyInt("Config", "Wheel4DriveMotor", &m_Param.iHasWheel4DriveMotor, true);
815  if (m_Param.iHasWheel4DriveMotor == 0)
816  {
817  // No motor
818  std::cout << "node Wheel4DriveMotor available = 0" << std::endl;
819  }
820  else
821  {
822  // Motor Harmonica
823  std::cout << "Wheel4DriveMotor available = type version 2" << std::endl;
824  m_vpMotor[6] = new CanDriveHarmonica();
825  ((CanDriveHarmonica*) m_vpMotor[6])->setCanOpenParam(
826  m_CanOpenIDParam.TxPDO1_W4Drive, m_CanOpenIDParam.TxPDO2_W4Drive, m_CanOpenIDParam.RxPDO2_W4Drive,
827  m_CanOpenIDParam.TxSDO_W4Drive, m_CanOpenIDParam.RxSDO_W4Drive);
828  m_vpMotor[6]->setCanItf(m_pCanCtrl);
829  m_vpMotor[6]->setDriveParam(DriveParamW4DriveMotor);
830  }
831  }
832 
833  // --- Motor Wheel 4 Steer
834  if(m_iNumDrives == 4)
835  {
836  m_IniFile.GetKeyInt("Config", "Wheel4SteerMotor", &m_Param.iHasWheel4SteerMotor, true);
837  if (m_Param.iHasWheel4SteerMotor == 0)
838  {
839  // No motor
840  std::cout << "node Wheel4SteerMotor available = 0" << std::endl;
841  }
842  else
843  {
844  // Motor Harmonica
845  std::cout << "Wheel4SteerMotor available = type version 2" << std::endl;
846  m_vpMotor[7] = new CanDriveHarmonica();
847  ((CanDriveHarmonica*) m_vpMotor[7])->setCanOpenParam(
848  m_CanOpenIDParam.TxPDO1_W4Steer, m_CanOpenIDParam.TxPDO2_W4Steer, m_CanOpenIDParam.RxPDO2_W4Steer,
849  m_CanOpenIDParam.TxSDO_W4Steer, m_CanOpenIDParam.RxSDO_W4Steer);
850  m_vpMotor[7]->setCanItf(m_pCanCtrl);
851  m_vpMotor[7]->setDriveParam(DriveParamW4SteerMotor);
852 
853  }
854  }
855 
856  m_IniFile.GetKeyInt("Config", "GenericBufferLen", &iMaxMessages, true);
857 
858 
859 }
860 
861 //-----------------------------------------------
862 int CanCtrlPltfCOb3::evalCanBuffer()
863 {
864  bool bRet;
865 // char cBuf[200];
866 
867  m_Mutex.lock();
868 
869  // as long as there is something in the can buffer -> read out next message
870  while(m_pCanCtrl->receiveMsg(&m_CanMsgRec) == true)
871  {
872  bRet = false;
873  // check for every motor if message belongs to it
874  for (unsigned int i = 0; i < m_vpMotor.size(); i++)
875  {
876  // if message belongs to this motor write data (Pos, Vel, ...) to internal member vars
877  bRet |= m_vpMotor[i]->evalReceivedMsg(m_CanMsgRec);
878  }
879 
880  if (bRet == false)
881  {
882  std::cout << "evalCanBuffer(): Received CAN_Message with unknown identifier " << m_CanMsgRec.m_iID << std::endl;
883  }
884  };
885 
886 
887  m_Mutex.unlock();
888 
889  return 0;
890 }
891 
892 //-----------------------------------------------
893 bool CanCtrlPltfCOb3::initPltf()
894 {
895  // read Configuration parameters from Inifile
896  readConfiguration();
897 
898  // Vectors for drive objects and return values
899  std::vector<bool> vbRetDriveMotor;
900  std::vector<bool> vbRetSteerMotor;
901  std::vector<CanDriveItf*> vpDriveMotor;
902  std::vector<CanDriveItf*> vpSteerMotor;
903  bool bHomingOk;
904 
905 // vbRetDriveMotor.assign(4,0);
906 // vbRetSteerMotor.assign(4,0);
907  vbRetDriveMotor.assign(m_iNumDrives,0);
908  vbRetSteerMotor.assign(m_iNumDrives,0);
909 
910  // Homing is done on a wheel-module base (steering and driving needs to be synchronized)
911  // copy Motor-Pointer into Steer/Drive vector for more insight
912  for(int i=0; i<=m_iNumMotors; i+=2)
913  vpDriveMotor.push_back(m_vpMotor[i]);
914 // vpDriveMotor.push_back(m_vpMotor[2]);
915 // vpDriveMotor.push_back(m_vpMotor[4]);
916 // vpDriveMotor.push_back(m_vpMotor[6]);
917  for(int i=1; i<=m_iNumMotors; i+=2)
918  vpSteerMotor.push_back(m_vpMotor[i]);
919 // vpSteerMotor.push_back(m_vpMotor[3]);
920 // vpSteerMotor.push_back(m_vpMotor[5]);
921 // vpSteerMotor.push_back(m_vpMotor[7]);
922 
923  std::vector<double> vdFactorVel;
924 // vdFactorVel.assign(4,0);
925  vdFactorVel.assign(m_iNumDrives,0);
926 
927 
928  // Start can open network
929  std::cout << "StartCanOpen" << std::endl;
930  sendNetStartCanOpen();
931 
932 
933  // initialize drives
934 
935  // 1st init watchdogs
936  std::cout << "Initialization of Watchdogs" << std::endl;
937  for(int i=0; i<m_iNumMotors; i++)
938  {
939  m_vpMotor[i]->startWatchdog(true);
940  }
941 /* m_vpMotor[0]->startWatchdog(true);
942  m_vpMotor[1]->startWatchdog(true);
943  m_vpMotor[2]->startWatchdog(true);
944  m_vpMotor[3]->startWatchdog(true);
945  m_vpMotor[4]->startWatchdog(true);
946  m_vpMotor[5]->startWatchdog(true);
947  m_vpMotor[6]->startWatchdog(true);
948  m_vpMotor[7]->startWatchdog(true);
949 */
950  usleep(10000);
951 
952  // 2nd send watchdogs to bed while initializing drives
953  for(int i=0; i<m_iNumMotors; i++)
954  {
955  m_vpMotor[i]->startWatchdog(false);
956  }
957 /* m_vpMotor[0]->startWatchdog(false);
958  m_vpMotor[1]->startWatchdog(false);
959  m_vpMotor[2]->startWatchdog(false);
960  m_vpMotor[3]->startWatchdog(false);
961  m_vpMotor[4]->startWatchdog(false);
962  m_vpMotor[5]->startWatchdog(false);
963  m_vpMotor[6]->startWatchdog(false);
964  m_vpMotor[7]->startWatchdog(false);
965 */
966  usleep(100000);
967 
968  std::cout << "Initialization of Watchdogs done" << std::endl;
969 
970 
971  // ---------------------- start homing procedurs
972  // Perform homing of all wheels simultaneously
973  // o.k. to avoid crashing hardware -> lets check that we have at least the 8 motors, like we have on cob
974  if( (int)m_vpMotor.size() == m_iNumMotors )
975  {
976  // Initialize and start all motors
977  for (int i = 0; i<m_iNumDrives; i++)
978  {
979  // init + start
980  vbRetDriveMotor[i] = vpDriveMotor[i]->init();
981  vbRetSteerMotor[i] = vpSteerMotor[i]->init();
982  usleep(10000);
983  vbRetDriveMotor[i] = vbRetDriveMotor[i] && vpDriveMotor[i]->start();
984  vbRetSteerMotor[i] = vbRetSteerMotor[i] && vpSteerMotor[i]->start();
985  usleep(10000);
986  // output State / Errors
987  if (vbRetDriveMotor[i] && vbRetSteerMotor[i])
988  std::cout << "Initialization of Wheel "<< (i+1) << " OK" << std::endl;
989  else if (!vbRetDriveMotor[i] && vbRetSteerMotor[i])
990  std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing DRIVE-Motor" << std::endl;
991  else if (vbRetDriveMotor[i] && !vbRetSteerMotor[i])
992  std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing STEER-Motor" << std::endl;
993  else
994  std::cout << "Initialization of Wheel "<< (i+1) << " ERROR while initializing STEER- and DRIVE-Motor" << std::endl;
995  // Just to be sure: Set vel to zero
996  vpDriveMotor[i]->setGearVelRadS(0);
997  vpSteerMotor[i]->setGearVelRadS(0);
998  }
999 
1000  // perform homing only when ALL drives are ERROR-Free
1001 // if (vbRetDriveMotor[0] && vbRetDriveMotor[1] && vbRetDriveMotor[2] && vbRetDriveMotor[3] &&
1002 // vbRetSteerMotor[0] && vbRetSteerMotor[1] && vbRetSteerMotor[2] && vbRetSteerMotor[3])
1003  bHomingOk = true;
1004  for(int i=0; i<m_iNumDrives; i++)
1005  {
1006  if((vbRetDriveMotor[i] && vbRetSteerMotor[i]) == false)
1007  bHomingOk = false;
1008  }
1009  if(bHomingOk)
1010  {
1011  // Calc Compensation factor for Velocity:
1012  if(m_iNumDrives >= 1)
1013  vdFactorVel[0] = - m_Param.dWheel1SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
1014  if(m_iNumDrives >= 2)
1015  vdFactorVel[1] = - m_Param.dWheel2SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
1016  if(m_iNumDrives >= 3)
1017  vdFactorVel[2] = - m_Param.dWheel3SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
1018  if(m_iNumDrives == 4)
1019  vdFactorVel[3] = - m_Param.dWheel4SteerDriveCoupling + double(m_Param.iDistSteerAxisToDriveWheelMM) / double(m_Param.iRadiusWheelMM);
1020 
1021  // initialize homing procedure
1022  for (int i = 0; i<m_iNumDrives; i++)
1023  vpSteerMotor[i]->initHoming();
1024 
1025  // make motors move
1026  for (int i = 0; i<m_iNumDrives; i++)
1027  {
1028  vpSteerMotor[i]->setGearVelRadS(m_Param.dHomeVeloRadS);
1029  vpDriveMotor[i]->setGearVelRadS(m_Param.dHomeVeloRadS * vdFactorVel[i]);
1030  }
1031 
1032  // wait at least 0.5 sec.
1033  usleep(500000);
1034 
1035  // Get rid of unnecessary can messages
1036  bool bRet;
1037  do
1038  bRet = m_pCanCtrl->receiveMsg(&m_CanMsgRec);
1039  while(bRet == true);
1040 
1041  // arm homing procedure
1042  for (int i = 0; i<m_iNumDrives; i++)
1043  vpSteerMotor[i]->IntprtSetInt(8, 'H', 'M', 1, 1);
1044 
1045  // wait until all steers are homed
1046  bool bAllDone, bTimeOut=false;
1047  int iCnt = 0;
1048  do
1049  {
1050  // send request for homing status
1051  for (int i = 0; i<m_iNumDrives; i++)
1052  vpSteerMotor[i]->IntprtSetInt(4, 'H', 'M', 1, 0);
1053 
1054  // eval Can Messages
1055  evalCanBuffer();
1056 
1057  // set already homed wheels to zero velocity
1058  bAllDone = true;
1059  for (int i = 0; i<m_iNumDrives; i++)
1060  {
1061  if (vpSteerMotor[i]->getStatusLimitSwitch())
1062  {
1063  vpSteerMotor[i]->setGearVelRadS(0);
1064  vpDriveMotor[i]->setGearVelRadS(0);
1065  }
1066  bAllDone = bAllDone && vpSteerMotor[i]->getStatusLimitSwitch();
1067  }
1068 
1069  // increment timeout counter
1070  if (iCnt++ > 1000) //cpc-ck has 500 for Cob 3.5 here
1071  bTimeOut = true;
1072 
1073  // Sleep: To avoid can overload
1074  usleep(20000);
1075  }
1076  while(!bAllDone && !bTimeOut);
1077 
1078  // Output State
1079  if (bTimeOut)
1080  {
1081  for (int i=0;i<m_iNumDrives;i++)
1082  {
1083  vpSteerMotor[i]->setGearVelRadS(0);
1084  vpDriveMotor[i]->setGearVelRadS(0);
1085  }
1086  std::cout << "Error while Homing: Timeout while waiting for homing signal" << std::endl;
1087  }
1088 
1089  // update Can Buffer once more
1090  // read current can buffer and update position and velocity measurements
1091  evalCanBuffer();
1092 
1093  // Now make steers move to position: zero
1094  // this could be handled also by the elmos themselve
1095  // however this way synchronization of steering and driving is better
1096  double m_d0 = 2.5;
1097  if (bTimeOut == false)
1098  {
1099  do
1100  {
1101  bAllDone = true;
1102  for (int i = 0; i<m_iNumDrives; i++)
1103  {
1104  // get current position of steer
1105  double dCurrentPosRad;
1106  vpSteerMotor[i]->getGearPosRad(&dCurrentPosRad);
1107  // P-Ctrl
1108  double dDeltaPhi = 0.0 - dCurrentPosRad;
1109  // check if steer is at pos zero
1110  if (fabs(dDeltaPhi) < 0.03) //alter Wert=0.03
1111  {
1112  dDeltaPhi = 0.0;
1113  }
1114  else
1115  {
1116  bAllDone = false;
1117  }
1118  double dVelCmd = m_d0 * dDeltaPhi;
1119  // set Outputs
1120  vpSteerMotor[i]->setGearVelRadS(dVelCmd);
1121  vpDriveMotor[i]->setGearVelRadS(dVelCmd*vdFactorVel[i]);
1122  }
1123  usleep(20000);
1124  evalCanBuffer();
1125  } while(!bAllDone);
1126 
1127 
1128  // Homing done. Wheels at position zero (+/- 0.5°)
1129  std::cout << "Wheels homed" << std::endl;
1130  for (int i=0;i<m_iNumDrives;i++)
1131  {
1132  vpSteerMotor[i]->setGearVelRadS(0);
1133  vpDriveMotor[i]->setGearVelRadS(0);
1134  }
1135  }
1136  }
1137  }
1138  // ---------------------- end homing procedure
1139 
1140  // homing done -> wake up watchdogs
1141  for(int i=0; i<m_iNumMotors; i++)
1142  {
1143  m_vpMotor[i]->startWatchdog(true);
1144  }
1145 /* m_vpMotor[0]->startWatchdog(true);
1146  m_vpMotor[1]->startWatchdog(true);
1147  m_vpMotor[2]->startWatchdog(true);
1148  m_vpMotor[3]->startWatchdog(true);
1149  m_vpMotor[4]->startWatchdog(true);
1150  m_vpMotor[5]->startWatchdog(true);
1151  m_vpMotor[6]->startWatchdog(true);
1152  m_vpMotor[7]->startWatchdog(true);
1153 */
1154 // return (
1155 // vbRetDriveMotor[0] && vbRetDriveMotor[1] && vbRetDriveMotor[2] && vbRetDriveMotor[3] &&
1156 // vbRetSteerMotor[0] && vbRetSteerMotor[1] && vbRetSteerMotor[2] && vbRetSteerMotor[3]);
1157  bHomingOk = true;
1158  for(int i=0; i<m_iNumDrives; i++)
1159  {
1160  if((vbRetDriveMotor[i] && vbRetSteerMotor[i]) == false)
1161  {
1162  bHomingOk = false;
1163  }
1164  }
1165  return (bHomingOk);
1166 }
1167 
1168 //-----------------------------------------------
1169 bool CanCtrlPltfCOb3::resetPltf()
1170 {
1171  bool bRetMotor = true;
1172  bool bRet = true;
1173 
1174  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1175  {
1176  bRetMotor = m_vpMotor[i]->start();
1177  if (bRetMotor == true)
1178  {
1179  m_vpMotor[i]->setGearVelRadS(0);
1180  }
1181  else
1182  {
1183  std::cout << "Resetting of Motor " << i << " failed" << std::endl;
1184  }
1185 
1186  bRet &= bRetMotor;
1187  }
1188  return(bRet);
1189 }
1190 
1191 //-----------------------------------------------
1192 bool CanCtrlPltfCOb3::shutdownPltf()
1193 {
1194 
1195  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1196  {
1197  m_vpMotor[i]->shutdown();
1198  }
1199 
1200  return true;
1201 }
1202 
1203 //-----------------------------------------------
1204 bool CanCtrlPltfCOb3::isPltfError()
1205 {
1206 
1207  bool bErrMotor = false;
1208  std::vector<bool> vbErrMotor;
1209  vbErrMotor.resize(m_vpMotor.size());
1210 
1211  // check all motors for errors
1212  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1213  {
1214  vbErrMotor[i] = m_vpMotor[i]->isError();
1215  // if watchdog is not yet in error state but one motor is errorneous Log Out Error-Msg
1216  if( (m_bWatchdogErr == false) && (vbErrMotor[i] == true) )
1217  {
1218  std::cout << "Motor " << i << " error" << std::endl;
1219  }
1220  // check whether no motor was errorneous
1221  bErrMotor |= vbErrMotor[i];
1222  }
1223 
1224  // if no motor is in error state force Watchdog to "No Error" state
1225  if(bErrMotor == false)
1226  m_bWatchdogErr = false;
1227  else
1228  m_bWatchdogErr = true;
1229 
1230  if (m_bWatchdogErr) return true;
1231 
1232 
1233  // Check communication
1234  double dWatchTime = 0;
1235 
1236  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1237  {
1238  dWatchTime = m_vpMotor[i]->getTimeToLastMsg();
1239 
1240  if(dWatchTime < m_Param.dCanTimeout)
1241  {
1242  m_bWatchdogErr = false;
1243  }
1244  if( (dWatchTime > m_Param.dCanTimeout) && (m_bWatchdogErr == false) )
1245  {
1246  std::cout << "timeout CAN motor " << i << std::endl;
1247  m_bWatchdogErr = true;
1248  return true;
1249  }
1250  }
1251 
1252  return false;
1253 }
1254 
1255 //-----------------------------------------------
1256 bool CanCtrlPltfCOb3::startWatchdog(bool bStarted)
1257 {
1258 
1259  bool bRet = true;
1260 
1261  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1262  {
1263  bRet = m_vpMotor[i]->startWatchdog(bStarted);
1264  }
1265 
1266  return (bRet);
1267 }
1268 
1269 //-----------------------------------------------
1270 void CanCtrlPltfCOb3::sendNetStartCanOpen()
1271 {
1272  CanMsg msg;
1273 
1274  msg.m_iID = 0;
1275  msg.m_iLen = 2;
1276  msg.set(1,0,0,0,0,0,0,0);
1277  m_pCanCtrl->transmitMsg(msg, false);
1278 
1279  usleep(100000);
1280 }
1281 
1282 
1283 //-----------------------------------------------
1284 // Motor Controlers
1285 //-----------------------------------------------
1286 
1287 
1288 //-----------------------------------------------
1289 int CanCtrlPltfCOb3::setVelGearRadS(int iCanIdent, double dVelGearRadS)
1290 {
1291  m_Mutex.lock();
1292 
1293  // If an error was detected and processed in isPltfErr().
1294  if (m_bWatchdogErr == true)
1295  {
1296  // Error -> Stop motor driving
1297  dVelGearRadS = 0;
1298  // The error is detected and quitted in the funtion isPltfErr().
1299  }
1300 
1301  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1302  {
1303  // check if Identifier fits to availlable hardware
1304  if(iCanIdent == m_viMotorID[i])
1305  {
1306  m_vpMotor[i]->setGearVelRadS(dVelGearRadS);
1307  }
1308  }
1309 
1310  m_Mutex.unlock();
1311 
1312  return 0;
1313 }
1314 
1315 //-----------------------------------------------
1316 int CanCtrlPltfCOb3::requestMotPosVel(int iCanIdent)
1317 {
1318  m_Mutex.lock();
1319 
1320  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1321  {
1322  // check if Identifier fits to availlable hardware
1323  if(iCanIdent == m_viMotorID[i])
1324  {
1325  m_vpMotor[i]->requestPosVel();
1326  }
1327  }
1328 
1329  m_Mutex.unlock();
1330 
1331  return 0;
1332 }
1333 
1334 //-----------------------------------------------
1335 void CanCtrlPltfCOb3::requestDriveStatus()
1336 {
1337  m_Mutex.lock();
1338 
1339  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1340  {
1341  m_vpMotor[i]->requestStatus();
1342  }
1343 
1344  m_Mutex.unlock();
1345 }
1346 
1347 //-----------------------------------------------
1348 int CanCtrlPltfCOb3::getGearPosVelRadS(int iCanIdent, double* pdAngleGearRad, double* pdVelGearRadS)
1349 {
1350 
1351  // init default outputs
1352  *pdAngleGearRad = 0;
1353  *pdVelGearRadS = 0;
1354 
1355  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1356  {
1357  // check if Identifier fits to availlable hardware
1358  if(iCanIdent == m_viMotorID[i])
1359  {
1360  m_vpMotor[i]->getGearPosVelRadS(pdAngleGearRad, pdVelGearRadS);
1361  }
1362  }
1363 
1364  return 0;
1365 }
1366 
1367 //-----------------------------------------------
1368 int CanCtrlPltfCOb3::getGearDeltaPosVelRadS(int iCanIdent, double* pdAngleGearRad,
1369  double* pdVelGearRadS)
1370 {
1371 
1372  // init default outputs
1373  *pdAngleGearRad = 0;
1374  *pdVelGearRadS = 0;
1375 
1376  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1377  {
1378  // check if Identifier fits to availlable hardware
1379  if(iCanIdent == m_viMotorID[i])
1380  {
1381  m_vpMotor[i]->getGearDeltaPosVelRadS(pdAngleGearRad, pdVelGearRadS);
1382  }
1383  }
1384 
1385  return 0;
1386 }
1387 
1388 //-----------------------------------------------
1389 void CanCtrlPltfCOb3::getStatus(int iCanIdent, int* piStatus, int* piTempCel)
1390 {
1391  // init default outputs
1392  *piStatus = 0;
1393  *piTempCel = 0;
1394 
1395  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1396  {
1397  // check if Identifier fits to availlable hardware
1398  if(iCanIdent == m_viMotorID[i])
1399  {
1400  m_vpMotor[i]->getStatus(piStatus, piTempCel);
1401  }
1402  }
1403 
1404 }
1405 //-----------------------------------------------
1406 void CanCtrlPltfCOb3::requestMotorTorque()
1407 {
1408  m_Mutex.lock();
1409 
1410  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1411  {
1412  m_vpMotor[i]->requestMotorTorque();
1413  }
1414 
1415  m_Mutex.unlock();
1416 }
1417 
1418 //-----------------------------------------------
1419 void CanCtrlPltfCOb3::getMotorTorque(int iCanIdent, double* pdTorqueNm)
1420 {
1421  // init default outputs
1422  *pdTorqueNm = 0;
1423 
1424  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1425  {
1426  // check if Identifier fits to availlable hardware
1427  if(iCanIdent == m_viMotorID[i])
1428  {
1429  m_vpMotor[i]->getMotorTorque(pdTorqueNm);
1430  }
1431  }
1432 
1433 }
1434 //-----------------------------------------------
1435 void CanCtrlPltfCOb3::setMotorTorque(int iCanIdent, double dTorqueNm)
1436 {
1437  m_Mutex.lock();
1438 
1439  for(unsigned int i = 0; i < m_vpMotor.size(); i++)
1440  {
1441  // check if Identifier fits to availlable hardware
1442  if(iCanIdent == m_viMotorID[i])
1443  {
1444  m_vpMotor[i]->setMotorTorque(dTorqueNm);
1445  }
1446  }
1447 
1448  m_Mutex.unlock();
1449 }
1450 
1451 //-----------------------------------------------
1452 // Read out Elmo-Recorder via CAN (cpc-pk)
1453 //-----------------------------------------------
1454 
1455 //-----------------------------------------------
1456 int CanCtrlPltfCOb3::ElmoRecordings(int iFlag, int iParam, std::string sString) {
1457  int tempRet = 0;
1458  int bRet = 0;
1459 
1460  switch(iFlag) {
1461  case 0: //Flag = 0 means reset recorder and configure it
1462  for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
1463  m_vpMotor[i]->setRecorder(0, iParam); //Configure Elmo Recorder with RecordingGap and start immediately
1464  }
1465  return 0;
1466 
1467  case 1: //Flag = 1 means start readout process, mustn't be called too early (while Rec is in process..)
1468  for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
1469  if((tempRet = m_vpMotor[i]->setRecorder(1, iParam, sString)) > bRet) {
1470  bRet = tempRet; //Query Readout of Index to Log Directory
1471  }
1472  }
1473  return bRet;
1474 
1475  case 99:
1476  for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
1477  m_vpMotor[i]->setRecorder(99, 0); //Stop any ongoing SDO transfer and clear corresponding data.
1478  }
1479  return 0;
1480 
1481  case 100:
1482  for(unsigned int i = 0; i < m_vpMotor.size(); i++) {
1483  bRet += m_vpMotor[i]->setRecorder(2, 0); //Request state of transmission
1484  }
1485  return bRet;
1486 
1487  default:
1488  return -1;
1489  }
1490 }
CanCtrlPltfCOb3::sIniDirectory
std::string sIniDirectory
Definition: CanCtrlPltfCOb3.h:218
CanCtrlPltfCOb3::m_GearMotDrive3
GearMotorParamType m_GearMotDrive3
Definition: CanCtrlPltfCOb3.h:393
CanCtrlPltfCOb3::setVelGearRadS
int setVelGearRadS(int iCanIdent, double dVelGearRadS)
Definition: CanCtrlPltfCOb3.cpp:1289
CanCtrlPltfCOb3::sComposed
std::string sComposed
Definition: CanCtrlPltfCOb3.h:219
DriveParam::setParam
void setParam(int iDriveIdent, int iEncIncrPerRevMot, double dVelMeasFrqHz, double dBeltRatio, double dGearRatio, int iSign, double dVelMaxEncIncrS, double dAccIncrS2, double dDecIncrS2)
CanCtrlPltfCOb3::m_GearMotDrive1
GearMotorParamType m_GearMotDrive1
Definition: CanCtrlPltfCOb3.h:391
CanCtrlPltfCOb3::m_CanOpenIDParam
CanOpenIDType m_CanOpenIDParam
Definition: CanCtrlPltfCOb3.h:388
CanCtrlPltfCOb3::m_GearMotSteer1
GearMotorParamType m_GearMotSteer1
Definition: CanCtrlPltfCOb3.h:395
CanCtrlPltfCOb3::requestMotPosVel
int requestMotPosVel(int iCanIdent)
Definition: CanCtrlPltfCOb3.cpp:1316
CanMsg::m_iLen
int m_iLen
IniFile::GetKeyBool
int GetKeyBool(const char *pSect, const char *pKey, bool *pValue, bool bWarnIfNotfound=true)
CanCtrlPltfCOb3::m_vpMotor
std::vector< CanDriveItf * > m_vpMotor
Definition: CanCtrlPltfCOb3.h:423
CanCtrlPltfCOb3::getGearDeltaPosVelRadS
int getGearDeltaPosVelRadS(int iCanIdent, double *pdDeltaAngleGearRad, double *pdVelGearRadS)
Definition: CanCtrlPltfCOb3.cpp:1368
CanCtrlPltfCOb3::getMotorTorque
void getMotorTorque(int iCanIdent, double *pdTorqueNm)
Definition: CanCtrlPltfCOb3.cpp:1419
IniFile::GetKeyInt
int GetKeyInt(const char *pSect, const char *pKey, int *pValue, bool bWarnIfNotfound=true)
CanCtrlPltfCOb3::startWatchdog
bool startWatchdog(bool bStarted)
Definition: CanCtrlPltfCOb3.cpp:1256
CanCtrlPltfCOb3::m_GearMotDrive4
GearMotorParamType m_GearMotDrive4
Definition: CanCtrlPltfCOb3.h:394
CanCtrlPltfCOb3::getGearPosVelRadS
int getGearPosVelRadS(int iCanIdent, double *pdAngleGearRad, double *pdVelGearRadS)
Definition: CanCtrlPltfCOb3.cpp:1348
IniFile::GetKeyDouble
int GetKeyDouble(const char *pSect, const char *pKey, double *pValue, bool bWarnIfNotfound=true)
CanCtrlPltfCOb3::getStatus
void getStatus(int iCanIdent, int *piStatus, int *piTempCel)
Definition: CanCtrlPltfCOb3.cpp:1389
Mutex::lock
bool lock(unsigned int uiTimeOut=INFINITE)
CanCtrlPltfCOb3::m_GearMotSteer4
GearMotorParamType m_GearMotSteer4
Definition: CanCtrlPltfCOb3.h:398
CanCtrlPltfCOb3::m_GearMotSteer3
GearMotorParamType m_GearMotSteer3
Definition: CanCtrlPltfCOb3.h:397
CanCtrlPltfCOb3::m_GearMotDrive2
GearMotorParamType m_GearMotDrive2
Definition: CanCtrlPltfCOb3.h:392
CanCtrlPltfCOb3::CanCtrlPltfCOb3
CanCtrlPltfCOb3(std::string iniDirectory)
Definition: CanCtrlPltfCOb3.cpp:32
IniFile::SetFileName
int SetFileName(std::string fileName, std::string strIniFileUsedBy="", bool bCreate=false)
CanCtrlPltfCOb3::m_viMotorID
std::vector< int > m_viMotorID
Definition: CanCtrlPltfCOb3.h:426
CanItf::receiveMsg
virtual bool receiveMsg(CanMsg *pCMsg)=0
CanCtrlPltfCOb3::setMotorTorque
void setMotorTorque(int iCanIdent, double dTorqueNm)
Definition: CanCtrlPltfCOb3.cpp:1435
CanCtrlPltfCOb3::m_GearMotSteer2
GearMotorParamType m_GearMotSteer2
Definition: CanCtrlPltfCOb3.h:396
CanMsg::m_iID
int m_iID
CanMsg::set
void set(BYTE Data0=0, BYTE Data1=0, BYTE Data2=0, BYTE Data3=0, BYTE Data4=0, BYTE Data5=0, BYTE Data6=0, BYTE Data7=0)
Mutex::unlock
void unlock()
CanCtrlPltfCOb3::ElmoRecordings
int ElmoRecordings(int iFlag, int iParam, std::string sString)
Definition: CanCtrlPltfCOb3.cpp:1456
CanItf::transmitMsg
virtual bool transmitMsg(CanMsg CMsg, bool bBlocking=true)=0

cob_base_drive_chain
Author(s): Christian Connette
autogenerated on Wed Apr 7 2021 02:11:54