schunk_libm5api: schunk_libm5api: CP5X11Device.cpp Source File

00001 
00002 /******************************************************************************
00003  * 
00004  * Copyright (c) 2012 
00005  * 
00006  * SCHUNK GmbH & Co. KG
00007  *  
00008  * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
00009  * 
00010  * Project name: Drivers for "Amtec M5 Protocol" Electronics V4
00011  *                                                                        
00012  * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
00013  * 
00014  * Email:robotics@schunk.com
00015  * 
00016  * ToDo: 
00017  * 
00018  * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 
00019  * 
00020  * Redistribution and use in source and binary forms, with or without 
00021  * modification, are permitted provided that the following conditions are met: 
00022  * 
00023  *  * Redistributions of source code must retain the above copyright 
00024  *    notice, this list of conditions and the following disclaimer. 
00025  *  * Redistributions in binary form must reproduce the above copyright 
00026  *    notice, this list of conditions and the following disclaimer in the 
00027  *    documentation and/or other materials provided with the distribution. 
00028  *  * Neither the name of SCHUNK GmbH & Co. KG nor the names of its 
00029  *    contributors may be used to endorse or promote products derived from 
00030  *    this software without specific prior written permission. 
00031  * 
00032  * This program is free software: you can redistribute it and/or modify 
00033  * it under the terms of the GNU Lesser General Public License LGPL as 
00034  * published by the Free Software Foundation, either version 3 of the 
00035  * License, or (at your option) any later version. 
00036  * 
00037  * This program is distributed in the hope that it will be useful, 
00038  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00039  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
00040  * GNU Lesser General Public License LGPL for more details. 
00041  * 
00042  * You should have received a copy of the GNU Lesser General Public 
00043  * License LGPL along with this program. 
00044  * If not, see <http://www.gnu.org/licenses/>.
00045  * 
00046  ******************************************************************************/
00047 
00048 
00049 #include "CP5X11Device.h"
00050 #define DPN_WIN
00051 #include "../include/dpn_user.h"
00052 // ========================================================================== ;
00053 //                                                                            ;
00054 // ---- private auxiliary functions ----------------------------------------- ;
00055 //                                                                            ;
00056 // ========================================================================== ;
00057 
00058 // ========================================================================== ;
00059 //                                                                            ;
00060 // ---- protected auxiliary functions --------------------------------------- ;
00061 //                                                                            ;
00062 // ========================================================================== ;
00063 int CCP5X11Device::getDeviceError(int iErrorState)
00064 {
00065         return iErrorState;
00066 }
00067 
00068 int CCP5X11Device::setBaudRate()
00069 {       
00070         m_iErrorState = 0;
00071         return m_iErrorState;
00072 }
00073 
00074 int CCP5X11Device::setMessageId(unsigned long uiMessageId)
00075 {
00076         m_iErrorState = 0;
00077         return m_iErrorState;
00078 }
00079 
00080 int CCP5X11Device::clearReadQueue()
00081 {       
00082         m_iErrorState = 0;
00083         return m_iErrorState;
00084 }
00085 
00086 int CCP5X11Device::reinit(unsigned char ucBaudRateId)
00087 {
00088         m_iErrorState = 0;
00089         return m_iErrorState;
00090 }
00091 
00092 int CCP5X11Device::readDevice(CProtocolMessage& rclProtocolMessage)
00093 {       
00094         unsigned short int iRetVal = 0;
00095         double fTime = 0;
00096         m_iErrorState = 0;
00097 
00098 /*      dpn_interface dpn;
00099         dpn.reference.board_select = m_hDevice;
00100         dpn.reference.access = m_access;
00101         dpn.stat_nr = rclProtocolMessage.m_iModuleId;
00102         dpn.length = 255;
00103         dpn_interface* pDpn = &dpn;
00104 */
00105         struct dpn_interface_m  dpnIn;  
00106         dpnIn.dpn_if_single[0].reference.access = m_access;
00107         dpnIn.dpn_if_single[0].reference.board_select = m_hDevice;
00108         dpnIn.dpn_if_single[0].stat_nr = rclProtocolMessage.m_iModuleId;
00109         for( int j = 1; j < DPN_MULTIPLE_SIZE; j++ )
00110                 dpnIn.dpn_if_single[j].stat_nr = DPN_IF_S_UNUSED;
00111         dpn_interface_m* pDpnIn = &dpnIn;
00112         
00113         m_clTimer.start();
00114         do
00115         {       
00116                 Sleep(1);
00117                 //iRetVal = dpn_in_slv( pDpn );
00118                 iRetVal = dpn_in_slv_m( &dpnIn );
00119 
00120 /*              for( int i = 0; i < 16; i++ )
00121                         printf( "%02x ", pDpnIn->dpn_if_single[0].user_data[i] );
00122                 printf( "\n" );
00123 */
00124                 m_clTimer.stop();
00125                 fTime = m_clTimer.executionTime() * 1000;
00126                 if(fTime > m_uiTimeOut)
00127                 {       
00128                         warning("CP5X11 readDevice timeout");
00129                         m_iErrorState = ERRID_DEV_READERROR;
00130                         return m_iErrorState;
00131                 }
00132 
00133                 if( iRetVal != DPN_NO_ERROR )
00134                 {       warning("CP5X11 dpn_in_slv failure: %x", iRetVal );
00135                         m_iErrorState = ERRID_DEV_READERROR;
00136                         return m_iErrorState;
00137                 }
00138 
00139 /*              if( rclProtocolMessage.m_ucMessageLength == 1 )
00140                         if( pDpn->user_data[0] == rclProtocolMessage.m_aucMessageData[0] )
00141                                 break;
00142                 if( rclProtocolMessage.m_ucMessageLength > 1 )
00143                         if( pDpn->user_data[0] == rclProtocolMessage.m_aucMessageData[0] && 
00144                                         pDpn->user_data[1] == rclProtocolMessage.m_aucMessageData[1] )
00145                                 break;
00146 */
00147                 if( rclProtocolMessage.m_ucMessageLength == 1 )
00148                         if( pDpnIn->dpn_if_single[0].user_data[0] == rclProtocolMessage.m_aucMessageData[0] )
00149                                 break;
00150                 if( rclProtocolMessage.m_ucMessageLength > 1 )
00151                         if( pDpnIn->dpn_if_single[0].user_data[0] == rclProtocolMessage.m_aucMessageData[0] && 
00152                                         pDpnIn->dpn_if_single[0].user_data[1] == rclProtocolMessage.m_aucMessageData[1] )
00153                                 break;
00154 /*                      
00155                 for( i = 0; i < 8; i++ )
00156                         printf( "%02x ", rclProtocolMessage.m_aucMessageData[i] );
00157                 printf( "\n\n" );
00158                 if( kbhit() )
00159                         break;
00160 */
00161         }       while( 1 );
00162 
00163 /*      if( pDpn->length != 16 )
00164         {       warning( "CP5X11: Slave does not have 16 input bytes" );
00165                 m_iErrorState = ERRID_DEV_READERROR;
00166                 return m_iErrorState;
00167         }
00168 
00169         if( pDpn->sys_state != DPN_SYS_OPERATE )
00170         {       warning( "CP5X11: Master not in in operate mode" );
00171                 m_iErrorState = ERRID_DEV_READERROR;
00172                 return m_iErrorState;
00173         }
00174 */
00175         rclProtocolMessage.m_uiMessageId = MSGID_ACK + rclProtocolMessage.m_iModuleId;
00176         memcpy( rclProtocolMessage.m_aucMessageData, pDpnIn->dpn_if_single[0].user_data, 8 );
00177         return m_iErrorState;
00178 }
00179 
00180 int CCP5X11Device::writeDevice(CProtocolMessage& rclProtocolMessage)
00181 {       
00182         unsigned short int iRetVal = 0;
00183         m_iErrorState = 0;
00184 
00185         if( rclProtocolMessage.m_ucMessageLength < 8 )
00186                 rclProtocolMessage.m_aucMessageData[7] = m_teaser++;
00187 
00188 /*      dpn_interface dpn;
00189         dpn.reference.board_select = m_hDevice;
00190         dpn.reference.access = m_access;
00191         dpn.stat_nr = rclProtocolMessage.m_iModuleId;
00192         dpn.length = 8;
00193         for( int i = 0; i < dpn.length; i++ )
00194                 dpn.user_data[i] = rclProtocolMessage.m_aucMessageData[i];
00195         dpn_interface* pDpn = &dpn;
00196 */
00197         struct dpn_interface_m  dpnOut;  
00198         dpnOut.dpn_if_single[0].reference.access = m_access;
00199         dpnOut.dpn_if_single[0].reference.board_select = m_hDevice;
00200         dpnOut.dpn_if_single[0].stat_nr = rclProtocolMessage.m_iModuleId;
00201         dpnOut.dpn_if_single[0].length = 8;
00202         for( int i = 0; i < 8; i++ )
00203                 dpnOut.dpn_if_single[0].user_data[i] = rclProtocolMessage.m_aucMessageData[i];
00204         for( int j = 1; j < DPN_MULTIPLE_SIZE; j++ )
00205                 dpnOut.dpn_if_single[j].stat_nr = DPN_IF_S_UNUSED;
00206         dpn_interface_m* pDpnOut = &dpnOut;
00207 
00208         iRetVal = dpn_out_slv_m( &dpnOut );
00209 
00210 /*      iRetVal = dpn_out_slv( pDpn );
00211         if( iRetVal != DPN_NO_ERROR )
00212         {       warning( "CP5X11 dpn_out_slv failure: %x", iRetVal );
00213                 m_iErrorState = ERRID_DEV_WRITEERROR;
00214         }
00215 
00216         if( pDpn->sys_state != DPN_SYS_OPERATE )
00217         {       warning( "CP5X11: Master not in in operate mode" );
00218                 m_iErrorState = ERRID_DEV_WRITEERROR;
00219         }
00220 */
00221         return m_iErrorState;
00222 }
00223 
00224 // ========================================================================== ;
00225 //                                                                            ;
00226 // ---- constructors / destructor ------------------------------------------- ;
00227 //                                                                            ;
00228 // ========================================================================== ;
00229 
00230 CCP5X11Device::CCP5X11Device() : m_hDevice(NULL), m_uiTimeOut(50), m_clTimer(util_REAL_TIME)
00231 {
00232         initMessage("CCP5X11Device", g_iDebugLevel, g_bDebug, g_bDebugFile);
00233 }
00234 
00235 CCP5X11Device::CCP5X11Device(const CCP5X11Device& rclCP5X11Device)
00236 {
00237         error(-1, "Sorry constructor is not implemented");
00238 }
00239 
00240 CCP5X11Device::~CCP5X11Device()
00241 {
00242 }
00243 
00244 // ========================================================================== ;
00245 //                                                                            ;
00246 // ---- operators ----------------------------------------------------------- ;
00247 //                                                                            ;
00248 // ========================================================================== ;
00249 
00250 CCP5X11Device& CCP5X11Device::operator=(const CCP5X11Device& rclCP5X11Device)
00251 {
00252         error(-1, "Sorry operator= is not implemented");
00253         return *this;
00254 }
00255 
00256 // ========================================================================== ;
00257 //                                                                            ;
00258 // ---- query functions ----------------------------------------------------- ;
00259 //                                                                            ;
00260 // ========================================================================== ;
00261 
00262 // ========================================================================== ;
00263 //                                                                            ;
00264 // ---- modify functions ---------------------------------------------------- ;
00265 //                                                                            ;
00266 // ========================================================================== ;
00267 
00268 void CCP5X11Device::setTimeOut(unsigned long uiTimeOut)
00269 {       
00270         m_uiTimeOut= uiTimeOut;
00271 }
00272 
00273 // ========================================================================== ;
00274 //                                                                            ;
00275 // ---- I/O functions ------------------------------------------------------- ;
00276 //                                                                            ;
00277 // ========================================================================== ;
00278 
00279 // ========================================================================== ;
00280 //                                                                            ;
00281 // ---- exec functions ------------------------------------------------------ ;
00282 //                                                                            ;
00283 // ========================================================================== ;
00284 
00285 int CCP5X11Device::init()
00286 {
00287         return init(m_acInitString);
00288 }
00289 
00290 int CCP5X11Device::init(const char* acInitString)
00291 {
00292         InitializeCriticalSection(&m_csDevice);
00293 
00294         unsigned short int iRetVal = 0;
00295         char* pcToken;
00296         char acString[128];
00297         m_bInitFlag = false;
00298         m_hDevice = NULL;
00299         m_iErrorState = 0;
00300         dpn_interface dpn;
00301         dpn_interface dpnReinit;
00302 
00303         strncpy( m_acInitString, acInitString, 128 );
00304         strncpy( acString, acInitString, 128 );
00305 
00306         pcToken = strtok( acString, ":" );
00307         if( !pcToken )
00308         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00309                 return m_iErrorState;
00310         }
00311         if( strcmp( pcToken, "CP5X11" ) != 0 )
00312         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00313                 return m_iErrorState;
00314         }
00315 
00316         pcToken = strtok( NULL, "," );
00317         if( !pcToken )
00318         {       m_iErrorState = ERRID_DEV_BADINITSTRING;
00319                 return m_iErrorState;
00320         }
00321         m_hDevice = atoi( pcToken );
00322         
00323         try
00324         {
00325                 // First init to retrieve infromation what slaves are configured
00326                 dpn.reference.board_select = m_hDevice;
00327                 dpn.reference.access = DPN_SYS_NOT_CENTRAL | DPN_ROLE_NOT_CENTRAL;
00328                 dpn.length = 126;
00329                 for( int i = 0; i <     126; i++ )
00330                         dpn.user_data[i] = DPN_SLV_NO_ACCESS; //DPN_SLV_WRITE_READ; 
00331                 dpn_interface* pDpn = &(dpn);
00332 
00333                 iRetVal = dpn_init( pDpn );
00334                 if( iRetVal != DPN_NO_ERROR )
00335                 {       warning( "CP5X11 dpn_init failed: %x", iRetVal );
00336                         m_iErrorState = ERRID_DEV_INITERROR;
00337                         return m_iErrorState;
00338                 }
00339                 m_hDevice = pDpn->reference.board_select;
00340                 m_access = pDpn->reference.access;
00341 
00342                 // Get board config
00343                 dpn.reference.board_select = m_hDevice;
00344                 dpn.length = 126;
00345                 pDpn = &(dpn);
00346                 iRetVal = dpn_read_cfg( pDpn );
00347                 if( iRetVal != DPN_NO_ERROR )
00348                 {       warning( "CP5X11 dpn_init failed: %x", iRetVal );
00349                         m_iErrorState = ERRID_DEV_INITERROR;
00350                         return m_iErrorState;
00351                 }
00352 
00353                 // save configuration
00354                 for( i = 0; i < pDpn->length; i++ )
00355                 {       if( pDpn->user_data[i] == DPN_CFG_NORM )
00356                                 dpnReinit.user_data[i] = DPN_SLV_WRITE_READ;
00357                         else
00358                                 dpnReinit.user_data[i] = DPN_SLV_NO_ACCESS; 
00359                 }
00360                 dpn.reference.board_select = m_hDevice;
00361                 dpn.reference.access = m_access;
00362                 pDpn = &(dpn);
00363 
00364                 // reset to close the board
00365                 iRetVal = dpn_reset( pDpn );
00366                 if( iRetVal != DPN_NO_ERROR )
00367                 {       warning( "dpn_reset failed with %x", iRetVal );
00368                         m_iErrorState = ERRID_DEV_EXITERROR;
00369                 }
00370                 
00371                 // Reinit after knowing what slaves are configured...
00372                 dpnReinit.reference.board_select = m_hDevice;
00373                 dpnReinit.reference.access = DPN_SYS_NOT_CENTRAL | DPN_ROLE_NOT_CENTRAL;
00374                 dpnReinit.length = 126;
00375                 pDpn = &(dpnReinit);
00376 
00377                 iRetVal = dpn_init( pDpn );
00378                 if( iRetVal != DPN_NO_ERROR )
00379                 {       warning( "CP5X11 dpn_init failed: %x", iRetVal );
00380                         m_iErrorState = ERRID_DEV_INITERROR;
00381                         return m_iErrorState;
00382                 }
00383 
00384                 m_hDevice = pDpn->reference.board_select;
00385                 m_access = pDpn->reference.access;
00386         }
00387         catch(...)
00388         {
00389                         warning("init CP5X11 device failed no library found");
00390                         m_iErrorState = ERRID_DEV_NOLIBRARY;
00391                         return m_iErrorState;
00392         }
00393 
00394         m_iErrorState = clearReadQueue();
00395         if(m_iErrorState != 0)
00396                 return m_iErrorState;
00397 
00398         if(m_iErrorState == 0)
00399                 m_bInitFlag = true;
00400         updateModuleIdMap();
00401         return m_iErrorState;
00402 }
00403 
00404 int CCP5X11Device::exit()
00405 {       
00406         unsigned short int iRetVal = 0;
00407         m_iErrorState = 0;
00408         dpn_interface dpn;
00409 
00410         if(!m_bInitFlag)
00411         {       
00412                 warning("device not initialzed");
00413                 m_iErrorState = ERRID_DEV_NOTINITIALIZED;
00414         }
00415         EnterCriticalSection(&m_csDevice);
00416         dpn.reference.board_select = m_hDevice;
00417         dpn.reference.access = m_access;
00418         dpn_interface* pDpn = &(dpn);
00419 
00420         iRetVal = dpn_reset( pDpn );
00421         if( iRetVal != DPN_NO_ERROR )
00422         {       
00423                 warning( "Could not exit CP5X11" );
00424                 m_iErrorState = ERRID_DEV_EXITERROR;
00425         }
00426 
00427         m_hDevice = NULL;
00428         m_bInitFlag = false;
00429         LeaveCriticalSection(&m_csDevice);
00430         DeleteCriticalSection(&m_csDevice);
00431         return m_iErrorState;
00432 }