ElmoRecorder.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 #include <math.h>
19 #include <vector>
20 #include <stdio.h>
21 #include <sstream>
24 
26  m_pHarmonicaDrive = pParentHarmonicaDrive;
27 
28  m_bIsInitialized = false;
30 }
31 
33 }
34 
35 bool ElmoRecorder::isInitialized(bool initNow) {
36  if(initNow) m_bIsInitialized = true;
37  return m_bIsInitialized;
38 }
39 
40 int ElmoRecorder::configureElmoRecorder(int iRecordingGap, int driveID, int startImmediately){ //iRecordingGap = N indicates that a new sample should be taken once per N time quanta
41  m_iDriveID = driveID;
42 
43  if(startImmediately >=2 ) startImmediately = 1;
44 
45  m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'R', 0, 0); // Stop Recorder if it's active
46  // Record Main speed (index 0, ID1)
47  // Active Current (index 9, ID10)
48  // Main Position (index 1, ID2)
49  // Speed Command (index 15, ID16)
50  // RC = 2^(Signal1Index) + 2^(Signal2Index) + ..; e.g.: 2^0 + 2^1 + 2^9 + 2^15 = 33283;
51  m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'C', 0, 33283);
52  // Set trigger type to immediate
53  m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'P', 3, 0);
54  // Set Recording Gap
55  m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'G', 0, iRecordingGap);
56  // Set Recording Length
57  // RL = (4096 / Number of Signals)
58  m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'L', 0, 1024);
59 
60  // Set Time Quantum, Default: RP=0 -> TS * 4; TS is 90us by default
61  // m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'P', 0, 0);
62  // ----> Total Recording Time = 90us * 4 * RG * RL
63 
64  m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'R', 0, startImmediately + 1); //2 launches immediately (8, 'R', 'R', 0, 1) launches at next BG
65 
66  m_fRecordingStepSec = 0.000090 * 4 * iRecordingGap;
67 
68  return 0;
69 }
70 
71 int ElmoRecorder::readoutRecorderTry(int iObjSubIndex) {
72  //Request the SR (status register) and begin all the read-out process with this action.
73  //SDOData.statusFlag is segData::SDO_SEG_WAITING;
74 
76  m_iCurrentObject = iObjSubIndex;
77 
79 
80  return 0;
81 }
82 
83 int ElmoRecorder::readoutRecorderTryStatus(int iStatusReg, segData& SDOData) {
84  if(m_iReadoutRecorderTry == 0) return 0; //only evaluate the SR, if we are really waiting for it (to save time and not to unintionally start a read-out)
85 
87 
88  //Bits 16-17 of status register contain recorder information
89  int iRecorderStatus = (0x30000 & iStatusReg) >> 16;
90 
91  if(iRecorderStatus == 0) {
92  std::cout << "Recorder " << m_iDriveID << " inactive with no valid data to upload" << std::endl;
94  } else if(iRecorderStatus == 1) {
95  std::cout << "Recorder " << m_iDriveID << " waiting for a trigger event" << std::endl;
97  } else if(iRecorderStatus == 2) {
98  std::cout << "Recorder " << m_iDriveID << " finished, valid data ready for use" << std::endl;
100  //already set to SDOData.statusFlag = segData::SDO_SEG_WAITING;
101  } else if(iRecorderStatus == 3) {
102  std::cout << "Recorder " << m_iDriveID << " is still recording" << std::endl;
104  }
105 
106  return 0;
107 }
108 
109 int ElmoRecorder::readoutRecorder(int iObjSubIndex){
110  //initialize Upload of Recorded Data (object 0x2030)
111  int iObjIndex = 0x2030;
112 
113  m_pHarmonicaDrive->sendSDOUpload(iObjIndex, iObjSubIndex);
114  m_iCurrentObject = iObjSubIndex;
115 
116  return 0;
117 }
118 
120  int iItemSize = 4;
121  int iItemCount = 0;
122  unsigned int iNumDataItems = 0;
123  bool bCollectFloats = true;
124  float fFloatingPointFactor = 0;
125 
126  std::vector<float> vfResData[2];
127 
128  //see SimplIQ CANopen DS 301 Implementation Guide, object 0x2030
129 
130  //HEADER
131  //--------------------------------------
132  //First 7 Bytes of the data sequence contain header information:
133  //Byte 0: First four bits: 4 = Long Int data type, 1 = Half Float data type, 5 = Double Float
134  // Next four bits: Recording frequency in 1 per n * TS => deltaT = n * 90┬Ásec
135  //Byte 2, Byte 3: Number of recorded data points
136  //Byte 3 to 6: Floating point factor for data to be multiplied with
137  //
138  //Byte 7 to Byte (7+ iNumdataItems * 4) contain data
139 
140  //B[0]: Time quantum and data type
141  switch ((SDOData.data[0] >> 4) ) {
142  case 4:
143  bCollectFloats = false;
144  iItemSize = 4;
145  break;
146  case 5:
147  bCollectFloats = true;
148  iItemSize = 4;
149  break;
150  case 1:
151  bCollectFloats = true;
152  iItemSize = 2;
153  break;
154  default:
155  bCollectFloats = false;
156  iItemSize = 4;
157  break;
158  }
159  std::cout << ">>>>>ElmoRec: HEADER INFOS<<<<<\nData type is: " << (SDOData.data[0] >> 4) << std::endl;
160 
161  //std::cout << "fTimeQuantum from Header is " << fTimeQuantum << " m_fRecordingStepSec is " << m_fRecordingStepSec << std::endl;
162 
163 
164  //B[1]..[2] //Number of recorded items
165  iNumDataItems = (SDOData.data[2] << 8 | SDOData.data[1]);
166  //std::cout << "Number of recorded data points: " << iNumDataItems << std::endl;
167 
168  //B[3] ... [6] //Floating point factor
169  fFloatingPointFactor = convertBinaryToFloat( (SDOData.data[6] << 24) | (SDOData.data[5] << 16) | (SDOData.data[4] << 8) | (SDOData.data[3]) );
170  std::cout << "Floating point factor for recorded values is: " << fFloatingPointFactor << std::endl;
171 
172 
173  if( ((SDOData.numTotalBytes-7)/iItemSize) != iNumDataItems)
174  std::cout << "SDODataSize announced in SDO-Header" << ((SDOData.numTotalBytes-7)/iItemSize) << " differs from NumDataItems by ElmoData-Header" << iNumDataItems << std::endl;
175  //END HEADER
176  //--------------------------------------
177 
178  vfResData[0].assign(iNumDataItems, 0.0);
179  vfResData[1].assign(iNumDataItems, 0.0);
180  iItemCount = 0;
181 
182  //extract values from data stream, consider Little Endian conversion for every single object!
183  for(unsigned int i=7;i<=SDOData.data.size() - iItemSize; i=i+iItemSize) {
184  if(bCollectFloats) {
185  if(iItemSize == 4)
186  vfResData[1][iItemCount] = fFloatingPointFactor * convertBinaryToFloat( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) );
187 
188  //DEBUG
189  if(iItemCount == 120)
190  std::cout << (unsigned int)( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) ) << std::endl;
191 
192  else vfResData[1][iItemCount] = fFloatingPointFactor * convertBinaryToHalfFloat( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) );
193  iItemCount ++;
194  } else {
195  vfResData[1][iItemCount] = fFloatingPointFactor * (float)( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) );
196  iItemCount ++;
197  }
198 
199  vfResData[0][iItemCount] = m_fRecordingStepSec * iItemCount;
200  }
201 
202  logToFile(m_sLogFilename, vfResData);
203 
205  return 0;
206 }
207 
208 int ElmoRecorder::setLogFilename(std::string sLogFileprefix) {
209  m_sLogFilename = sLogFileprefix;
210  return 0;
211 }
212 
213 
214 
215 float ElmoRecorder::convertBinaryToFloat(unsigned int iBinaryRepresentation) {
216  //Converting binary-numbers to 32bit float values according to IEEE 754 see http://de.wikipedia.org/wiki/IEEE_754
217  int iSign;
218  int iExponent;
219  unsigned int iMantissa;
220  float iNumMantissa = 0.0f;
221 
222  if((iBinaryRepresentation & (1 << 31)) == 0) //first bit is sign bit: 0 = +, 1 = -
223  iSign = 1;
224  else
225  iSign = -1;
226 
227  iExponent = ((iBinaryRepresentation >> 23) & 0xFF) - 127; //take away Bias(127) for positive and negative exponents
228 
229  iMantissa = (iBinaryRepresentation & 0x7FFFFF); //only keep mantissa part of binary number
230 
231  iNumMantissa = 1.0f;
232 
233  for(int i=1; i<=23; i++) { //calculate decimal places (convert binary mantissa to decimal number
234  if((iMantissa & (1 << (23-i))) > 0) {
235  iNumMantissa = iNumMantissa + pow(2,(-1)*i);
236  }
237  }
238 
239  return iSign * pow(2,iExponent) * iNumMantissa;
240 }
241 
242 float ElmoRecorder::convertBinaryToHalfFloat(unsigned int iBinaryRepresentation) {
243  //Converting binary-numbers to 16bit float values according to IEEE 754 see http://de.wikipedia.org/wiki/IEEE_754
244  int iSign;
245  int iExponent;
246  unsigned int iMantissa;
247  float iNumMantissa = 0.0f;
248 
249  if((iBinaryRepresentation & (1 << 15)) == 0) //first bit is sign bit: 0 = +, 1 = -
250  iSign = 1;
251  else
252  iSign = -1;
253 
254  iExponent = ((iBinaryRepresentation >> 10) & 0x1F) - 15; //take away Bias(15) for positive and negative exponents
255 
256  iMantissa = (iBinaryRepresentation & 0x3FF); //only keep mantissa part of binary number
257 
258  iNumMantissa = 1.0f;
259 
260  for(int i=1; i<=10; i++) { //calculate decimal places (convert binary mantissa to decimal number
261  if((iMantissa & (1 << (10-i))) > 0) {
262  iNumMantissa = iNumMantissa + pow(2,(-1)*i);
263  }
264  }
265 
266  return iSign * pow(2,iExponent) * iNumMantissa;
267 }
268 
269 // Function for writing Logfile
270 int ElmoRecorder::logToFile(std::string filename, std::vector<float> vtValues[]) {
271  std::stringstream outputFileName;
272  outputFileName << filename << "mot_" << m_iDriveID << "_" << m_iCurrentObject << ".log";
273 
274  FILE* pFile;
275  //open FileStream
276  pFile = fopen(outputFileName.str().c_str(), "w");
277 
278  //Check if there was a problem
279  if( pFile == NULL )
280  {
281  std::cout << "Error while writing file: " << outputFileName.str() << " Maybe the selected folder does'nt exist." << std::endl;
282  }
283  else
284  {
285  // write all data from vector to file
286  for (unsigned int i = 0; i < vtValues[0].size(); i++)
287  fprintf(pFile, "%e %e\n", vtValues[0][i], vtValues[1][i]);
288  fclose(pFile);
289  }
290 
291  return true;
292 }
std::string m_sLogFilename
Definition: ElmoRecorder.h:89
bool isInitialized(bool initNow)
std::vector< unsigned char > data
Definition: SDOSegmented.h:96
float convertBinaryToFloat(unsigned int binaryRepresentation)
ElmoRecorder(CanDriveHarmonica *pParentHarmonicaDrive)
bool m_bIsInitialized
Definition: ElmoRecorder.h:106
float m_fRecordingStepSec
Definition: ElmoRecorder.h:87
int processData(segData &SDOData)
int m_iReadoutRecorderTry
Definition: ElmoRecorder.h:94
unsigned int numTotalBytes
Definition: SDOSegmented.h:91
int logToFile(std::string filename, std::vector< float > vtValues[])
int m_iCurrentObject
Definition: ElmoRecorder.h:85
int configureElmoRecorder(int iRecordingGap, int driveID, int startImmediately=1)
int readoutRecorderTryStatus(int iStatusReg, segData &SDOData)
void IntprtSetInt(int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData)
int setLogFilename(std::string sLogFileprefix)
int readoutRecorder(int iObjSubIndex)
void sendSDOUpload(int iObjIndex, int iObjSub)
int statusFlag
Definition: SDOSegmented.h:71
CanDriveHarmonica * m_pHarmonicaDrive
Definition: ElmoRecorder.h:96
float convertBinaryToHalfFloat(unsigned int iBinaryRepresentation)
int readoutRecorderTry(int iObjSubIndex)


cob_canopen_motor
Author(s): Christian Connette
autogenerated on Wed Apr 7 2021 02:11:53