ElmoRecorder.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob3_common
00012  * ROS package name: canopen_motor
00013  * Description:
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Philipp Köhler
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Mar 2010
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *       * Redistributions of source code must retain the above copyright
00029  *         notice, this list of conditions and the following disclaimer.
00030  *       * Redistributions in binary form must reproduce the above copyright
00031  *         notice, this list of conditions and the following disclaimer in the
00032  *         documentation and/or other materials provided with the distribution.
00033  *       * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *         Engineering and Automation (IPA) nor the names of its
00035  *         contributors may be used to endorse or promote products derived from
00036  *         this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 #include <math.h>
00055 #include <vector>
00056 #include <stdio.h>
00057 #include <sstream>
00058 #include <cob_canopen_motor/ElmoRecorder.h>
00059 #include <cob_canopen_motor/CanDriveHarmonica.h>
00060 
00061 ElmoRecorder::ElmoRecorder(CanDriveHarmonica * pParentHarmonicaDrive) {
00062         m_pHarmonicaDrive = pParentHarmonicaDrive;
00063         
00064         m_bIsInitialized = false;
00065         m_iReadoutRecorderTry = 0;
00066 }
00067 
00068 ElmoRecorder::~ElmoRecorder() {
00069 }
00070 
00071 bool ElmoRecorder::isInitialized(bool initNow) {
00072         if(initNow) m_bIsInitialized = true;
00073         return m_bIsInitialized;
00074 }
00075 
00076 int ElmoRecorder::configureElmoRecorder(int iRecordingGap, int driveID, int startImmediately){ //iRecordingGap = N indicates that a new sample should be taken once per N time quanta
00077         m_iDriveID = driveID;
00078         
00079         if(startImmediately >=2 ) startImmediately = 1;
00080 
00081         m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'R', 0, 0);     // Stop Recorder if it's active 
00082         // Record Main speed (index 0, ID1) 
00083         // Active Current (index 9, ID10)
00084         // Main Position (index 1, ID2)
00085         // Speed Command (index 15, ID16)
00086         // RC = 2^(Signal1Index) + 2^(Signal2Index) + ..; e.g.: 2^0 + 2^1 + 2^9 + 2^15 = 33283;
00087         m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'C', 0, 33283);
00088         // Set trigger type to immediate
00089         m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'P', 3, 0);
00090         // Set Recording Gap
00091         m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'G', 0, iRecordingGap);
00092         // Set Recording Length
00093         // RL = (4096 / Number of Signals)
00094         m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'L', 0, 1024);
00095         
00096         // Set Time Quantum, Default: RP=0 -> TS * 4; TS is 90us by default
00097         // m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'P', 0, 0);
00098         // ----> Total Recording Time = 90us * 4 * RG * RL
00099 
00100         m_pHarmonicaDrive->IntprtSetInt(8, 'R', 'R', 0, startImmediately + 1); //2 launches immediately (8, 'R', 'R', 0, 1) launches at next BG
00101         
00102         m_fRecordingStepSec = 0.000090 * 4 * iRecordingGap;
00103         
00104         return 0;
00105 }
00106 
00107 int ElmoRecorder::readoutRecorderTry(int iObjSubIndex) {
00108         //Request the SR (status register) and begin all the read-out process with this action.
00109         //SDOData.statusFlag is segData::SDO_SEG_WAITING;
00110         
00111         m_iReadoutRecorderTry = 1;
00112         m_iCurrentObject = iObjSubIndex;
00113         
00114         m_pHarmonicaDrive->requestStatus();
00115         
00116         return 0;
00117 }
00118 
00119 int ElmoRecorder::readoutRecorderTryStatus(int iStatusReg, segData& SDOData) {
00120         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)
00121 
00122         m_iReadoutRecorderTry = 0;
00123         
00124         //Bits 16-17 of status register contain recorder information
00125         int iRecorderStatus = (0x30000 & iStatusReg) >> 16;
00126 
00127         if(iRecorderStatus == 0) {
00128                 std::cout << "Recorder " << m_iDriveID << " inactive with no valid data to upload" << std::endl;
00129                 SDOData.statusFlag = segData::SDO_SEG_FREE;
00130         } else if(iRecorderStatus == 1) {
00131                 std::cout << "Recorder " << m_iDriveID << " waiting for a trigger event" << std::endl;
00132                 SDOData.statusFlag = segData::SDO_SEG_FREE;
00133         } else if(iRecorderStatus == 2) {
00134                 std::cout << "Recorder " << m_iDriveID << " finished, valid data ready for use" << std::endl;
00135                 readoutRecorder(m_iCurrentObject);
00136                 //already set to SDOData.statusFlag = segData::SDO_SEG_WAITING;
00137         } else if(iRecorderStatus == 3) {
00138                 std::cout << "Recorder " << m_iDriveID << " is still recording" << std::endl;
00139                 SDOData.statusFlag = segData::SDO_SEG_FREE;
00140         }
00141         
00142         return 0;
00143 }
00144 
00145 int ElmoRecorder::readoutRecorder(int iObjSubIndex){
00146         //initialize Upload of Recorded Data (object 0x2030)
00147         int iObjIndex = 0x2030;
00148 
00149         m_pHarmonicaDrive->sendSDOUpload(iObjIndex, iObjSubIndex);
00150         m_iCurrentObject = iObjSubIndex;
00151         
00152         return 0;
00153 }
00154 
00155 int ElmoRecorder::processData(segData& SDOData) {
00156         int iItemSize = 4;
00157         int iItemCount = 0;
00158         unsigned int iNumDataItems = 0;
00159         bool bCollectFloats = true;
00160         float fFloatingPointFactor = 0;
00161         
00162         std::vector<float> vfResData[2];
00163         
00164         //see SimplIQ CANopen DS 301 Implementation Guide, object 0x2030
00165         
00166         //HEADER
00167         //--------------------------------------
00168         //First 7 Bytes of the data sequence contain header information:
00169         //Byte 0: First four bits: 4 = Long Int data type, 1 = Half Float data type, 5 = Double Float
00170         //                      Next four bits: Recording frequency in 1 per n * TS => deltaT =  n * 90µsec
00171         //Byte 2, Byte 3: Number of recorded data points
00172         //Byte 3 to 6: Floating point factor for data to be multiplied with
00173         //
00174         //Byte 7 to Byte (7+ iNumdataItems * 4) contain data
00175         
00176         //B[0]: Time quantum and data type
00177         switch ((SDOData.data[0] >> 4) ) {
00178                 case 4:
00179                         bCollectFloats = false;
00180                         iItemSize = 4;
00181                         break;
00182                 case 5:
00183                         bCollectFloats = true;
00184                         iItemSize = 4;
00185                         break;
00186                 case 1:
00187                         bCollectFloats = true;
00188                         iItemSize = 2;
00189                         break;
00190                 default:
00191                         bCollectFloats = false;
00192                         iItemSize = 4;
00193                         break;
00194         }
00195         std::cout << ">>>>>ElmoRec: HEADER INFOS<<<<<\nData type is: " << (SDOData.data[0] >> 4) << std::endl;
00196         
00197         //std::cout << "fTimeQuantum from Header is " << fTimeQuantum << " m_fRecordingStepSec is " << m_fRecordingStepSec << std::endl;
00198         
00199         
00200         //B[1]..[2] //Number of recorded items
00201         iNumDataItems = (SDOData.data[2] << 8 | SDOData.data[1]);
00202         //std::cout << "Number of recorded data points: " << iNumDataItems << std::endl;
00203 
00204         //B[3] ... [6] //Floating point factor
00205         fFloatingPointFactor = convertBinaryToFloat( (SDOData.data[6] << 24) | (SDOData.data[5] << 16) | (SDOData.data[4] << 8) | (SDOData.data[3]) );
00206         std::cout << "Floating point factor for recorded values is: " << fFloatingPointFactor << std::endl;
00207         
00208         
00209         if( ((SDOData.numTotalBytes-7)/iItemSize) != iNumDataItems) 
00210                 std::cout << "SDODataSize announced in SDO-Header" << ((SDOData.numTotalBytes-7)/iItemSize) << " differs from NumDataItems by ElmoData-Header" <<  iNumDataItems << std::endl;
00211         //END HEADER
00212         //--------------------------------------
00213 
00214         vfResData[0].assign(iNumDataItems, 0.0);
00215         vfResData[1].assign(iNumDataItems, 0.0);
00216         iItemCount = 0;
00217         
00218         //extract values from data stream, consider Little Endian conversion for every single object!
00219         for(unsigned int i=7;i<=SDOData.data.size() - iItemSize; i=i+iItemSize) {
00220                 if(bCollectFloats) {
00221                         if(iItemSize == 4)
00222                                 vfResData[1][iItemCount] = fFloatingPointFactor * convertBinaryToFloat( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) );
00223                                 
00224                                 //DEBUG
00225                                 if(iItemCount == 120)
00226                                         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;
00227                                 
00228                         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) );
00229                         iItemCount ++;
00230                 } else {
00231                         vfResData[1][iItemCount] = fFloatingPointFactor * (float)( (SDOData.data[i] << 0) | (SDOData.data[i+1] << 8) | (SDOData.data[i+2] << 16) | (SDOData.data[i+3] << 24) );
00232                         iItemCount ++;
00233                 }
00234                 
00235                 vfResData[0][iItemCount] = m_fRecordingStepSec * iItemCount;
00236         }
00237         
00238         logToFile(m_sLogFilename, vfResData);
00239 
00240         SDOData.statusFlag = segData::SDO_SEG_FREE;
00241         return 0;
00242 }
00243 
00244 int ElmoRecorder::setLogFilename(std::string sLogFileprefix) {
00245         m_sLogFilename = sLogFileprefix;
00246         return 0;
00247 }
00248 
00249 
00250 
00251 float ElmoRecorder::convertBinaryToFloat(unsigned int iBinaryRepresentation) {
00252         //Converting binary-numbers to 32bit float values according to IEEE 754 see http://de.wikipedia.org/wiki/IEEE_754
00253         int iSign;
00254         int iExponent;
00255         unsigned int iMantissa;
00256         float iNumMantissa = 0.0f;
00257 
00258         if((iBinaryRepresentation & (1 << 31)) == 0) //first bit is sign bit: 0 = +, 1 = -
00259                 iSign = 1;
00260         else
00261                 iSign = -1;
00262 
00263         iExponent = ((iBinaryRepresentation >> 23) & 0xFF) - 127; //take away Bias(127) for positive and negative exponents
00264 
00265         iMantissa = (iBinaryRepresentation & 0x7FFFFF); //only keep mantissa part of binary number
00266 
00267         iNumMantissa = 1.0f;
00268         
00269         for(int i=1; i<=23; i++) { //calculate decimal places (convert binary mantissa to decimal number
00270                 if((iMantissa & (1 << (23-i))) > 0) {
00271                         iNumMantissa = iNumMantissa + pow(2,(-1)*i);
00272                 }
00273         }
00274 
00275         return iSign * pow(2,iExponent) * iNumMantissa;
00276 }
00277 
00278 float ElmoRecorder::convertBinaryToHalfFloat(unsigned int iBinaryRepresentation) {
00279         //Converting binary-numbers to 16bit float values according to IEEE 754 see http://de.wikipedia.org/wiki/IEEE_754
00280         int iSign;
00281         int iExponent;
00282         unsigned int iMantissa;
00283         float iNumMantissa = 0.0f;
00284 
00285         if((iBinaryRepresentation & (1 << 15)) == 0) //first bit is sign bit: 0 = +, 1 = -
00286                 iSign = 1;
00287         else
00288                 iSign = -1;
00289 
00290         iExponent = ((iBinaryRepresentation >> 10) & 0x1F) - 15; //take away Bias(15) for positive and negative exponents
00291 
00292         iMantissa = (iBinaryRepresentation & 0x3FF); //only keep mantissa part of binary number
00293 
00294         iNumMantissa = 1.0f;
00295         
00296         for(int i=1; i<=10; i++) { //calculate decimal places (convert binary mantissa to decimal number
00297                 if((iMantissa & (1 << (10-i))) > 0) {
00298                         iNumMantissa = iNumMantissa + pow(2,(-1)*i);
00299                 }
00300         }
00301 
00302         return iSign * pow(2,iExponent) * iNumMantissa;
00303 }
00304 
00305 // Function for writing Logfile
00306 int ElmoRecorder::logToFile(std::string filename, std::vector<float> vtValues[]) {
00307         std::stringstream outputFileName;
00308         outputFileName << filename << "mot_" << m_iDriveID << "_" << m_iCurrentObject << ".log";
00309 
00310         FILE* pFile;
00311         //open FileStream
00312         pFile = fopen(outputFileName.str().c_str(), "w");
00313         
00314         //Check if there was a problem
00315         if( pFile == NULL ) 
00316         {       
00317                 std::cout << "Error while writing file: " << outputFileName.str() << " Maybe the selected folder does'nt exist." << std::endl;
00318         } 
00319         else 
00320         {
00321                 // write all data from vector to file
00322                 for (unsigned int i = 0; i < vtValues[0].size(); i++)
00323                         fprintf(pFile, "%e %e\n", vtValues[0][i], vtValues[1][i]);
00324                 fclose(pFile);
00325         }
00326         
00327         return true;
00328 }


cob_canopen_motor
Author(s): Christian Connette
autogenerated on Sun Oct 5 2014 23:02:12