Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
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
00135 iNrBytesInQueue = m_SerIO.getSizeRXQueue();
00136 if(iNrBytesInQueue < c_iNrBytesMin)
00137 {
00138
00139 siNoMsgCnt++;
00140 if(siNoMsgCnt > 29)
00141 {
00142
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
00155 iNrBytesRead = m_SerIO.readBlocking((char*)&cDat[0], iNrBytesInQueue);
00156 for(i = (iNrBytesRead - c_iNrBytesMin); i >= 0 ; i--)
00157 {
00158
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
00164 if( convRecMsgToData(&cDat[iDataStart]) )
00165 {
00166 return errorFlag;
00167 }
00168 else
00169 {
00170
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
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
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
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
00443 m_iCmdRelayBoard &= ~CMD_RESET_POS_CNT;
00444
00445 }
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
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
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
00525 int iCnt = 0;
00526
00527
00528 m_iRelBoardStatus = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00529 iCnt += 2;
00530
00531
00532 m_iChargeCurrent = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00533 iCnt += 2;
00534
00535
00536 m_iRelBoardBattVoltage = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00537 iCnt += 2;
00538
00539
00540 m_iRelBoardKeyPad = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00541 iCnt += 2;
00542
00543
00544 for(i = 0; i < 4; i++)
00545 {
00546 m_iRelBoardAnalogIn[i] = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00547 iCnt += 2;
00548 }
00549
00550
00551 m_iRelBoardTempSensor = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00552 iCnt += 2;
00553
00554
00555
00556 m_iDigIn = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00557 iCnt += 2;
00558
00559
00560
00561 m_Mutex.unlock();
00562 return true;
00563 }