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 #include <stdio.h>
00037 #include <math.h>
00038 #include <ros/ros.h>
00039 #include "../include/RelayBoard_v2.h"
00040
00041
00042
00043
00044
00045
00046 #define RS422_BAUDRATE 420000
00047
00048 #define RS422_TIMEOUT 0.025
00049
00050
00051 RelayBoardV2::RelayBoardV2()
00052 {
00053 m_iNumBytesSend = 0;
00054 m_iNumBytesRec = 0;
00055 m_ihasRelayData = 0;
00056 m_ihas_LCD_DATA = 0;
00057 m_iHasIOBoard = 0;
00058 m_iHasUSBoard = 0;
00059 m_iHasSpeakerData = 0;
00060 m_iChargeState = 0;
00061
00062
00063
00064 m_REC_MSG.iRelayBoard_Status = 0;
00065 m_REC_MSG.iCharging_Current = 0;
00066 m_REC_MSG.iCharging_State = 0;
00067 m_REC_MSG.iBattery_Voltage = 0;
00068 m_REC_MSG.iKey_Pad = 0;
00069 m_REC_MSG.iTemp_Sensor = 0;
00070 m_REC_MSG.iIODig_In = 0;
00071 m_REC_MSG.iIOBoard_State = 0;
00072 m_REC_MSG.iUSBoard_State = 0;
00073
00074 for(int a = 0; a<8;a++)
00075 {
00076 m_Motor[a].lEnc = 0.0;
00077 m_Motor[a].lEncS = 0.0;
00078 m_Motor[a].iStatus = 0;
00079 m_Motor[a].ldesiredEncS = 0.0;
00080 }
00081
00082 m_S_MSG.iSoftEM = 0;
00083 m_S_MSG.iCmdRelayBoard = 0;
00084 m_S_MSG.IOBoard_Cmd = 0;
00085 m_S_MSG.USBoard_Cmd = 0;
00086 m_S_MSG.Speaker = 0;
00087 m_S_MSG.SpeakerLoud = 0;
00088
00089 m_blogging = false;
00090
00091 ROS_INFO("Starting RelayBoardV2 Node\n");
00092
00093 }
00094
00095 RelayBoardV2::~RelayBoardV2()
00096 {
00097 m_SerIO.closeIO();
00098 }
00099
00100 int RelayBoardV2::evalRxBuffer()
00101 {
00102 int errorFlag = NO_ERROR;
00103
00104 bool found_header = false;
00105 int waiting_cnt = 0;
00106 int waiting_cnt2 = 0;
00107 int msg_type = 100;
00108 int error_cnt = 0;
00109 int no_data_cnt = 0;
00110 int BytesToRead = 0;
00111 int received_checksum = 0;
00112 int my_checksum = 0;
00113 const int c_iSizeBuffer = 130;
00114 int iNrBytesRead = 0;
00115 unsigned char cDat[c_iSizeBuffer];
00116 unsigned char cHeader[4] = {0x00, 0x00, 0x00, 0x00};
00117
00118 while(found_header == false)
00119 {
00120 if(m_SerIO.getSizeRXQueue() >= 1)
00121 {
00122 waiting_cnt = 0;
00123
00124 cHeader[3] = cHeader[2];
00125 cHeader[2] = cHeader[1];
00126 cHeader[1] = cHeader[0];
00127 iNrBytesRead = m_SerIO.readBlocking((char*)&cHeader[0], 1);
00128
00129 if((cHeader[3] == 0x08) && (cHeader[2] == 0xFE) && (cHeader[1] == 0xEF) && (cHeader[0] == 0x08))
00130 {
00131
00132 msg_type = 1;
00133 found_header = true;
00134 }
00135 else if((cHeader[3] == 0x02) && (cHeader[2] == 0x80) && (cHeader[1] == 0xD6) && (cHeader[0] == 0x02))
00136 {
00137
00138 msg_type = 2;
00139 found_header = true;
00140 }
00141 else if((cHeader[3] == 0x02) && (cHeader[2] == 0xFF) && (cHeader[1] == 0xD6) && (cHeader[0] == 0x02))
00142 {
00143
00144 ROS_INFO("Error Msg");
00145 msg_type = 3;
00146 found_header = true;
00147 }
00148 if(++error_cnt > 20)
00149 {
00150
00151
00152 return 99;
00153 }
00154
00155 }
00156 else
00157 {
00158 waiting_cnt++;
00159 if(waiting_cnt > 60000)
00160 {
00161 waiting_cnt = 0;
00162 waiting_cnt2++;
00163 if(waiting_cnt2 > 10)
00164 {
00165
00166 return 101;
00167 }
00168 }
00169 }
00170 }
00171 switch(msg_type)
00172 {
00173 case 1: BytesToRead = m_iNumBytesRec - 4;
00174 break;
00175 case 2: BytesToRead = 6;
00176 break;
00177 case 3: BytesToRead = 3;
00178 break;
00179 default: return 98;
00180 }
00181 error_cnt = 0;
00182 while(m_SerIO.getSizeRXQueue() < BytesToRead)
00183 {
00184 usleep(2000);
00185 }
00186 iNrBytesRead = m_SerIO.readBlocking((char*)&cDat[0], BytesToRead);
00187
00188 if(m_blogging == true)
00189 {
00190 log_to_file(2, cDat);
00191 }
00192
00193 my_checksum += cHeader[3];
00194 my_checksum %= 0xFF00;
00195 my_checksum += cHeader[2];
00196 my_checksum %= 0xFF00;
00197 my_checksum += cHeader[1];
00198 my_checksum %= 0xFF00;
00199 my_checksum += cHeader[0];
00200 for(int e = 0; e < iNrBytesRead - 2; e++)
00201 {
00202 my_checksum %= 0xFF00;
00203 my_checksum += cDat[e];
00204 }
00205
00206 received_checksum = (cDat[BytesToRead - 1] << 8);
00207 received_checksum += cDat[BytesToRead - 2];
00208
00209 if(received_checksum != my_checksum)
00210 {
00211
00212 return 97;
00213 }
00214 if(msg_type == 1)
00215 {
00216 convRecMsgToData(&cDat[0]);
00217 return 0;
00218 }
00219 else if(msg_type == 2)
00220 {
00221 m_iFoundMotors = cDat[0];
00222 m_iHomedMotors = cDat[1];
00223 m_iFoundExtHardware = cDat[2];
00224 m_iConfigured = cDat[3];
00225
00226
00227 ROS_INFO("Ext Hardware: %d",cDat[2]);
00228
00229 return 1;
00230 }
00231 else if(msg_type == 3)
00232 {
00233
00234 return 2;
00235 }
00236 else
00237 {
00238 return 100;
00239 }
00240
00241 return 0;
00242 }
00243
00244 int RelayBoardV2::init(const char* cdevice_name, int iactive_motors, int ihoming_motors, int iext_hardware, long lModulo0, long lModulo1, long lModulo2, long lModulo3, long lModulo4, long lModulo5, long lModulo6, long lModulo7)
00245 {
00246 m_SerIO.setDeviceName(cdevice_name);
00247 m_SerIO.setBaudRate(RS422_BAUDRATE);
00248 m_SerIO.openIO();
00249 unsigned char cConfig_Data[33];
00250 int iChkSum = 0;
00251 int byteswritten = 0;
00252 int answertype = 0;
00253 int iNumDrives = 0;
00254
00255 cConfig_Data[0] = 0x02;
00256 cConfig_Data[1] = 0x80;
00257 cConfig_Data[2] = 0xD6;
00258 cConfig_Data[3] = 0x02;
00259
00260 cConfig_Data[4] = iactive_motors;
00261 cConfig_Data[5] = ihoming_motors;
00262 cConfig_Data[6] = iext_hardware;
00263
00264
00265 if((iactive_motors & 0x80) == 0x80) m_Motor[7].iMotorActive = 1;
00266 else m_Motor[7].iMotorActive = 0;
00267 if((iactive_motors & 0x40) == 0x40) m_Motor[6].iMotorActive = 1;
00268 else m_Motor[6].iMotorActive = 0;
00269 if((iactive_motors & 0x20) == 0x20) m_Motor[5].iMotorActive = 1;
00270 else m_Motor[5].iMotorActive = 0;
00271 if((iactive_motors & 0x10) == 0x10) m_Motor[4].iMotorActive = 1;
00272 else m_Motor[4].iMotorActive = 0;
00273 if((iactive_motors & 0x8) == 0x8) m_Motor[3].iMotorActive = 1;
00274 else m_Motor[3].iMotorActive = 0;
00275 if((iactive_motors & 0x4) == 0x4) m_Motor[2].iMotorActive = 1;
00276 else m_Motor[2].iMotorActive = 0;
00277 if((iactive_motors & 0x2) == 0x2) m_Motor[1].iMotorActive = 1;
00278 else m_Motor[1].iMotorActive = 0;
00279 if((iactive_motors & 0x1) == 0x1) m_Motor[0].iMotorActive = 1;
00280 else m_Motor[0].iMotorActive = 0;
00281
00282
00283 cConfig_Data[7] = lModulo0 >> 16;
00284 cConfig_Data[8] = lModulo0 >> 8;
00285 cConfig_Data[9] = lModulo0;
00286
00287 cConfig_Data[10] = lModulo1 >> 16;
00288 cConfig_Data[11] = lModulo1 >> 8;
00289 cConfig_Data[12] = lModulo1;
00290
00291 cConfig_Data[13] = lModulo2 >> 16;
00292 cConfig_Data[14] = lModulo2 >> 8;
00293 cConfig_Data[15] = lModulo2;
00294
00295 cConfig_Data[16] = lModulo3 >> 16;
00296 cConfig_Data[17] = lModulo3 >> 8;
00297 cConfig_Data[18] = lModulo3;
00298
00299 cConfig_Data[19] = lModulo4 >> 16;
00300 cConfig_Data[20] = lModulo4 >> 8;
00301 cConfig_Data[21] = lModulo4;
00302
00303 cConfig_Data[22] = lModulo5 >> 16;
00304 cConfig_Data[23] = lModulo5 >> 8;
00305 cConfig_Data[24] = lModulo5;
00306
00307 cConfig_Data[25] = lModulo6 >> 16;
00308 cConfig_Data[26] = lModulo6 >> 8;
00309 cConfig_Data[27] = lModulo6;
00310
00311 cConfig_Data[28] = lModulo7 >> 16;
00312 cConfig_Data[29] = lModulo7 >> 8;
00313 cConfig_Data[30] = lModulo7;
00314
00315
00316 m_iNumBytesRec = 4;
00317 m_iNumBytesRec += 13;
00318 for(int g = 0; g < 8;g++)
00319 {
00320 if(m_Motor[g].iMotorActive == 1)
00321 {
00322 iNumDrives++;
00323 }
00324 }
00325
00326 m_iNumBytesRec += (iNumDrives*10);
00327
00328 if((iext_hardware & 1) == 1)
00329 {
00330 ROS_INFO("Ext. Hardware: IOBoard");
00331 m_iNumBytesRec += 20;
00332 }
00333 if((iext_hardware & 0x2) == 0x2)
00334 {
00335 ROS_INFO("Ext. Hardware: USBoard");
00336 m_iNumBytesRec += 26;
00337 }
00338
00339 m_iNumBytesRec += 2;
00340
00341
00342 for(int i=4;i<=30;i++)
00343 {
00344 iChkSum %= 0xFF00;
00345 iChkSum += cConfig_Data[i];
00346 }
00347
00348
00349
00350 cConfig_Data[31] = iChkSum >> 8;
00351 cConfig_Data[32] = iChkSum;
00352
00353 if(m_blogging == true)
00354 {
00355 log_to_file(3, cConfig_Data);
00356 }
00357 byteswritten = m_SerIO.writeIO((char*)cConfig_Data,33);
00358 if(byteswritten != 33)
00359 {
00360
00361 return 60;
00362 }
00363 else
00364 {
00365
00366 }
00367
00368 answertype = evalRxBuffer();
00369 if(answertype == 1)
00370 {
00371
00372 if(m_iConfigured == 1)
00373 {
00374 ROS_INFO("Configured");
00375 return 1;
00376 }
00377 else
00378 {
00379 ROS_ERROR("FAILED: Invalid configuration");
00380 return 59;
00381 }
00382
00383 }
00384
00385 return 50;
00386
00387
00388 }
00389
00390 bool RelayBoardV2::shutdownPltf()
00391 {
00392 m_SerIO.closeIO();
00393 return true;
00394 }
00395
00396 bool RelayBoardV2::isEMStop()
00397 {
00398 if( (m_REC_MSG.iRelayBoard_Status & 0x0001) != 0)
00399 {
00400 return true;
00401 }
00402 else
00403 {
00404 return false;
00405 }
00406 }
00407
00408 void RelayBoardV2::setEMStop()
00409 {
00410 m_S_MSG.iSoftEM |= 0x01;
00411 ROS_ERROR("Software Emergency Stop AKTIVE");
00412 }
00413
00414
00415 void RelayBoardV2::resetEMStop()
00416 {
00417 m_S_MSG.iSoftEM |= 0x02;
00418 ROS_ERROR("Software Emergency Stop INAKTIVE");
00419 }
00420
00421 bool RelayBoardV2::isScannerStop()
00422 {
00423 if( (m_REC_MSG.iRelayBoard_Status & 0x0002) != 0)
00424 {
00425 return true;
00426 }
00427 else
00428 {
00429 return false;
00430 }
00431 }
00432
00433 int RelayBoardV2::sendDataToRelayBoard()
00434 {
00435
00436 int errorFlag = NO_ERROR;
00437 int iNrBytesWritten;
00438
00439 unsigned char cMsg[80];
00440
00441 m_Mutex.lock();
00442
00443 m_iNumBytesSend = convDataToSendMsg(cMsg);
00444
00445 m_SerIO.purgeTx();
00446 iNrBytesWritten = m_SerIO.writeIO((char*)cMsg,m_iNumBytesSend);
00447
00448 if(iNrBytesWritten < m_iNumBytesSend) {
00449
00450
00451 }
00452
00453 if(m_blogging == true)
00454 {
00455 log_to_file(1, cMsg);
00456 }
00457
00458 m_Mutex.unlock();
00459 return errorFlag;
00460
00461 };
00462
00463
00464
00465 void RelayBoardV2::setMotorDesiredEncS(int imotor_nr, long lEncS)
00466 {
00467 m_Mutex.lock();
00468
00469 m_Motor[imotor_nr].ldesiredEncS = lEncS;
00470
00471 m_Mutex.unlock();
00472 }
00473
00474 void RelayBoardV2::getMotorEnc(int imotor_nr,long* lmotorEnc)
00475 {
00476 m_Mutex.lock();
00477
00478 *lmotorEnc = m_Motor[imotor_nr].lEnc;
00479
00480 m_Mutex.unlock();
00481
00482 }
00483
00484 void RelayBoardV2::getMotorEncS(int imotor_nr,long* lmotorEncS)
00485 {
00486 m_Mutex.lock();
00487
00488 *lmotorEncS = m_Motor[imotor_nr].lEncS;
00489
00490 m_Mutex.unlock();
00491 }
00492
00493 void RelayBoardV2::getMotorState(int imotor_nr,int* imotorStatus)
00494 {
00495 m_Mutex.lock();
00496
00497 *imotorStatus = m_Motor[imotor_nr].iStatus;
00498
00499 m_Mutex.unlock();
00500 }
00501
00502
00503
00504 void RelayBoardV2::getRelayBoardState(int* State)
00505 {
00506 m_Mutex.lock();
00507 *State = m_REC_MSG.iRelayBoard_Status;
00508 m_Mutex.unlock();
00509 }
00510
00511 void RelayBoardV2::getRelayBoardDigOut(int* DigOut)
00512 {
00513 m_Mutex.lock();
00514 *DigOut = m_S_MSG.iCmdRelayBoard;
00515 m_Mutex.unlock();
00516 }
00517
00518 void RelayBoardV2::setRelayBoardDigOut(int iChannel, bool bOn)
00519 {
00520
00521
00522
00523
00524
00525 m_ihasRelayData = 1;
00526 switch( iChannel)
00527 {
00528 case 1:
00529
00530 if(bOn) { m_S_MSG.iCmdRelayBoard |= 8; }
00531 else { m_S_MSG.iCmdRelayBoard &= ~ 8; }
00532
00533 break;
00534
00535 case 2:
00536
00537 if(bOn) { m_S_MSG.iCmdRelayBoard |= 16; }
00538 else { m_S_MSG.iCmdRelayBoard &= ~ 16; }
00539
00540 break;
00541
00542 case 3:
00543
00544 if(bOn) { m_S_MSG.iCmdRelayBoard |= 32; }
00545 else { m_S_MSG.iCmdRelayBoard &= ~ 32; }
00546
00547 break;
00548
00549 case 4:
00550
00551 if(bOn) { m_S_MSG.iCmdRelayBoard |= 64; }
00552 else { m_S_MSG.iCmdRelayBoard &= ~ 64; }
00553
00554 break;
00555
00556 default:
00557
00558 m_S_MSG.iCmdRelayBoard = 0;
00559 }
00560 }
00561
00562 void RelayBoardV2::getTemperature(int* temp)
00563 {
00564 m_Mutex.lock();
00565
00566 *temp = m_REC_MSG.iTemp_Sensor;
00567
00568 m_Mutex.unlock();
00569 }
00570
00571 void RelayBoardV2::getBattVoltage(int* battvolt)
00572 {
00573 m_Mutex.lock();
00574
00575 *battvolt = m_REC_MSG.iBattery_Voltage;
00576
00577 m_Mutex.unlock();
00578 }
00579
00580 void RelayBoardV2::getChargingCurrent(int* current)
00581 {
00582 m_Mutex.lock();
00583
00584 *current = m_REC_MSG.iCharging_Current;
00585
00586 m_Mutex.unlock();
00587 }
00588
00589 void RelayBoardV2::getChargingState(int* state)
00590 {
00591 m_Mutex.lock();
00592
00593 *state = m_REC_MSG.iCharging_State;
00594
00595 m_Mutex.unlock();
00596 }
00597
00598 void RelayBoardV2::writeLCD(const std::string& sText)
00599 {
00600 int iSize;
00601 m_ihas_LCD_DATA = 1;
00602 m_Mutex.lock();
00603
00604 iSize = sText.size();
00605
00606 for(int i = 0; i < 20; i++)
00607 {
00608 if(i < iSize)
00609 {
00610 m_S_MSG.LCD_Txt[i] = sText[i];
00611 }
00612 else
00613 {
00614 m_S_MSG.LCD_Txt[i] = ' ';
00615 }
00616 }
00617 m_Mutex.unlock();
00618 }
00619
00620 void RelayBoardV2::getKeyPad(int* keypad)
00621 {
00622 m_Mutex.lock();
00623 *keypad = m_REC_MSG.iKey_Pad;
00624 m_Mutex.unlock();
00625 }
00626
00627 void RelayBoardV2::startCharging()
00628 {
00629 m_Mutex.lock();
00630 m_ihasRelayData = 1;
00631 m_S_MSG.iCmdRelayBoard |= 1;
00632 m_Mutex.unlock();
00633 }
00634
00635 void RelayBoardV2::stopCharging()
00636 {
00637 m_Mutex.lock();
00638 m_ihasRelayData = 1;
00639 m_S_MSG.iCmdRelayBoard &= ~ 1;
00640 m_Mutex.unlock();
00641 }
00642
00643
00644
00645 void RelayBoardV2::getIOBoardDigIn(int* DigIn)
00646 {
00647 m_Mutex.lock();
00648 *DigIn = m_REC_MSG.iIODig_In;
00649 m_Mutex.unlock();
00650 }
00651
00652 void RelayBoardV2::getIOBoardDigOut(int* DigOut)
00653 {
00654 m_Mutex.lock();
00655 *DigOut = m_S_MSG.IOBoard_Cmd;
00656 m_Mutex.unlock();
00657 }
00658
00659 void RelayBoardV2::setIOBoardDigOut(int iChannel, bool bVal)
00660 {
00661 m_iHasIOBoard = 1;
00662 int iMask;
00663
00664 iMask = (1 << iChannel);
00665
00666 if(bVal)
00667 {
00668 m_S_MSG.IOBoard_Cmd |= iMask;
00669 }
00670 else
00671 {
00672 m_S_MSG.IOBoard_Cmd &= ~iMask;
00673 }
00674 }
00675
00676 void RelayBoardV2::getIOBoardAnalogIn(int* iAnalogIn)
00677 {
00678 int i;
00679
00680 m_Mutex.lock();
00681
00682 for(i = 0; i < 8; i++)
00683 {
00684 iAnalogIn[i] = m_REC_MSG.iIOAnalog_In[i];
00685 }
00686
00687 m_Mutex.unlock();
00688 }
00689
00690
00691
00692 void RelayBoardV2::startUSBoard(int iChannelActive)
00693 {
00694 m_Mutex.lock();
00695 m_iHasUSBoard = 1;
00696 m_S_MSG.USBoard_Cmd = iChannelActive;
00697 m_Mutex.unlock();
00698 }
00699
00700 void RelayBoardV2::stopUSBoard()
00701 {
00702 m_Mutex.lock();
00703 m_iHasUSBoard = 1;
00704 m_S_MSG.USBoard_Cmd = 0x00;
00705 m_Mutex.unlock();
00706 }
00707
00708 void RelayBoardV2::getUSBoardData1To8(int* piUSDistMM)
00709 {
00710 int i;
00711
00712 m_Mutex.lock();
00713
00714 for(i = 0; i < 8; i++)
00715 {
00716 piUSDistMM[i] = m_REC_MSG.iUSSensor_Dist[i];
00717 }
00718
00719 m_Mutex.unlock();
00720 }
00721
00722 void RelayBoardV2::getUSBoardData9To16(int* piUSDistMM)
00723 {
00724 int i;
00725
00726 m_Mutex.lock();
00727
00728 for(i = 0; i < 8; i++)
00729 {
00730 piUSDistMM[i] = m_REC_MSG.iUSSensor_Dist[i + 8];
00731 }
00732
00733 m_Mutex.unlock();
00734 }
00735
00736 void RelayBoardV2::getUSBoardAnalogIn(int* piAnalogIn)
00737 {
00738 int i;
00739
00740 m_Mutex.lock();
00741
00742 for(i = 0; i < 4; i++)
00743 {
00744 piAnalogIn[i] = m_REC_MSG.iUSAnalog_In[i];
00745 }
00746
00747 m_Mutex.unlock();
00748 }
00749
00750
00751
00752 void RelayBoardV2::enable_logging()
00753 {
00754 m_blogging = true;
00755 }
00756
00757 void RelayBoardV2::disable_logging()
00758 {
00759 m_blogging = false;
00760 }
00761
00762 void RelayBoardV2::log_to_file(int direction, unsigned char cMsg[])
00763 {
00764 FILE * pFile;
00765
00766 pFile = fopen ("neo_relayboard_RX_TX_log.log","a");
00767
00768 if(direction == 1)
00769 {
00770 fprintf (pFile, "\n\n Direction: %i", direction);
00771 for(int i=0; i<m_iNumBytesSend; i++)
00772 fprintf(pFile," %.2x", cMsg[i]);
00773 fprintf(pFile,"\n");
00774 }
00775 if(direction == 2)
00776 {
00777 fprintf (pFile, "\n\n Direction: %i", direction);
00778 for(int i=0; i<(m_iNumBytesRec + 2); i++)
00779 fprintf(pFile," %.2x", cMsg[i]);
00780 fprintf(pFile,"\n");
00781 }
00782 if(direction == 3)
00783 {
00784 fprintf (pFile, "\n\n Direction: %i", direction);
00785 for(int i=0; i<(33); i++)
00786 fprintf(pFile," %.2x", cMsg[i]);
00787 fprintf(pFile,"\n");
00788 }
00789
00790 fclose (pFile);
00791 }
00792
00793
00794
00795 void RelayBoardV2::convRecMsgToData(unsigned char cMsg[])
00796 {
00797
00798 int data_in_message = 0;
00799
00800 m_Mutex.lock();
00801
00802
00803 int iCnt = 0;
00804
00805
00806 data_in_message = cMsg[iCnt];
00807 iCnt++;
00808
00809 m_REC_MSG.iRelayBoard_Status = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00810 iCnt += 2;
00811
00812 m_REC_MSG.iCharging_Current = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00813 iCnt += 2;
00814
00815 m_REC_MSG.iCharging_State = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00816 iCnt += 2;
00817
00818 m_REC_MSG.iBattery_Voltage = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00819 iCnt += 2;
00820
00821 m_REC_MSG.iKey_Pad = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00822 iCnt += 2;
00823
00824 m_REC_MSG.iTemp_Sensor = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00825 iCnt += 2;
00826
00827
00828 if((m_iFoundExtHardware & 0x1) == 0x1)
00829 {
00830 m_REC_MSG.iIODig_In = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00831 iCnt += 2;
00832 for(int i = 0; i < 8; i++)
00833 {
00834 m_REC_MSG.iIOAnalog_In[i] = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00835 iCnt += 2;
00836 }
00837
00838 m_REC_MSG.iIOBoard_State = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00839 iCnt += 2;
00840 }
00841
00842 if((m_iFoundExtHardware & 0x2) == 0x2)
00843 {
00844 for(int i = 0; i < 16; i++)
00845 {
00846 m_REC_MSG.iUSSensor_Dist[i] = (cMsg[iCnt++]);
00847 }
00848 for(int i = 0; i < 4; i++)
00849 {
00850 m_REC_MSG.iUSAnalog_In[i] = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00851 iCnt += 2;
00852 }
00853
00854 m_REC_MSG.iUSBoard_State = (cMsg[iCnt + 1] << 8) | cMsg[iCnt];
00855 iCnt += 2;
00856 }
00857
00858 if(m_Motor[0].iMotorActive)
00859 {
00860
00861 m_Motor[0].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00862 iCnt += 4;
00863
00864 m_Motor[0].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00865 iCnt += 4;
00866
00867 m_Motor[0].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00868 iCnt += 2;
00869 }
00870 if(m_Motor[1].iMotorActive)
00871 {
00872
00873 m_Motor[1].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00874 iCnt += 4;
00875
00876 m_Motor[1].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00877 iCnt += 4;
00878
00879 m_Motor[1].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00880 iCnt += 2;
00881 }
00882 if(m_Motor[2].iMotorActive)
00883 {
00884
00885 m_Motor[2].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00886 iCnt += 4;
00887
00888 m_Motor[2].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00889 iCnt += 4;
00890
00891 m_Motor[2].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00892 iCnt += 2;
00893 }
00894 if(m_Motor[3].iMotorActive)
00895 {
00896
00897 m_Motor[3].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00898 iCnt += 4;
00899
00900 m_Motor[3].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00901 iCnt += 4;
00902
00903 m_Motor[3].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00904 iCnt += 2;
00905 }
00906 if(m_Motor[4].iMotorActive)
00907 {
00908
00909 m_Motor[4].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00910 iCnt += 4;
00911
00912 m_Motor[4].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00913 iCnt += 4;
00914
00915 m_Motor[4].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00916 iCnt += 2;
00917 }
00918 if(m_Motor[5].iMotorActive)
00919 {
00920
00921 m_Motor[5].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00922 iCnt += 4;
00923
00924 m_Motor[5].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00925 iCnt += 4;
00926
00927 m_Motor[5].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00928 iCnt += 2;
00929 }
00930 if(m_Motor[6].iMotorActive)
00931 {
00932
00933 m_Motor[6].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00934 iCnt += 4;
00935
00936 m_Motor[6].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00937 iCnt += 4;
00938
00939 m_Motor[6].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00940 iCnt += 2;
00941 }
00942 if(m_Motor[7].iMotorActive)
00943 {
00944
00945 m_Motor[7].lEnc = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00946 iCnt += 4;
00947
00948 m_Motor[7].lEncS = (cMsg[iCnt+3] << 24) + (cMsg[iCnt+2] << 16) + (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00949 iCnt += 4;
00950
00951 m_Motor[7].iStatus = (cMsg[iCnt+1] << 8) + cMsg[iCnt];
00952 iCnt += 2;
00953 }
00954
00955 m_Mutex.unlock();
00956 }
00957
00958
00959 int RelayBoardV2::convDataToSendMsg(unsigned char cMsg[])
00960 {
00961
00962 int iCnt = 0;
00963 int iChkSum = 0;
00964
00965 int has_data = 0;
00966 int has_motor_data8 = 0;
00967 int has_motor_data4 = 0;
00968
00969 if((m_Motor[0].iMotorActive) || (m_Motor[1].iMotorActive) || (m_Motor[2].iMotorActive) || (m_Motor[3].iMotorActive))
00970 {
00971 has_motor_data4 = 1;
00972 if((m_Motor[4].iMotorActive) || (m_Motor[5].iMotorActive) || (m_Motor[6].iMotorActive) || (m_Motor[7].iMotorActive))
00973 {
00974 has_motor_data8 = 1;
00975 }
00976 }
00977
00978 has_data = (has_motor_data8 << 6) + (has_motor_data4 << 5) + (m_ihasRelayData << 4) + (m_ihas_LCD_DATA << 3) + (m_iHasIOBoard << 2) +(m_iHasUSBoard << 1) + (m_iHasSpeakerData);
00979
00980
00981 cMsg[iCnt++] = 0x02;
00982 cMsg[iCnt++] = 0xD6;
00983 cMsg[iCnt++] = 0x80;
00984 cMsg[iCnt++] = 0x02;
00985
00986
00987
00988
00989
00990
00991
00992
00993
00994
00995 cMsg[iCnt++] = has_data;
00996 cMsg[iCnt++] = m_S_MSG.iSoftEM;
00997
00998 if(has_motor_data8)
00999 {
01000
01001 cMsg[iCnt++] = ((m_Motor[7].ldesiredEncS & 0xFF000000) >> 24);
01002 cMsg[iCnt++] = ((m_Motor[7].ldesiredEncS & 0xFF0000) >> 16);
01003 cMsg[iCnt++] = ((m_Motor[7].ldesiredEncS & 0xFF00) >> 8);
01004 cMsg[iCnt++] = (m_Motor[7].ldesiredEncS & 0xFF);
01005
01006 cMsg[iCnt++] = ((m_Motor[6].ldesiredEncS & 0xFF000000) >> 24);
01007 cMsg[iCnt++] = ((m_Motor[6].ldesiredEncS & 0xFF0000) >> 16);
01008 cMsg[iCnt++] = ((m_Motor[6].ldesiredEncS & 0xFF00) >> 8);
01009 cMsg[iCnt++] = (m_Motor[6].ldesiredEncS & 0xFF);
01010
01011 cMsg[iCnt++] = ((m_Motor[5].ldesiredEncS & 0xFF000000) >> 24);
01012 cMsg[iCnt++] = ((m_Motor[5].ldesiredEncS & 0xFF0000) >> 16);
01013 cMsg[iCnt++] = ((m_Motor[5].ldesiredEncS & 0xFF00) >> 8);
01014 cMsg[iCnt++] = (m_Motor[5].ldesiredEncS & 0xFF);
01015
01016 cMsg[iCnt++] = ((m_Motor[4].ldesiredEncS & 0xFF000000) >> 24);
01017 cMsg[iCnt++] = ((m_Motor[4].ldesiredEncS & 0xFF0000) >> 16);
01018 cMsg[iCnt++] = ((m_Motor[4].ldesiredEncS & 0xFF00) >> 8);
01019 cMsg[iCnt++] = (m_Motor[4].ldesiredEncS & 0xFF);
01020 }
01021 if(has_motor_data4)
01022 {
01023
01024 cMsg[iCnt++] = ((m_Motor[3].ldesiredEncS & 0xFF000000) >> 24);
01025 cMsg[iCnt++] = ((m_Motor[3].ldesiredEncS & 0xFF0000) >> 16);
01026 cMsg[iCnt++] = ((m_Motor[3].ldesiredEncS & 0xFF00) >> 8);
01027 cMsg[iCnt++] = (m_Motor[3].ldesiredEncS & 0xFF);
01028
01029 cMsg[iCnt++] = ((m_Motor[2].ldesiredEncS & 0xFF000000) >> 24);
01030 cMsg[iCnt++] = ((m_Motor[2].ldesiredEncS & 0xFF0000) >> 16);
01031 cMsg[iCnt++] = ((m_Motor[2].ldesiredEncS & 0xFF00) >> 8);
01032 cMsg[iCnt++] = (m_Motor[2].ldesiredEncS & 0xFF);
01033
01034 cMsg[iCnt++] = ((m_Motor[1].ldesiredEncS & 0xFF000000) >> 24);
01035 cMsg[iCnt++] = ((m_Motor[1].ldesiredEncS & 0xFF0000) >> 16);
01036 cMsg[iCnt++] = ((m_Motor[1].ldesiredEncS & 0xFF00) >> 8);
01037 cMsg[iCnt++] = (m_Motor[1].ldesiredEncS & 0xFF);
01038
01039 cMsg[iCnt++] = ((m_Motor[0].ldesiredEncS & 0xFF000000) >> 24);
01040 cMsg[iCnt++] = ((m_Motor[0].ldesiredEncS & 0xFF0000) >> 16);
01041 cMsg[iCnt++] = ((m_Motor[0].ldesiredEncS & 0xFF00) >> 8);
01042 cMsg[iCnt++] = (m_Motor[0].ldesiredEncS & 0xFF);
01043 }
01044
01045 if(m_ihasRelayData)
01046 {
01047 cMsg[iCnt++] = m_S_MSG.iCmdRelayBoard >> 8;
01048 cMsg[iCnt++] = m_S_MSG.iCmdRelayBoard;
01049 }
01050
01051 if(m_ihas_LCD_DATA)
01052 {
01053
01054 for(int u = 0; u < 20; u++)
01055 {
01056 cMsg[iCnt++] = m_S_MSG.LCD_Txt[u];
01057 }
01058 }
01059
01060 if(m_iHasIOBoard)
01061 {
01062 cMsg[iCnt++] = m_S_MSG.IOBoard_Cmd >> 8;
01063 cMsg[iCnt++] = m_S_MSG.IOBoard_Cmd;
01064 }
01065
01066 if(m_iHasUSBoard)
01067 {
01068 cMsg[iCnt++] = m_S_MSG.USBoard_Cmd >> 8;
01069 cMsg[iCnt++] = m_S_MSG.USBoard_Cmd;
01070 }
01071
01072 if(m_iHasSpeakerData)
01073 {
01074 cMsg[iCnt++] = m_S_MSG.Speaker >> 8;
01075 cMsg[iCnt++] = m_S_MSG.SpeakerLoud;
01076 }
01077 m_iNumBytesSend = iCnt;
01078
01079 for(int i = 4; i < m_iNumBytesSend; i++)
01080 {
01081 iChkSum %= 0xFF00;
01082 iChkSum += cMsg[i];
01083 }
01084 cMsg[m_iNumBytesSend] = iChkSum >> 8;
01085 cMsg[m_iNumBytesSend + 1] = iChkSum;
01086
01087
01088 m_iHasIOBoard = 0;
01089 m_ihasRelayData = 0;
01090 m_ihas_LCD_DATA = 0;
01091 m_iHasIOBoard = 0;
01092 m_iHasUSBoard = 0;
01093 m_iHasSpeakerData = 0;
01094
01095
01096
01097 return m_iNumBytesSend + 2;
01098 }