SerRelayBoard.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: cob_driver
00012  * ROS package name: cob_relayboard
00013  * Description: Class for communication with relayboard. The relayboard is mainly used for reading the Emergencystop and Laserscannerstop states.
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Philipp Koehler
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: March 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 
00055 #include <math.h>
00056 #include <cob_relayboard/SerRelayBoard.h>
00057 #include <iostream>
00058 
00059 //-----------------------------------------------
00060 
00061 
00062 #define NUM_BYTE_SEND 79 //Total amount of data sent to relayboard in one message, is now passed and set as protocol-version argument in constructor
00063 
00064 #define RS422_BAUDRATE 420000
00065 #define RS422_RX_BUFFERSIZE 1024
00066 #define RS422_TX_BUFFERSIZE 1024
00067 
00068 #define RS422_TIMEOUT 0.025
00069 
00070 #define NUM_BYTE_REC_MAX 120
00071 #define NUM_BYTE_REC_HEADER 4 //Header bytes, which are used to identify the beginning of a new received message {0x02, 0x80, 0xD6, 0x02}
00072 #define NUM_BYTE_REC_CHECKSUM 2 //checksum for message, that is built as the sum of all data bytes contained in the message
00073 #define NUM_BYTE_REC 104 //Total amount of data bytes in a received message (from the relayboard)
00074 
00075 #define NUM_BYTE_SEND_RELAYBOARD_14 88
00076 #define NUM_BYTE_REC_RELAYBOARD_14 124
00077 
00078 
00079 //-----------------------------------------------
00080 SerRelayBoard::SerRelayBoard(std::string ComPort, int ProtocolVersion)
00081 {
00082         m_iProtocolVersion = ProtocolVersion;
00083         if(m_iProtocolVersion == 1)
00084                 m_NUM_BYTE_SEND = 50;
00085         else if(m_iProtocolVersion == 2)
00086         {       m_NUM_BYTE_SEND = 79;
00087                 m_iTypeLCD = LCD_60CHAR_TEXT;
00088         }       
00089         else if(m_iProtocolVersion == 3)
00090         {
00091                 m_NUM_BYTE_SEND = NUM_BYTE_SEND_RELAYBOARD_14;
00092                 m_iTypeLCD = RELAY_BOARD_1_4;
00093         }
00094         m_bComInit = false;
00095         m_sNumComPort = ComPort;
00096 
00097         m_iRelBoardBattVoltage = 0;
00098         m_iConfigRelayBoard = 0;
00099         m_iRelBoardKeyPad = 0xFFFF;
00100         m_iCmdRelayBoard = 0;
00101         m_iDigIn = 0;
00102         m_cSoftEMStop = 0;
00103 
00104 }
00105 
00106 //-----------------------------------------------
00107 SerRelayBoard::~SerRelayBoard()
00108 {
00109         m_SerIO.closeIO();
00110 }
00111 
00112 //-----------------------------------------------
00113 int SerRelayBoard::evalRxBuffer()
00114 {
00115         static int siNoMsgCnt = 0;
00116 
00117         int iNumByteRec = NUM_BYTE_REC;
00118         if(m_iTypeLCD == RELAY_BOARD_1_4)
00119         {
00120                 iNumByteRec = NUM_BYTE_REC_RELAYBOARD_14;
00121         }
00122         
00123         const int c_iNrBytesMin = NUM_BYTE_REC_HEADER + iNumByteRec + NUM_BYTE_REC_CHECKSUM;
00124         const int c_iSizeBuffer = 4096;
00125 
00126         int i;
00127         int errorFlag = NO_ERROR;
00128         int iNrBytesInQueue, iNrBytesRead, iDataStart;
00129         unsigned char cDat[c_iSizeBuffer];
00130         unsigned char cTest[4] = {0x02, 0x80, 0xD6, 0x02};
00131 
00132         if( !m_bComInit ) return NOT_INITIALIZED;
00133 
00134         //enough data in queue?
00135         iNrBytesInQueue = m_SerIO.getSizeRXQueue();
00136         if(iNrBytesInQueue < c_iNrBytesMin)
00137         {
00138                 //there are too less bytes in queue
00139                 siNoMsgCnt++;
00140                 if(siNoMsgCnt > 29)
00141                 {
00142                         //std::cerr << "Relayboard: " << siNoMsgCnt << " cycles no msg received";
00143                         siNoMsgCnt = 0;
00144                         errorFlag = NO_MESSAGES;
00145                 } else errorFlag = TOO_LESS_BYTES_IN_QUEUE;
00146                 
00147                 return errorFlag;
00148         }
00149         else
00150         {
00151                 siNoMsgCnt = 0;
00152         }
00153 
00154         // search most recent data from back of queue
00155         iNrBytesRead = m_SerIO.readBlocking((char*)&cDat[0], iNrBytesInQueue);
00156         for(i = (iNrBytesRead - c_iNrBytesMin); i >= 0 ; i--)
00157         {
00158                 //try to find start bytes
00159                 if((cDat[i] == cTest[0]) && (cDat[i+1] == cTest[1]) && (cDat[i+2] == cTest[2]) && (cDat[i+3] == cTest[3]))
00160                 {
00161                         iDataStart = i + 4;
00162 
00163                         // checksum ok?
00164                         if( convRecMsgToData(&cDat[iDataStart]) )
00165                         {
00166                                 return errorFlag;
00167                         }
00168                         else
00169                         {
00170                                 //std::cerr << "Relayboard: checksum error";
00171                                 errorFlag = CHECKSUM_ERROR;
00172                                 return errorFlag;
00173                         }
00174                 }
00175         }
00176 
00177         return errorFlag;
00178 }
00179 
00180 //-----------------------------------------------
00181 bool SerRelayBoard::init()
00182 {
00183         m_SerIO.setBaudRate(RS422_BAUDRATE);
00184         m_SerIO.setDeviceName( m_sNumComPort.c_str() );
00185         m_SerIO.setBufferSize(RS422_RX_BUFFERSIZE, RS422_TX_BUFFERSIZE);
00186         m_SerIO.setTimeout(RS422_TIMEOUT);
00187 
00188         m_SerIO.openIO();
00189 
00190         m_bComInit = true;
00191 
00192         return true;
00193 }
00194 
00195 //-----------------------------------------------
00196 bool SerRelayBoard::reset()
00197 {
00198         m_SerIO.closeIO();
00199         m_bComInit = false;
00200 
00201         init();
00202 
00203         return true;
00204 }
00205 
00206 //-----------------------------------------------
00207 bool SerRelayBoard::shutdown()
00208 {
00209         m_SerIO.closeIO();
00210 
00211         m_bComInit = false;
00212         
00213         return true;
00214 }
00215 
00216 
00217 //-----------------------------------------------
00218 bool SerRelayBoard::isEMStop()
00219 {
00220         if( (m_iRelBoardStatus & 0x0001) != 0)
00221         {
00222                 return true;
00223         }
00224         else
00225         {
00226                 return false;
00227         }
00228 }
00229 
00230 //-----------------------------------------------
00231 bool SerRelayBoard::isScannerStop()
00232 {
00233         if( (m_iRelBoardStatus & 0x0002) != 0)
00234         {
00235                 return true;
00236         }
00237         else
00238         {
00239                 return false;
00240         }
00241 }
00242 
00243 //-----------------------------------------------
00244 int SerRelayBoard::sendRequest() {
00245         int errorFlag = NO_ERROR;
00246         int iNrBytesWritten;
00247 
00248         unsigned char cMsg[m_NUM_BYTE_SEND];
00249         
00250         m_Mutex.lock();
00251         
00252                 convDataToSendMsg(cMsg);
00253 
00254                 m_SerIO.purgeTx();
00255 
00256                 iNrBytesWritten = m_SerIO.writeIO((char*)cMsg, m_NUM_BYTE_SEND);
00257         
00258                 if(iNrBytesWritten < m_NUM_BYTE_SEND) {
00259                         //std::cerr << "Error in sending message to Relayboard over SerialIO, lost bytes during writing" << std::endl;
00260                         errorFlag = GENERAL_SENDING_ERROR;
00261                 }
00262 
00263         m_Mutex.unlock();
00264 
00265         return errorFlag;
00266 }
00267 
00268 int SerRelayBoard::setDigOut(int iChannel, bool bOn)
00269 {
00270         switch( iChannel)
00271         {
00272         case 0:
00273 
00274                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_CHARGE_RELAY; }
00275                 else { m_iCmdRelayBoard &= ~CMD_SET_CHARGE_RELAY; }
00276                 
00277                 break;
00278 
00279         case 1:
00280 
00281                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY1; }
00282                 else { m_iCmdRelayBoard &= ~CMD_SET_RELAY1; }
00283 
00284                 break;
00285 
00286         case 2:
00287 
00288                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY2; }
00289                 else { m_iCmdRelayBoard &= ~CMD_SET_RELAY2; }
00290 
00291                 break;
00292 
00293         case 3:
00294 
00295                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY3; }
00296                 else { m_iCmdRelayBoard &= ~CMD_SET_RELAY3; }
00297 
00298                 break;
00299 
00300         case 4:
00301 
00302                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY4; }
00303                 else { m_iCmdRelayBoard &= ~CMD_SET_RELAY4; }
00304 
00305                 break;
00306 
00307         case 5:
00308 
00309                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY5; }
00310                 else { m_iCmdRelayBoard &= ~CMD_SET_RELAY5; }
00311 
00312                 break;
00313 
00314         case 6:
00315 
00316                 if(bOn) { m_iCmdRelayBoard |= CMD_SET_RELAY6; }
00317                 else { m_iCmdRelayBoard &= ~CMD_SET_RELAY6; }
00318 
00319                 break;
00320 
00321         default:
00322 
00323                 return -1;
00324         }
00325         
00326         return 0;
00327 }
00328 //-----------------------------------------------
00329 int SerRelayBoard::getAnalogIn(int* piAnalogIn)
00330 {
00331         piAnalogIn[0] = m_iChargeCurrent;
00332         piAnalogIn[1] = m_iRelBoardBattVoltage;
00333         piAnalogIn[2] = m_iRelBoardTempSensor;
00334         piAnalogIn[3] = m_iRelBoardKeyPad;
00335         piAnalogIn[4] = m_iRelBoardAnalogIn[0];
00336         piAnalogIn[5] = m_iRelBoardAnalogIn[1];
00337         piAnalogIn[6] = m_iRelBoardAnalogIn[2];
00338         piAnalogIn[7] = m_iRelBoardAnalogIn[3];
00339 
00340         return 0;
00341 }
00342 
00343 //-----------------------------------------------
00344 int SerRelayBoard::getDigIn()
00345 {
00346         return m_iDigIn;
00347 }
00348 
00349 void SerRelayBoard::convDataToSendMsg(unsigned char cMsg[])
00350 {
00351         int i;
00352         static int j = 0;
00353         int iCnt = 0;
00354         int iChkSum = 0;
00355         
00356         if (m_cSoftEMStop & 0x02)
00357         {
00358                 if (j == 1)
00359                 {
00360                         m_cSoftEMStop &= 0xFD;
00361                         j = 0;
00362                 }
00363                 else if (j == 0)
00364                 {
00365                         j = 1;
00366                 }
00367         }
00368 
00369         cMsg[iCnt++] = CMD_RELAISBOARD_GET_DATA;
00370 
00371         cMsg[iCnt++] = m_iConfigRelayBoard >> 8;
00372         cMsg[iCnt++] = m_iConfigRelayBoard;
00373 
00374         cMsg[iCnt++] = m_iCmdRelayBoard >> 8;
00375         cMsg[iCnt++] = m_iCmdRelayBoard;
00376 
00377         cMsg[iCnt++] = m_iIOBoardDigOut >> 8;
00378         cMsg[iCnt++] = m_iIOBoardDigOut;
00379 
00380         cMsg[iCnt++] = m_iVelCmdMotRightEncS >> 24;
00381         cMsg[iCnt++] = m_iVelCmdMotRightEncS >> 16;
00382         cMsg[iCnt++] = m_iVelCmdMotRightEncS >> 8;
00383         cMsg[iCnt++] = m_iVelCmdMotRightEncS;
00384 
00385         cMsg[iCnt++] = m_iVelCmdMotLeftEncS >> 24;
00386         cMsg[iCnt++] = m_iVelCmdMotLeftEncS >> 16;
00387         cMsg[iCnt++] = m_iVelCmdMotLeftEncS >> 8;
00388         cMsg[iCnt++] = m_iVelCmdMotLeftEncS;
00389 
00390         if(m_iTypeLCD == RELAY_BOARD_1_4)
00391         {
00392                 cMsg[iCnt++] = m_iVelCmdMotRearRightEncS >> 24;
00393                 cMsg[iCnt++] = m_iVelCmdMotRearRightEncS >> 16;
00394                 cMsg[iCnt++] = m_iVelCmdMotRearRightEncS >> 8;
00395                 cMsg[iCnt++] = m_iVelCmdMotRearRightEncS;
00396 
00397                 cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS >> 24;
00398                 cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS >> 16;
00399                 cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS >> 8;
00400                 cMsg[iCnt++] = m_iVelCmdMotRearLeftEncS;
00401         }
00402 
00403         cMsg[iCnt++] = m_iUSBoardSensorActive >> 8;
00404         cMsg[iCnt++] = m_iUSBoardSensorActive;
00405 
00406         if(m_iTypeLCD == LCD_20CHAR_TEXT)
00407         {
00408                 for(i = 0; i < 20; i++)
00409                 {
00410                         cMsg[iCnt++] = m_cTextDisplay[i];
00411                 }
00412 
00413                 // fill remaining msg with 0's
00414                 do
00415                 {
00416                         cMsg[iCnt++] = 0;
00417                 }
00418                 while(iCnt < (m_NUM_BYTE_SEND - 2));
00419         }
00420         else
00421         {
00422                 for(i = 0; i < 60; i++)
00423                 {
00424                         cMsg[iCnt++] = m_cTextDisplay[i];
00425                 }
00426         }
00427         
00428         if(m_iTypeLCD == RELAY_BOARD_1_4)
00429         {
00430                 cMsg[iCnt++] = m_cSoftEMStop;
00431         }
00432         // calc checksum
00433         for(i = 0; i < (m_NUM_BYTE_SEND - 2); i++)
00434         {
00435                 iChkSum %= 0xFF00;
00436                 iChkSum += cMsg[i];
00437         }
00438                 
00439         cMsg[m_NUM_BYTE_SEND - 2] = iChkSum >> 8;
00440         cMsg[m_NUM_BYTE_SEND - 1] = iChkSum;
00441         
00442         // reset flags
00443         m_iCmdRelayBoard &= ~CMD_RESET_POS_CNT;
00444 
00445 }
00446 
00447 
00448 //-----------------------------------------------
00449 /*void SerRelayBoard::convDataToSendMsg(unsigned char cMsg[])
00450 {
00451         int i;
00452         int iCnt = 0;
00453         int iChkSum = 0;
00454 
00455         cMsg[iCnt++] = 1;//CMD_RELAISBOARD_GET_DATA;
00456 
00457         cMsg[iCnt++] = m_iConfigRelayBoard >> 8;
00458         cMsg[iCnt++] = m_iConfigRelayBoard;
00459 
00460         cMsg[iCnt++] = m_iCmdRelayBoard >> 8;
00461         cMsg[iCnt++] = m_iCmdRelayBoard;
00462         
00463         // fill remaining msg with 0's
00464         do
00465         {
00466                 cMsg[iCnt++] = 0;
00467         }
00468         while(iCnt < (m_NUM_BYTE_SEND - 2));
00469 
00470         // calc checksum: summation of all bytes in the message
00471         for(i = 0; i < (m_NUM_BYTE_SEND - 2); i++)
00472         {
00473                 iChkSum += cMsg[i];
00474         }
00475 
00476         cMsg[m_NUM_BYTE_SEND - 2] = iChkSum >> 8;
00477         cMsg[m_NUM_BYTE_SEND - 1] = iChkSum;
00478 
00479         // reset flags
00480         m_iCmdRelayBoard &= ~CMD_RESET_POS_CNT;
00481 }
00482 */
00483 //-----------------------------------------------
00484 bool SerRelayBoard::convRecMsgToData(unsigned char cMsg[])
00485 {
00486 
00487         int iNumByteRec = NUM_BYTE_REC;
00488         if(m_iTypeLCD == LCD_20CHAR_TEXT)
00489         {
00490                 iNumByteRec = NUM_BYTE_REC;
00491         }
00492         if(m_iTypeLCD == LCD_60CHAR_TEXT)
00493         {
00494                 iNumByteRec = NUM_BYTE_REC;
00495         }
00496         if(m_iTypeLCD == RELAY_BOARD_1_4)
00497         {
00498                 iNumByteRec = NUM_BYTE_REC_RELAYBOARD_14;
00499         }
00500         
00501         const int c_iStartCheckSum = iNumByteRec;
00502         
00503         int i;
00504         unsigned int iTxCheckSum;
00505         unsigned int iCheckSum;
00506 
00507         m_Mutex.lock();
00508 
00509         // test checksum: checksum should be sum of all bytes
00510         iTxCheckSum = (cMsg[c_iStartCheckSum + 1] << 8) | cMsg[c_iStartCheckSum];
00511         
00512         iCheckSum = 0;
00513         for(i = 0; i < c_iStartCheckSum; i++)
00514         {
00515                 iCheckSum %= 0xFF00;
00516                 iCheckSum += cMsg[i];
00517         }
00518 
00519         if(iCheckSum != iTxCheckSum)
00520         {
00521                 return false;
00522         }
00523 
00524         // convert data
00525         int iCnt = 0;
00526 
00527         //RelayboardStatus bytes contain EM-Stop and Scanner-Stop bits
00528         m_iRelBoardStatus = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00529         iCnt += 2;
00530 
00531         //unused at the moment
00532         m_iChargeCurrent = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00533         iCnt += 2;
00534 
00535         //unused at the moment
00536         m_iRelBoardBattVoltage = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00537         iCnt += 2;
00538 
00539         //unused at the moment  
00540         m_iRelBoardKeyPad = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00541         iCnt += 2;
00542 
00543         //unused at the moment
00544         for(i = 0; i < 4; i++)
00545         {
00546                 m_iRelBoardAnalogIn[i] = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00547                 iCnt += 2;
00548         }
00549 
00550         //unused at the moment
00551         m_iRelBoardTempSensor = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00552         iCnt += 2;
00553         
00554         //Digital Inputs
00555         //unused at the moment
00556         m_iDigIn = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00557         iCnt += 2;
00558 
00559         //Throw away rest of the message, it was used for earlier purposes
00560 
00561         m_Mutex.unlock();
00562         return true;
00563 }


cob_relayboard
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:34