00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "rhino.h"
00012
00013
00014
00027
00028
00029 Rhino::Rhino()
00030 {
00031
00032 uiErrorCode = 0;
00033 uiStatusCode = 0;
00034 uiLogLevel = 3;
00035 pFile = NULL;
00036
00037 InitMessageVectors(RhinoBotErrorMsg, RhinoClassErrorMsg, RhinoBotStatusMsg, RhinoClassStatusMsg);
00038 }
00039
00040
00041
00051
00052 Rhino::~Rhino()
00053 {
00054 pFile = NULL;
00055 }
00056
00057
00058
00079
00080 int Rhino::set_LogLevel(unsigned int _uiLogLevel)
00081 {
00082 if(_uiLogLevel < 0 || _uiLogLevel > MAX_LOG_LEVEL)
00083 {
00084 uiLogLevel = 3;
00085 ProcessErrorMsg(LOG_LEV);
00086 ProcessStatusMsg(RC_DEF_LOG_LEV);
00087 return -1;
00088 }
00089
00090 uiLogLevel = _uiLogLevel;
00091 return 0;
00092 }
00093
00094
00095
00108
00109 int Rhino::get_LogLevel(unsigned int *_uiLogLevel)
00110 {
00111 *_uiLogLevel = uiLogLevel;
00112 return 0;
00113 }
00114
00115
00116
00131
00132 int Rhino::get_ErrorMsg(unsigned int _uiErrorCode, char *_cTextError)
00133 {
00134
00135 if(_uiErrorCode >= 0 && _uiErrorCode <= MAX_RB_ERROR_CODES)
00136 {
00137 strcpy(_cTextError, RhinoBotErrorMsg[_uiErrorCode]);
00138 return 0;
00139 }
00140
00141
00142 if(_uiErrorCode > MAX_RB_ERROR_CODES && _uiErrorCode <= (MAX_RB_ERROR_CODES + MAX_RC_ERROR_CODES))
00143 {
00144 strcpy(_cTextError, RhinoClassErrorMsg[_uiErrorCode-MAX_RB_ERROR_CODES]);
00145 return 0;
00146 }
00147
00148 ProcessErrorMsg(ERR_CODE);
00149 return -1;
00150 }
00151
00152
00153
00166
00167
00168 int Rhino::get_ErrorMsg(char *_cTextError)
00169 {
00170
00171 if(uiErrorCode >= 0 && uiErrorCode <= MAX_RB_ERROR_CODES)
00172 {
00173 strcpy(_cTextError, RhinoBotErrorMsg[uiErrorCode]);
00174 return 0;
00175 }
00176
00177
00178 if(uiErrorCode > MAX_RB_ERROR_CODES && uiErrorCode <= (MAX_RB_ERROR_CODES + MAX_RC_ERROR_CODES))
00179 {
00180 strcpy(_cTextError, RhinoClassErrorMsg[uiErrorCode-MAX_RB_ERROR_CODES]);
00181 return 0;
00182 }
00183
00184 ProcessErrorMsg(ERR_CODE);
00185 return -1;
00186 }
00187
00188
00189
00204
00205
00206 int Rhino::ProcessErrorMsg(unsigned int _uiErrorCode)
00207
00208 {
00209 if((_uiErrorCode < 0) || (_uiErrorCode > (MAX_RC_ERROR_CODES + MAX_RB_ERROR_CODES)))
00210 {
00211 ProcessErrorMsg(ERR_CODE);
00212 }
00213 else
00214 {
00215 uiErrorCode = _uiErrorCode;
00216
00217 get_ErrorMsg(cErrorMessage);
00218 sprintf(RhinoMessages, "RhinoClass->ProcessErrorMsg:\n\túltimo cmd enviado: %s\n\tUltima cadena recibida: %s\n\tcódigo de error: %d\n\tLog Level: %d\n\tMSG: %s\n", LastCmdAndMsg.LastCommandtoRhino, LastCmdAndMsg.LastMessageFromRhino, uiErrorCode, uiLogLevel, cErrorMessage);
00219
00220 if(uiLogLevel == 0)
00221 {
00222 LogMessage(LMOCBk, RhinoMessages);
00223 }
00224 if(uiLogLevel == 1)
00225 {
00226 LogMessage(LMODisplay, RhinoMessages);
00227 }
00228 if(uiLogLevel == 2)
00229 {
00230 LogMessage(LMOFile, RhinoMessages);
00231 }
00232 if(uiLogLevel == 3)
00233 {
00234 LogMessage(LMODisplay, RhinoMessages);
00235 LogMessage(LMOFile, RhinoMessages);
00236 }
00237 }
00238
00239 return 0;
00240 }
00241
00242
00243
00258
00259 int Rhino::ProcessStatusMsg(unsigned int _uiStatusCode)
00260 {
00261 if((_uiStatusCode < 0) || (_uiStatusCode > (MAX_RC_STATUS_CODES + MAX_RB_STATUS_CODES)))
00262 {
00263 ProcessErrorMsg(ERR_CODE);
00264 }
00265 else
00266 {
00267 uiStatusCode = _uiStatusCode;
00268
00269 get_StatusMsg(cStatusMessage);
00270 sprintf(RhinoMessages, "RhinoClass->ProcessStatusMsg:\n\túltimo cmd enviado: %s\n\tUltima cadena recibida: %s\n\tcódigo de estado: %d\n\tLog Level: %d\n\tMSG: %s\n", LastCmdAndMsg.LastCommandtoRhino, LastCmdAndMsg.LastMessageFromRhino, uiStatusCode, uiLogLevel, cStatusMessage);
00271
00272 if(uiLogLevel == 0)
00273 {
00274 LogMessage(LMOCBk, RhinoMessages);
00275 }
00276 if(uiLogLevel == 1)
00277 {
00278 LogMessage(LMODisplay, RhinoMessages);
00279 }
00280 if(uiLogLevel == 2)
00281 {
00282 LogMessage(LMOFile, RhinoMessages);
00283 }
00284 if(uiLogLevel == 3)
00285 {
00286 LogMessage(LMODisplay, RhinoMessages);
00287 LogMessage(LMOFile, RhinoMessages);
00288 }
00289 }
00290
00291 return 0;
00292 }
00293
00294
00295
00308
00309 int Rhino::get_ErrorCode(unsigned int *_uiErrorCode)
00310 {
00311 *_uiErrorCode = uiErrorCode;
00312 return 0;
00313 }
00314
00315
00316
00329
00330 int Rhino::DisplayAllErrorMessages(void)
00331 {
00332 sprintf(RhinoMessages, "ERROR MESSEGES\n\n");
00333 LogMessage(LMODisplay, RhinoMessages);
00334 int i=0;
00335
00336 sprintf(RhinoMessages, "RHINO CONTROLLER ERRORS\n");
00337 LogMessage(LMODisplay, RhinoMessages);
00338 for(i=0; i<MAX_RB_ERROR_CODES ; i++)
00339 {
00340 sprintf(RhinoMessages, "CODE:%d %s\n", i, RhinoBotErrorMsg[i]);
00341 LogMessage(LMODisplay, RhinoMessages);
00342 }
00343
00344 sprintf(RhinoMessages, "\nRHINO CLASS ERRORS\n");
00345 LogMessage(LMODisplay, RhinoMessages);
00346 for(i=0; i<MAX_RC_ERROR_CODES ; i++)
00347 {
00348 sprintf(RhinoMessages, "CODE:%d %s\n", i, RhinoClassErrorMsg[i]);
00349 LogMessage(LMODisplay, RhinoMessages);
00350 }
00351
00352 return 0;
00353 }
00354
00355
00356
00371
00372 int Rhino::LogMessage(int _LogType, char * _Message)
00373 {
00374 if(_LogType == LMOCBk)
00375 {
00376 strcpy(CallBack.sCBkMessage, _Message);
00377 CallBack.bNewMsg = true;
00378 }
00379
00380 if(_LogType == LMODisplay)
00381 {
00382 fprintf(stderr, "%s", _Message);
00383 }
00384
00385 if(_LogType == LMOFile)
00386 {
00387 pFile = fopen("rc_error_log.txt","a+");
00388 if(pFile == NULL){
00389 ProcessErrorMsg(NO_FILE);
00390 return -1;
00391 }
00392 fprintf(pFile,"%s\n", _Message);
00393 fclose(pFile);
00394 }
00395
00396 return 0;
00397 }
00398
00399
00400
00413
00414
00415 int Rhino::CBkMsg(void)
00416 {
00417 if(CallBack.bNewMsg == true)
00418 return 1;
00419 else
00420 return 0;
00421 }
00422
00423
00424
00437
00438 int Rhino::get_CBkMsg(char * _Message)
00439 {
00440 if(CallBack.bNewMsg == true){
00441 strcpy(_Message, CallBack.sCBkMessage);
00442 CallBack.bNewMsg = false;
00443 }
00444 else
00445 strcpy(_Message, "No new message from Rhino Class\n");
00446
00447 return 0;
00448 }
00449
00450
00451
00472
00473 int Rhino::InitMessageVectors(char _RhinoBotErrorMsg[MAX_RB_ERROR_CODES][MSG_LENGTH], char _RhinoClassErrorMsg[MAX_RC_ERROR_CODES][MSG_LENGTH], char _RhinoBotStatusMsg[MAX_RB_STATUS_CODES][MSG_LENGTH], char _RhinoClassStatusMsg[MAX_RC_STATUS_CODES][MSG_LENGTH])
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490 {
00491 const char RhinoBotErrorMsgSOURCE[MAX_RB_ERROR_CODES][MSG_LENGTH]={
00492 {" Rhino Controller Error: BE HAPY, NO PROBLEM "},
00493 {" Rhino Controller Error: NO ERROR "},
00494 {" Rhino Controller Error: NO ERROR "},
00495 {" Rhino Controller Error: NO ERROR "},
00496 {" Rhino Controller Error: NO ERROR "},
00497 {" Rhino Controller Error: NO ERROR "},
00498 {" Rhino Controller Error: NO ERROR "},
00499 {" Rhino Controller Error: NO ERROR "},
00500 {" Rhino Controller Error: NO ERROR "},
00501 {" Rhino Controller Error: NO ERROR "},
00502 {" Rhino Controller Error: Invalid command. "},
00503 {" Rhino Controller Error: Parameter out of bounds. "},
00504 {" Rhino Controller Error: Missing parameter. "},
00505 {" Rhino Controller Error: Expected delimiter not seen. "},
00506 {" Rhino Controller Error: Command string too long. "},
00507 {" Rhino Controller Error: NO ERROR "},
00508 {" Rhino Controller Error: The teach pendant is active or busy. "},
00509 {" Rhino Controller Error: NO ERROR "},
00510 {" Rhino Controller Error: NO ERROR "},
00511 {" Rhino Controller Error: NO ERROR "},
00512 {" Rhino Controller Error: Host input buffer overflow. "},
00513 {" Rhino Controller Error: Host USART timed out. "},
00514 {" Rhino Controller Error: Host USART error (framing, parity, etc.). "},
00515 {" Rhino Controller Error: Teach pendant input buffer overflow. "},
00516 {" Rhino Controller Error: Teach pendant USART timed out. "},
00517 {" Rhino Controller Error: Teach pendant USART error (framing, parity, etc.). "},
00518 {" Rhino Controller Error: Teach pendant USART overrun. "},
00519 {" Rhino Controller Error: Host USART overrun. "},
00520 {" Rhino Controller Error: NO ERROR "},
00521 {" Rhino Controller Error: NO ERROR "},
00522 {" Rhino Controller Error: Bad RAM location. "},
00523 {" Rhino Controller Error: NO ERROR "},
00524 {" Rhino Controller Error: Teach pendant returned diagnostic error. "},
00525 {" Rhino Controller Error: NO ERROR "},
00526 {" Rhino Controller Error: Teach pendant not present. "},
00527 {" Rhino Controller Error: NO ERROR "},
00528 {" Rhino Controller Error: NO ERROR "},
00529 {" Rhino Controller Error: NO ERROR "},
00530 {" Rhino Controller Error: NO ERROR "},
00531 {" Rhino Controller Error: NO ERROR "},
00532 {" Rhino Controller Error: Missing label. "},
00533 {" Rhino Controller Error: No program in memory. "},
00534 {" Rhino Controller Error: Insufficient teach pendant memory. "},
00535 {" Rhino Controller Error: Insufficient EEPROM memory. "},
00536 {" Rhino Controller Error: Can't replace first record. "},
00537 {" Rhino Controller Error: A pendant program already exists. "},
00538 {" Rhino Controller Error: NO ERROR "},
00539 {" Rhino Controller Error: NO ERROR "},
00540 {" Rhino Controller Error: NO ERROR "},
00541 {" Rhino Controller Error: NO ERROR "},
00542 {" Rhino Controller Error: Hard home routine failed. "},
00543 {" Rhino Controller Error: Hard home not set. "},
00544 {" Rhino Controller Error: Soft home not set. "},
00545 {" Rhino Controller Error: Arithmetic overflow. "},
00546 {" Rhino Controller Error: Trig function returned error. "},
00547 {" Rhino Controller Error: Error stack overflow. "},
00548 {" Rhino Controller Error: Still executing a trapezoidal move. "},
00549 {" Rhino Controller Error: Inactive motor referenced. "},
00550 {" Rhino Controller Error: Insufficient move data (velocity or acceleration = 0). "},
00551 {" Rhino Controller Error: Improper motor mode for command. "},
00552 {" Rhino Controller Error: Limit switch A not found. "},
00553 {" Rhino Controller Error: Limit switch B not found. "},
00554 {" Rhino Controller Error: Limit switch C not found. "},
00555 {" Rhino Controller Error: Limit switch D not found. "},
00556 {" Rhino Controller Error: Limit switch E not found. "},
00557 {" Rhino Controller Error: Limit switch F not found. "},
00558 {" Rhino Controller Error: Limit switch G not found. "},
00559 {" Rhino Controller Error: Limit switch H not found. "},
00560 {" Rhino Controller Error: NO ERROR "},
00561 {" Rhino Controller Error: Interpolation move out of bounds. "},
00562 {" Rhino Controller Error: xvz position out of bounds. "},
00563 {" Rhino Controller Error: NO ERROR "},
00564 {" Rhino Controller Error: NO ERROR "},
00565 {" Rhino Controller Error: NO ERROR "},
00566 {" Rhino Controller Error: NO ERROR "},
00567 {" Rhino Controller Error: NO ERROR "},
00568 {" Rhino Controller Error: NO ERROR "},
00569 {" Rhino Controller Error: NO ERROR "},
00570 {" Rhino Controller Error: NO ERROR "},
00571 {" Rhino Controller Error: NO ERROR "},
00572 {" Rhino Controller Error: Limit switch A stuck. "},
00573 {" Rhino Controller Error: Limit switch B stuck. "},
00574 {" Rhino Controller Error: Limit switch C stuck. "},
00575 {" Rhino Controller Error: Limit switch D stuck. "},
00576 {" Rhino Controller Error: Lim it switch E stuck. "},
00577 {" Rhino Controller Error: Limit switch F stuck. "},
00578 {" Rhino Controller Error: Lim it switch G stuck. "},
00579 {" Rhino Controller Error: Lim it switch H stuck. "},
00580 {" Rhino Controller Error: NO ERROR "},
00581 {" Rhino Controller Error: NO ERROR "},
00582 {" Rhino Controller Error: NO ERROR "},
00583 {" Rhino Controller Error: Invalid robot type specifier. "},
00584 {" Rhino Controller Error: Invalid motor specifier. "},
00585 {" Rhino Controller Error: NO ERROR "},
00586 {" Rhino Controller Error: Invalid pendant mode. "},
00587 {" Rhino Controller Error: NO ERROR "},
00588 {" Rhino Controller Error: Invalid xyz specifier. "},
00589 {" Rhino Controller Error: Invalid xyz parameter. "},
00590 {" Rhino Controller Error: NO ERROR "},
00591 {" Rhino Controller Error: NO ERROR "},
00592 {" Rhino Controller Error: Motor A stalled. "},
00593 {" Rhino Controller Error: Motor B stalled. "},
00594 {" Rhino Controller Error: Motor C stalled. "},
00595 {" Rhino Controller Error: Motor D stalled. "},
00596 {" Rhino Controller Error: Motor E stalled. "},
00597 {" Rhino Controller Error: Motor F stalled. "},
00598 {" Rhino Controller Error: Motor G stalled. "},
00599 {" Rhino Controller Error: Motor H stalled. "},
00600 {" Rhino Controller Error: NO ERROR "},
00601 {" Rhino Controller Error: NO ERROR "},
00602 {" Rhino Controller Error: Motor A's current limit circuit was activated. "},
00603 {" Rhino Controller Error: Motor B's current limit circuit was activated. "},
00604 {" Rhino Controller Error: Motor C's current limit circuit was activated. "},
00605 {" Rhino Controller Error: Motor D's current limit circuit was activated. "},
00606 {" Rhino Controller Error: Motor E's current limit circuit was activated. "},
00607 {" Rhino Controller Error: Motor F's current limit circuit was activated. "},
00608 {" Rhino Controller Error: Motor G's current limit circuit was activated. "},
00609 {" Rhino Controller Error: Motor H's current limit circuit was activated. "},
00610 {" Rhino Controller Error: Aux Port 1's current limit circuit was activated. "},
00611 {" Rhino Controller Error: Aux Port 2's current limit circuit was activated. "},
00612 {" Rhino Controller Error: NO ERROR "},
00613 {" Rhino Controller Error: NO ERROR "},
00614 {" Rhino Controller Error: NO ERROR "},
00615 {" Rhino Controller Error: NO ERROR "},
00616 {" Rhino Controller Error: NO ERROR "},
00617 {" Rhino Controller Error: NO ERROR "},
00618 {" Rhino Controller Error: NO ERROR "},
00619 {" Rhino Controller Error: NO ERROR "}
00620 };
00621
00622 const char RhinoClassErrorMsgSOURCE[MAX_RC_ERROR_CODES][MSG_LENGTH]={
00623 {" Rhino Class Error: BE HAPY, NO PROBLEM "},
00624 {" Rhino Class Error: 'Error Code' fuera de rango "},
00625 {" Rhino Class Error: 'Log Level' fuera de rango "},
00626 {" Rhino Class Error: No se pudo crear el archivo de registro "},
00627 {" Rhino Class Error: NO ERROR "},
00628 {" Rhino Class Error: NO ERROR "},
00629 {" Rhino Class Error: NO ERROR "},
00630 {" Rhino Class Error: NO ERROR "},
00631 {" Rhino Class Error: NO ERROR "},
00632 {" Rhino Class Error: NO ERROR "},
00633 {" Rhino Class Error: NO ERROR "},
00634 {" Rhino Class Error: NO ERROR "},
00635 {" Rhino Class Error: NO ERROR "},
00636 {" Rhino Class Error: NO ERROR "},
00637 {" Rhino Class Error: NO ERROR "},
00638 {" Rhino Class Error: NO ERROR "},
00639 {" Rhino Class Error: NO ERROR "},
00640 {" Rhino Class Error: NO ERROR "},
00641 {" Rhino Class Error: NO ERROR "},
00642 {" Rhino Class Error: NO ERROR "},
00643 {" Rhino Class Error: NO ERROR "},
00644 {" Rhino Class Error: NO ERROR "},
00645 {" Rhino Class Error: NO ERROR "},
00646 {" Rhino Class Error: NO ERROR "},
00647 {" Rhino Class Error: NO ERROR "},
00648 {" Rhino Class Error: NO ERROR "},
00649 {" Rhino Class Error: NO ERROR "},
00650 {" Rhino Class Error: NO ERROR "},
00651 {" Rhino Class Error: NO ERROR "},
00652 {" Rhino Class Error: NO ERROR "},
00653 {" Rhino Class Error: NO ERROR "},
00654 {" Rhino Class Error: NO ERROR "}
00655 };
00656
00657 for(int i=0; i<MAX_RB_ERROR_CODES; i++)
00658 strcpy(_RhinoBotErrorMsg[i], RhinoBotErrorMsgSOURCE[i]);
00659
00660 for(int i=0; i<MAX_RC_ERROR_CODES; i++)
00661 strcpy(_RhinoClassErrorMsg[i], RhinoClassErrorMsgSOURCE[i]);
00662
00663
00664 const char RhinoBotStatusMsgSOURCE[MAX_RB_STATUS_CODES][MSG_LENGTH]={
00665 {" Rhino Controller Status: BE HAPY, NO PROBLEM "},
00666 {" Rhino Controller Status: At least one motor is performing a trapezoidal move"},
00667 {" Rhino Controller Status: A system error has ocurred"},
00668 {" Rhino Controller Status: The general purpose delay timer is active "},
00669 {" Rhino Controller Status: At least one wait on input or wait on switch is still pending "},
00670 {" Rhino Controller Status: No teach pedant is connected "},
00671 {" Rhino Controller Status: The teach pedant ENTER key hast been pressed "},
00672 {" Rhino Controller Status: The teach pedant ESCAPER key hast been pressed "},
00673 {" Rhino Controller Status: A teach pedant error has ocurred "},
00674 {" Rhino Controller Status: Motor A is executing a trapezoidal move "},
00675 {" Rhino Controller Status: Motor B is executing a trapezoidal move "},
00676 {" Rhino Controller Status: Motor C is executing a trapezoidal move "},
00677 {" Rhino Controller Status: Motor D is executing a trapezoidal move "},
00678 {" Rhino Controller Status: Motor E is executing a trapezoidal move "},
00679 {" Rhino Controller Status: Motor F is executing a trapezoidal move "},
00680 {" Rhino Controller Status: Motor G is executing a trapezoidal move "},
00681 {" Rhino Controller Status: Motor H is executing a trapezoidal move "},
00682 {" Rhino Controller Status: The motor is either stationary or in a mode other than trapezoidal "},
00683 {" Rhino Controller Status: NC "}
00684 };
00685 const char RhinoClassStatusMsgSOURCE[MAX_RC_STATUS_CODES][MSG_LENGTH]={
00686 {" Rhino Class Status: BE HAPY, NO PROBLEM "},
00687 {" Rhino Class Status: Se asume LogLevel 3 por defecto "},
00688 {" Rhino Class Status: Depuracion de comando pendiente "},
00689 {" Rhino Class Status: NC "}
00690 };
00691
00692 for(int i=0; i<MAX_RB_STATUS_CODES; i++)
00693 strcpy(_RhinoBotStatusMsg[i], RhinoBotStatusMsgSOURCE[i]);
00694
00695 for(int i=0; i<MAX_RC_STATUS_CODES; i++)
00696 strcpy(_RhinoClassStatusMsg[i], RhinoClassStatusMsgSOURCE[i]);
00697
00698 return 0;
00699 }
00700
00701
00702
00721
00722 int Rhino::ProcessCommandToRhino(char * _sCommand)
00723 {
00724 if(!strcmp(_sCommand, "SA") || !strcmp(_sCommand, "sa"))
00725 LastCmdAndMsg.uiLastCommandtoRhino = SA;
00726
00727
00728 else if(!strcmp(_sCommand, "SE") || !strcmp(_sCommand, "se"))
00729 LastCmdAndMsg.uiLastCommandtoRhino = SE;
00730
00731
00732
00733
00734 else if(!strcmp(_sCommand, "SS") || !strcmp(_sCommand, "ss"))
00735 LastCmdAndMsg.uiLastCommandtoRhino = SS;
00736
00737
00738
00739
00740
00741
00742
00743
00744 else
00745 LastCmdAndMsg.uiLastCommandtoRhino = NC;
00746
00747 strcpy(LastCmdAndMsg.LastCommandtoRhino, _sCommand);
00748 strcpy(LastCmdAndMsg.LastMessageFromRhino, "");
00749
00750 return 0;
00751
00752 }
00753
00754
00755
00776
00777 int Rhino::ProcessCommandToRhino(char * _sCommand, unsigned int *_iCmdCode)
00778 {
00779 if(!strcmp(_sCommand, "SA") || !strcmp(_sCommand, "sa")){
00780 LastCmdAndMsg.uiLastCommandtoRhino = SA;
00781 *_iCmdCode = SA;
00782 }
00783
00784
00785 else if(!strcmp(_sCommand, "SE") || !strcmp(_sCommand, "se")){
00786 LastCmdAndMsg.uiLastCommandtoRhino = SE;
00787 *_iCmdCode = SE;
00788 }
00789
00790
00791
00792
00793 else if(!strcmp(_sCommand, "SS") || !strcmp(_sCommand, "ss")){
00794 LastCmdAndMsg.uiLastCommandtoRhino = SS;
00795 *_iCmdCode = SS;
00796 }
00797
00798
00799
00800
00801
00802
00803
00804
00805 else{
00806 LastCmdAndMsg.uiLastCommandtoRhino = NC;
00807 *_iCmdCode = NC;
00808 }
00809
00810 strcpy(LastCmdAndMsg.LastCommandtoRhino, _sCommand);
00811 strcpy(LastCmdAndMsg.LastMessageFromRhino, "");
00812
00813 return 0;
00814
00815 }
00816
00817
00818
00835
00836 int Rhino::ProcessMessageFromRhino(char * _StringCode)
00837 {
00838 strcpy(LastCmdAndMsg.LastMessageFromRhino, _StringCode);
00839
00840 if(LastCmdAndMsg.uiLastCommandtoRhino != NC){
00841
00842 unsigned int _uiCode = atoi(_StringCode);
00843
00844 switch(LastCmdAndMsg.uiLastCommandtoRhino)
00845 {
00846 case SS:
00847 uiStatusCode = _uiCode;
00848
00849 if(_uiCode & 128){
00850 ProcessStatusMsg(RB_MOTOR_MOVE);
00851 }
00852 if(_uiCode & 64){
00853 ProcessStatusMsg(RB_ERROR);
00854 }
00855 if(_uiCode & 32){
00856 ProcessStatusMsg(RB_DTIMER_ACT);
00857 }
00858 if(_uiCode & 16){
00859 ProcessStatusMsg(RB_WAIT_PENDING);
00860 }
00861 if(_uiCode & 8){
00862 ProcessStatusMsg(RB_NO_TP);
00863 }
00864 if(_uiCode & 4){
00865 ProcessStatusMsg(RB_TP_ENTER);
00866 }
00867 if(_uiCode & 2){
00868 ProcessStatusMsg(RB_TP_ESC);
00869 }
00870 if(_uiCode & 1){
00871 ProcessStatusMsg(RB_TP_ERROR);
00872 }
00873 if(_uiCode == 0){
00874 ProcessStatusMsg(RB_STS_NS);
00875 }
00876 break;
00877
00878 case SA:
00879 uiStatusCode = _uiCode;
00880
00881 if(_uiCode & 128){
00882 ProcessStatusMsg(RB_MOTOR_H_TMON);
00883 }
00884 if(_uiCode & 64){
00885 ProcessStatusMsg(RB_MOTOR_G_TMON);
00886 }
00887 if(_uiCode & 32){
00888 ProcessStatusMsg(RB_MOTOR_F_TMON);
00889 }
00890 if(_uiCode & 16){
00891 ProcessStatusMsg(RB_MOTOR_E_TMON);
00892 }
00893 if(_uiCode & 8){
00894 ProcessStatusMsg(RB_MOTOR_D_TMON);
00895 }
00896 if(_uiCode & 4){
00897 ProcessStatusMsg(RB_MOTOR_C_TMON);
00898 }
00899 if(_uiCode & 2){
00900 ProcessStatusMsg(RB_MOTOR_B_TMON);
00901 }
00902 if(_uiCode & 1){
00903 ProcessStatusMsg(RB_MOTOR_A_TMON);
00904 }
00905 if(_uiCode == 0){
00906 ProcessStatusMsg(RB_MOTORS_NOTM);
00907 }
00908 break;
00909
00910 case SE:
00911 uiErrorCode = _uiCode;
00912 ProcessErrorMsg(_uiCode);
00913 break;
00914
00915 default:
00916 ProcessStatusMsg(RC_DEP_CMD_PENDING);
00917 break;
00918 }
00919 return 0;
00920 }
00921
00922 ProcessStatusMsg(RC_DEP_CMD_PENDING);
00923 return 0;
00924
00925 }
00926
00927
00928
00944
00946
00955
00956 int Rhino::ProcessMessageFromRhino(unsigned int _iStatusCmd, char * _StringCode, unsigned int *_iCode, char * _sMessage)
00957 {
00958 strcpy(LastCmdAndMsg.LastMessageFromRhino, _StringCode);
00959
00960 if(LastCmdAndMsg.uiLastCommandtoRhino != NC){
00961
00962 *_iCode = atoi(_StringCode);
00963
00964 unsigned int uiCode = *_iCode;
00965
00966 switch(_iStatusCmd)
00967 {
00968 case SS:
00969 uiStatusCode = uiCode;
00970
00971 if(uiCode & 128){
00972 ProcessStatusMsg(RB_MOTOR_MOVE);
00973 get_StatusMsg(_sMessage);
00974 }
00975 if(uiCode & 64){
00976 ProcessStatusMsg(RB_ERROR);
00977 get_StatusMsg(_sMessage);
00978 }
00979 if(uiCode & 32){
00980 ProcessStatusMsg(RB_DTIMER_ACT);
00981 get_StatusMsg(_sMessage);
00982 }
00983 if(uiCode & 16){
00984 ProcessStatusMsg(RB_WAIT_PENDING);
00985 get_StatusMsg(_sMessage);
00986 }
00987 if(uiCode & 8){
00988 ProcessStatusMsg(RB_NO_TP);
00989 get_StatusMsg(_sMessage);
00990 }
00991 if(uiCode & 4){
00992 ProcessStatusMsg(RB_TP_ENTER);
00993 get_StatusMsg(_sMessage);
00994 }
00995 if(uiCode & 2){
00996 ProcessStatusMsg(RB_TP_ESC);
00997 get_StatusMsg(_sMessage);
00998 }
00999 if(uiCode & 1){
01000 ProcessStatusMsg(RB_TP_ERROR);
01001 get_StatusMsg(_sMessage);
01002 }
01003 if(uiCode == 0){
01004 ProcessStatusMsg(RB_STS_NS);
01005 get_StatusMsg(_sMessage);
01006 }
01007 break;
01008
01009 case SE:
01010 uiErrorCode = uiCode;
01011 ProcessErrorMsg(uiCode);
01012 get_ErrorMsg(_sMessage);
01013 break;
01014
01015 case SA:
01016 uiStatusCode = uiCode;
01017
01018 if(uiCode & 128){
01019 ProcessStatusMsg(RB_MOTOR_H_TMON);
01020 get_StatusMsg(_sMessage);
01021 }
01022 if(uiCode & 64){
01023 ProcessStatusMsg(RB_MOTOR_G_TMON);
01024 get_StatusMsg(_sMessage);
01025 }
01026 if(uiCode & 32){
01027 ProcessStatusMsg(RB_MOTOR_F_TMON);
01028 get_StatusMsg(_sMessage);
01029 }
01030 if(uiCode & 16){
01031 ProcessStatusMsg(RB_MOTOR_E_TMON);
01032 get_StatusMsg(_sMessage);
01033 }
01034 if(uiCode & 8){
01035 ProcessStatusMsg(RB_MOTOR_D_TMON);
01036 get_StatusMsg(_sMessage);
01037 }
01038 if(uiCode & 4){
01039 ProcessStatusMsg(RB_MOTOR_C_TMON);
01040 get_StatusMsg(_sMessage);
01041 }
01042 if(uiCode & 2){
01043 ProcessStatusMsg(RB_MOTOR_B_TMON);
01044 get_StatusMsg(_sMessage);
01045 }
01046 if(uiCode & 1){
01047 ProcessStatusMsg(RB_MOTOR_A_TMON);
01048 get_StatusMsg(_sMessage);
01049 }
01050 if(uiCode == 0){
01051 ProcessStatusMsg(RB_MOTORS_NOTM);
01052 get_StatusMsg(_sMessage);
01053 }
01054 break;
01055
01056 default:
01057 ProcessStatusMsg(RC_DEP_CMD_PENDING);
01058 break;
01059 }
01060 return 0;
01061 }
01062
01063 ProcessStatusMsg(RC_DEP_CMD_PENDING);
01064 return 0;
01065
01066 }
01067
01068
01069
01080
01081 int Rhino::get_StatusMsg(char *_cTextStatus)
01082 {
01083
01084 if(uiStatusCode >= 0 && uiStatusCode <= MAX_RB_STATUS_CODES)
01085 {
01086 strcpy(_cTextStatus, RhinoBotStatusMsg[uiStatusCode]);
01087 return 0;
01088 }
01089
01090
01091 if(uiStatusCode > MAX_RB_STATUS_CODES && uiStatusCode <= (MAX_RB_STATUS_CODES + MAX_RC_STATUS_CODES))
01092 {
01093 strcpy(_cTextStatus, RhinoClassStatusMsg[uiStatusCode-MAX_RB_STATUS_CODES]);
01094 return 0;
01095 }
01096
01097 ProcessErrorMsg(ERR_CODE);
01098 return -1;
01099 }
01100
01101
01102
01115
01116 int Rhino::get_StatusMsg(unsigned int _uiStatusCode, char *_cTextStatus)
01117 {
01118
01119 if(_uiStatusCode >= 0 && _uiStatusCode <= MAX_RB_STATUS_CODES)
01120 {
01121 strcpy(_cTextStatus, RhinoBotStatusMsg[_uiStatusCode]);
01122 return 0;
01123 }
01124
01125
01126 if(_uiStatusCode > MAX_RB_STATUS_CODES && _uiStatusCode <= (MAX_RB_STATUS_CODES + MAX_RC_STATUS_CODES))
01127 {
01128 strcpy(_cTextStatus, RhinoClassStatusMsg[_uiStatusCode-MAX_RB_STATUS_CODES]);
01129 return 0;
01130 }
01131
01132 ProcessErrorMsg(ERR_CODE);
01133 return -1;
01134 }
01135
01146
01147 int Rhino::get_StatusCode(unsigned int *_uiStatusCode)
01148 {
01149 *_uiStatusCode = uiStatusCode;
01150 return 0;
01151 }