00001
00002
00004
00005 #include "BHandSerialDriver.h"
00006 #include "BHandSupervisoryRealTime.h"
00007 #include "BHandHardware.h"
00008
00009 #include "bhand_motors.h"
00010 #include "bhand_misc.h"
00011
00012 #include "BHand.h"
00013
00014
00015 #include "ctb-0.14/ctb.h"
00016
00017 #include <math.h>
00018
00019
00020
00021
00022 const static char comPortAsString[9][50] =
00023 { wxCOM1, wxCOM2, wxCOM3, wxCOM4, wxCOM5, wxCOM6, wxCOM7, wxCOM8, wxCOM9 };
00024
00025 const static char usbPortAsString[10][50] =
00026 { "/dev/ttyUSB0", "/dev/ttyUSB1", "/dev/ttyUSB2", "/dev/ttyUSB3", "/dev/ttyUSB4",
00027 "/dev/ttyUSB5", "/dev/ttyUSB6", "/dev/ttyUSB7", "/dev/ttyUSB8", "/dev/ttyUSB9"};
00028
00029 int BHandSerialDriver::defBaud = 9600;
00030
00031
00032 BHand* _BHandArray[BH_MAXPORT] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
00033
00034
00035
00036 void BHandSerialDriver::SetWaitCallbackFunc(BHCallback waitCallbackFunc)
00037 {
00038
00039 }
00040
00041
00042
00044
00046
00047 BHandSerialDriver::BHandSerialDriver(BHand *bhand, BHandSupervisoryRealtime *moduleSuperReal, unsigned int comport) :
00048 BHandDriver(bhand, moduleSuperReal, BH_SERIAL_COMMUNICATION),
00049 dev(NULL)
00050 {
00051 comBaud = defBaud;
00052 comPort = comport;
00053
00054
00055
00056
00057
00058 }
00059
00060 BHandSerialDriver::~BHandSerialDriver()
00061 {
00062 }
00063
00064
00065
00066
00068
00070
00071 int BHandSerialDriver::Initialize()
00072 {
00073 if (m_open)
00074 return BHERR_OPENCOMMPORT;
00075
00076
00077 int result;
00078 if ((result = ComInitialize(comPort)))
00079 return result;
00080
00081
00082 m_open = true;
00083
00084
00085
00086 return 0;
00087 }
00088
00089 void BHandSerialDriver::Close()
00090 {
00091 ComClose();
00092 m_open = false;
00093 }
00094
00095
00097
00099
00100 int BHandSerialDriver::HandInit(BHMotors bhMotors)
00101 {
00102
00103 char motor[5];
00104 toMotorChar(bhMotors, motor);
00105 sprintf(outbuf, "%sHI\r", motor);
00106 nout = strlen(outbuf);
00107
00108 return HandleSupervisoryCall();
00109 }
00110 int BHandSerialDriver::HandReset(BHMotors bhMotors, bool *responseReceived)
00111 {
00112
00113 nout = 1;
00114 outbuf[0] = '\r';
00115
00116
00117 int result = HandleSupervisoryCall();
00118
00119
00120
00121 if (result != 0)
00122 return result;
00123
00124
00125
00126
00127 sprintf(outbuf, "RESET\r");
00128 nout = strlen(outbuf);
00129 requestBaud = defBaud;
00130
00131
00132 result = HandleSupervisoryCall();
00133
00134
00135
00136
00137 if (responseReceived)
00138 *responseReceived = searchstrn(inbuf, "BarrettHand Firmware\n", nin);
00139
00140 return result;
00141 }
00142
00143 char * BHandSerialDriver::HandCommand(const char *send, int *errorCode)
00144 {
00145
00146 if ((nout = strlen(send) + 1) >= BH_MAXCHAR)
00147 {
00148 *errorCode = BHERR_LONGSTRING;
00149 return NULL;
00150 }
00151 strcpy(outbuf, send);
00152 strcat(outbuf, "\r");
00153
00154
00155 *errorCode = HandleSupervisoryCall();
00156
00157
00158 if (*errorCode)
00159 inbuf[0] = 0;
00160
00161 return inbuf;
00162 }
00163
00164 int BHandSerialDriver::HandClose(BHMotors bhMotors)
00165 {
00166
00167 char motor[5];
00168 toMotorChar(bhMotors, motor);
00169 sprintf(outbuf, "%sC\r", motor);
00170 nout = strlen(outbuf);
00171
00172
00173 return HandleSupervisoryCall();
00174 }
00175 int BHandSerialDriver::HandOpen(BHMotors bhMotors)
00176 {
00177
00178 char motor[5];
00179 toMotorChar(bhMotors, motor);
00180 sprintf(outbuf, "%sO\r", motor);
00181 nout = strlen(outbuf);
00182
00183
00184 return HandleSupervisoryCall();
00185 }
00186
00187 int BHandSerialDriver::HandGoToDefault(BHMotors bhMotors, bool valueIncluded, int defaultPosition)
00188 {
00189
00190 char motor[5];
00191 toMotorChar(bhMotors, motor);
00192 if (valueIncluded)
00193 sprintf(outbuf, "%sM %d\r", motor, defaultPosition);
00194 else
00195 sprintf(outbuf, "%sM\r", motor);
00196 nout = strlen(outbuf);
00197
00198
00199 return HandleSupervisoryCall();
00200 }
00201
00202 int BHandSerialDriver::HandGoToDifferentPositionsHand(const int *encoderPositions, unsigned int numEncoderPositions)
00203 {
00204 if (numEncoderPositions != 4)
00205 return BHERR_OUTOFRANGE;
00206
00207
00208 int result;
00209 for (unsigned int i = 0; i < numEncoderPositions; i++)
00210 if ((result = HandSet(1 << i, "DP", encoderPositions[i])))
00211 return result;
00212
00213 return HandGoToDefault(15);
00214 }
00215
00216 int BHandSerialDriver::HandGoToHome(BHMotors bhMotors)
00217 {
00218
00219 char motor[5];
00220 toMotorChar(bhMotors, motor);
00221
00222 int result;
00223
00224 if (ContainsAnyFingers(motor))
00225 {
00226 sprintf(outbuf, "GHOME\r");
00227 nout = strlen(outbuf);
00228
00229
00230 result = HandleSupervisoryCall();
00231
00232 if (result || !ContainsSpread(motor))
00233 return result;
00234 }
00235
00236
00237
00238 sprintf(outbuf, "SHOME\r");
00239 nout = strlen(outbuf);
00240
00241
00242 return HandleSupervisoryCall();
00243 }
00244
00245 int BHandSerialDriver::HandGoToPosition(BHMotors bhMotors, unsigned int encoderPositionTickCount)
00246 {
00247
00248 char motor[5];
00249 toMotorChar(bhMotors, motor);
00250
00251
00252 sprintf(outbuf, "%sM %d\r", motor, encoderPositionTickCount);
00253 nout = strlen(outbuf);
00254
00255
00256 return HandleSupervisoryCall();
00257 }
00258
00259 int BHandSerialDriver::HandStepClose(BHMotors bhMotors, bool valueIncluded, int stepAmount)
00260 {
00261
00262 char motor[5];
00263 toMotorChar(bhMotors, motor);
00264 if (valueIncluded)
00265 sprintf(outbuf, "%sIC %d\r", motor, stepAmount);
00266 else
00267 sprintf(outbuf, "%sIC\r", motor);
00268 nout = strlen(outbuf);
00269
00270
00271 return HandleSupervisoryCall();
00272 }
00273
00274 int BHandSerialDriver::HandStepOpen(BHMotors bhMotors, bool valueIncluded, int stepAmount)
00275 {
00276
00277 char motor[5];
00278 toMotorChar(bhMotors, motor);
00279 if (valueIncluded)
00280 sprintf(outbuf, "%sIO %d\r", motor, stepAmount);
00281 else
00282 sprintf(outbuf, "%sIO\r", motor);
00283 nout = strlen(outbuf);
00284
00285
00286 return HandleSupervisoryCall();
00287 }
00288
00289 int BHandSerialDriver::HandStopMotor(BHMotors bhMotors)
00290 {
00291
00292 char motor[5];
00293 toMotorChar(bhMotors, motor);
00294 sprintf(outbuf, "%sT\r", motor);
00295 nout = strlen(outbuf);
00296
00297
00298 return HandleSupervisoryCall();
00299 }
00300
00301 int BHandSerialDriver::HandTorqueClose(BHMotors bhMotors)
00302 {
00303
00304 char motor[5];
00305 toMotorChar(bhMotors, motor);
00306 sprintf(outbuf, "%sTC\r", motor);
00307 nout = strlen(outbuf);
00308
00309
00310 return HandleSupervisoryCall();
00311 }
00312
00313 int BHandSerialDriver::HandTorqueOpen(BHMotors bhMotors)
00314 {
00315
00316 char motor[5];
00317 toMotorChar(bhMotors, motor);
00318 sprintf(outbuf, "%sTO\r", motor);
00319 nout = strlen(outbuf);
00320
00321
00322 return HandleSupervisoryCall();
00323 }
00324
00325 int BHandSerialDriver::HandGet(BHMotors bhMotors, const char *property, int *propertyResult, int *nresults)
00326 {
00327
00328 char motor[5];
00329 toMotorChar(bhMotors, motor);
00330 sprintf(outbuf, "%sFGET %s\r", motor, property);
00331 nout = strlen(outbuf);
00332
00333
00334 int ret = HandleSupervisoryCall();
00335 if (ret)
00336 return ret;
00337
00338
00339 int sz = strlen(Response()), pos = 0, cnt = 0;
00340
00341
00342 if (sz >= 3 && Response()[0] == 'E' && Response()[1] == 'R' && Response()[2] == 'R')
00343 return BHERR_BADRESPONSE;
00344
00345 while (pos < sz)
00346 {
00347
00348 while (isspacechar(Response()[pos]) && pos < sz)
00349 pos++;
00350
00351
00352 if (pos < sz)
00353 sscanf(Response()+pos, "%d", propertyResult+cnt++);
00354
00355
00356 while ((isdigit(Response()[pos]) || Response()[pos] == '-') && pos < sz)
00357 pos++;
00358 }
00359
00360 *nresults = cnt;
00361
00362 return 0;
00363 }
00364
00365 int BHandSerialDriver::HandSet(BHMotors bhMotors, const char *property, int value)
00366 {
00367
00368 char motor[5];
00369 toMotorChar(bhMotors, motor);
00370 sprintf(outbuf, "%sFSET %s %d\r", motor, property, value);
00371 nout = strlen(outbuf);
00372
00373
00374 return HandleSupervisoryCall();
00375 }
00376
00377 int BHandSerialDriver::HandPGet(const char *property, int *propertyResult)
00378 {
00379
00380
00381
00382 sprintf(outbuf, "PGET %s\r", property);
00383 nout = strlen(outbuf);
00384
00385
00386 int ret = HandleSupervisoryCall();
00387 if (ret)
00388 return ret;
00389
00390
00391 int sz = strlen(Response()), pos = 0, cnt = 0;
00392
00393
00394 if (sz >= 3 && Response()[0] == 'E' && Response()[1] == 'R' && Response()[2] == 'R')
00395 return BHERR_BADRESPONSE;
00396
00397 while (pos < sz)
00398 {
00399
00400 while (isspacechar(Response()[pos]) && pos < sz)
00401 pos++;
00402
00403
00404 if (pos < sz)
00405 sscanf(Response() + pos, "%d", propertyResult + cnt++);
00406
00407
00408 while ((isdigit(Response()[pos]) || Response()[pos] == '-') && pos < sz)
00409 pos++;
00410 }
00411
00412 return 0;
00413 }
00414
00415 int BHandSerialDriver::HandPSet(const char *property, int value)
00416 {
00417
00418 sprintf(outbuf, "PSET %s %d\r", property, value);
00419 nout = strlen(outbuf);
00420
00421
00422 return HandleSupervisoryCall();
00423 }
00424
00425 int BHandSerialDriver::HandDefault(BHMotors bhMotors)
00426 {
00427
00428 char motor[5];
00429 toMotorChar(bhMotors, motor);
00430 sprintf(outbuf, "%sFDEF\r", motor);
00431 nout = strlen(outbuf);
00432
00433
00434 return HandleSupervisoryCall();
00435 }
00436
00437 int BHandSerialDriver::HandLoad(BHMotors bhMotors)
00438 {
00439
00440 char motor[5];
00441 toMotorChar(bhMotors, motor);
00442 sprintf(outbuf, "%sFLOAD\r", motor);
00443 nout = strlen(outbuf);
00444
00445
00446 return HandleSupervisoryCall();
00447 }
00448
00449 int BHandSerialDriver::HandSave(BHMotors bhMotors)
00450 {
00451
00452 char motor[5];
00453 toMotorChar(bhMotors, motor);
00454 sprintf(outbuf, "%sFSAVE\r", motor);
00455 nout = strlen(outbuf);
00456
00457
00458 return HandleSupervisoryCall();
00459 }
00460
00461 int BHandSerialDriver::HandTemperature(BHMotors bhMotors, int *temperature, unsigned int numTemperatures)
00462 {
00463
00464 char motor[5];
00465 toMotorChar(bhMotors, motor);
00466 sprintf(outbuf, "PGET TEMP\r");
00467 nout = strlen(outbuf);
00468
00469
00470 int ret = HandleSupervisoryCall();
00471
00472 sscanf(inbuf, "%d", temperature);
00473 *temperature = *temperature / 10;
00474
00475 return ret;
00476 }
00477
00478 int BHandSerialDriver::HandDelay(unsigned int msec)
00479 {
00480
00481 sprintf(outbuf, "DELAY %u\r", msec);
00482 nout = strlen(outbuf);
00483
00484
00485 return HandleSupervisoryCall();
00486 }
00487
00488 int BHandSerialDriver::HandBaud(unsigned int newbaud)
00489 {
00490
00491 sprintf(outbuf, "PSET BAUD %u\r", newbaud / 100);
00492 nout = strlen(outbuf);
00493 requestBaud = newbaud;
00494
00495
00496 return HandleSupervisoryCall();
00497 }
00498
00499 int BHandSerialDriver::HandleSupervisoryCall()
00500 {
00501 char dummy[BH_MAXCHAR];
00502 int result;
00503
00504
00505 outbuf[nout] = 0;
00506
00507
00508 sscanf(outbuf, "%s", dummy);
00509 if (!strcmp(dummy, "DELAY"))
00510 {
00511
00512 unsigned int msec;
00513 sscanf(outbuf+6, "%u", &msec);
00514 if (msec < 1)
00515 msec = 1;
00516 else if (msec > 100000)
00517 msec = 100000;
00518
00519 DELAY(msec);
00520
00521 inbuf[0] = 0;
00522
00523 return 0;
00524 }
00525
00526
00527 if (!ComClear(true))
00528 return BHERR_CLEARBUFFER;
00529
00530
00531 if (ComWrite(outbuf, nout))
00532 return BHERR_WRITECOM;
00533
00534
00535 if (nout > 1)
00536 {
00537 if (ComRead(dummy, nout - 1))
00538 return BHERR_READCOM;
00539 }
00540
00541
00542 nin = 0;
00543 inbuf[0] = 0;
00544
00545
00546 if (requestBaud)
00547 {
00548 if ((result = ComSetBaudrate(requestBaud)) != 0)
00549 return result;
00550
00551
00552 if (!strncmp(outbuf, "PSET BAUD", 9))
00553 {
00554 DELAY(1000);
00555
00556 if (!ComClear())
00557 {
00558
00559 return BHERR_CLEARBUFFER;
00560 }
00561 if (ComWrite("\r", 1))
00562 return BHERR_WRITECOM;
00563 }
00564
00565 requestBaud = 0;
00566 }
00567
00568
00569 if (!strcmp("LOOP\r", outbuf + MAX(0, nout - 5)))
00570 {
00571 if (ComRead(inbuf, 1))
00572 return BHERR_READCOM;
00573
00574 if (inbuf[0] != '*')
00575 {
00576 inbuf[0] = 0;
00577 nin = 0;
00578 }
00579 else
00580 return 0;
00581 }
00582
00583
00584 return Interactive();
00585 }
00586
00587 int BHandSerialDriver::Interactive()
00588 {
00589 int result;
00590
00591 _INTERACTIVE:
00592
00593 if (ComRead(inbuf + nin, 1))
00594 return BHERR_READCOM;
00595
00596
00597 if (nin == 0 && isspacechar(inbuf[0]))
00598 nin--;
00599
00600
00601 inbuf[++(nin)] = 0;
00602
00603
00604 if (nin < BH_MAXCHAR - 1 && strcmp("=> ", (inbuf + MAX(nin - 3, 0))))
00605 goto _INTERACTIVE;
00606
00607
00608
00609 if (nin >= BH_MAXCHAR - 1)
00610 return BHERR_BADRESPONSE;
00611
00612
00613 nin -= 3;
00614 inbuf[nin] = 0;
00615 while (nin > 0 && isspacechar(inbuf[nin - 1]))
00616 inbuf[--nin] = 0;
00617
00618
00619 if (!strncmp(inbuf, "ERR", 3))
00620 sscanf(inbuf + 4, "%d", &result);
00621 else
00622 result = 0;
00623
00624 return result;
00625 }
00626
00627 int BHandSerialDriver::ExecuteRealtimeCall()
00628 {
00629
00630
00631
00632 if (ComWrite(outbuf, nout))
00633 return BHERR_WRITECOM;
00634
00635
00636 nin = dev->Readv(&inbuf[0], nin, 10000);
00637
00638 if (nin < 0)
00639 {
00640
00641 return BHERR_READCOM;
00642 }
00643
00644 if (nin > 0 && inbuf[0] == '*')
00645 {
00646
00647
00648 memcpy(rtIn, &inbuf[1], nin - 1);
00649 return 0;
00650 }
00651 else
00652 {
00653
00654
00655
00656
00657 int numRead = dev->Readv(&inbuf[nin], 1024, 1000);
00658
00659
00660 if (numRead < 0)
00661 {
00662 return BHERR_READCOM;
00663 }
00664 nin += numRead;
00665
00666
00667 inbuf[nin] = '\0';
00668
00669
00670 char *errCode = strstr(inbuf, "\n\rERR ");
00671 if (errCode != NULL && strstr(inbuf, "\n\r=> ") != NULL)
00672 {
00673 return atoi(errCode + 6);
00674 }
00675 else
00676 {
00677 return BHERR_READCOM;
00678 }
00679 }
00680 }
00681
00682
00683
00684
00686
00688
00689 int BHandSerialDriver::ComInitialize(int comport)
00690 {
00691
00692 if (comport > 0)
00693 {
00694
00695 if (comport < 1 || comport > BH_MAXPORT)
00696 return BHERR_PORTOUTOFRANGE;
00697
00698
00699
00700
00701 }
00702 else if (comport < -BH_MAXPORT)
00703 return BHERR_PORTOUTOFRANGE;
00704
00705
00706 if (ComOpen(comport, defBaud))
00707 {
00708
00709 ComClose();
00710 return BHERR_OPENCOMMPORT;
00711 }
00712
00713
00714
00715
00716 if (comPort > 0)
00717 _BHandArray[comPort - 1] = m_bhand;
00718
00719 requestBaud = 0;
00720
00721 return 0;
00722 }
00723
00724 int BHandSerialDriver::ComOpen(int comport, int baudrate)
00725 {
00726
00727
00728 #ifdef LINUX
00729
00730
00731
00732
00733
00734
00735
00736 #endif
00737
00738 if (comport < -BH_MAXPORT || comport > BH_MAXPORT)
00739 return BHERR_PORTOUTOFRANGE;
00740
00741
00742 if (m_open)
00743 return BHERR_OPENCOMMPORT;
00744
00745
00746
00747
00748 comPort = comport;
00749
00750
00751
00752 dev = new wxSerialPort();
00753
00754
00755
00756
00757
00758
00759
00760 if ((comPort > 0 && dev->Open(comPortAsString[comPort - 1]) < 0) ||
00761 (comPort <= 0 && dev->Open(usbPortAsString[-comPort]) < 0))
00762 {
00763
00764 ComClose();
00765 return BHERR_OPENCOMMPORT;
00766 }
00767
00768
00769
00770
00771 if (ComSetBaudrate(baudrate))
00772 {
00773 ComClose();
00774 return BHERR_SETCOMMSTATE;
00775 }
00776
00777 int result;
00778 if ((result = ComSetTimeouts(50, 15000, 50, 5000)) > 0)
00779 return result;
00780
00781
00782
00783
00784 if (!ComClear(true))
00785 {
00786 ComClose();
00787 return BHERR_CLEARBUFFER;
00788 }
00789
00790
00791
00792
00793 m_open = true;
00794
00795 return 0;
00796 }
00797
00798 bool BHandSerialDriver::ComIsOpen()
00799 {
00800 return dev != NULL && m_open;
00801 }
00802
00803 void BHandSerialDriver::ComClose()
00804 {
00805 if (dev != NULL)
00806 {
00807 if (comPort > 0)
00808 _BHandArray[comPort - 1] = NULL;
00809 dev->Close();
00810 delete dev;
00811 dev = NULL;
00812 }
00813
00814 m_open = false;
00815 }
00816
00817
00818 int BHandSerialDriver::ComSetBaudrate(int baudrate)
00819 {
00820 if (dev == NULL)
00821 return BHERR_SETCOMMSTATE;
00822
00823 switch (baudrate)
00824 {
00825 case 600: { dev->SetBaudrate(wxBAUD_600); comBaud = 600; break; }
00826 case 1200: { dev->SetBaudrate(wxBAUD_1200); comBaud = 1200; break; }
00827 case 2400: { dev->SetBaudrate(wxBAUD_2400); comBaud = 2400; break; }
00828 case 4800: { dev->SetBaudrate(wxBAUD_4800); comBaud = 4800; break; }
00829 case 9600: { dev->SetBaudrate(wxBAUD_9600); comBaud = 9600; break; }
00830 case 19200: { dev->SetBaudrate(wxBAUD_19200); comBaud = 19200; break; }
00831 case 38400: { dev->SetBaudrate(wxBAUD_38400); comBaud = 38400; break; }
00832 default: { return BHERR_SETCOMMSTATE; }
00833 }
00834
00835 return 0;
00836 }
00837
00838 int BHandSerialDriver::ComSetTimeouts(unsigned int readMultiplier, unsigned int readConstant, unsigned int writeMultiplier, unsigned int writeConstant)
00839 {
00840 readTotalTimeoutMultiplier = readMultiplier;
00841 readTotalTimeoutConstant = readConstant;
00842 writeTotalTimeoutMultiplier = writeMultiplier;
00843 writeTotalTimeoutConstant = writeConstant;
00844
00845 return 0;
00846 }
00847
00848
00849 bool BHandSerialDriver::ComClear(bool rxOnly)
00850 {
00851
00852 if (!ComIsOpen())
00853 return true;
00854
00855 if (rxOnly)
00856 {
00857 int result;
00858 char temp[256];
00859
00860
00861 while ((result = dev->Read(temp, 256)) > 0) ;
00862
00863
00864 return (result == 0) ? true : false;
00865 }
00866 else
00867 {
00868
00869
00870
00871 dev->Close();
00872
00873
00874
00875
00876
00877
00878 bool notOpen =
00879 (comPort > 0 && dev->Open(comPortAsString[comPort - 1]) < 0) ||
00880 (comPort <= 0 && dev->Open(usbPortAsString[-comPort]) < 0 );
00881
00882
00883 if (notOpen || ComSetBaudrate(comBaud))
00884 return false;
00885 }
00886
00887 return true;
00888 }
00889
00890
00891 int BHandSerialDriver::ComRead(char* rxBuf, int rxNumBytes)
00892 {
00893 if (!ComIsOpen())
00894 {
00895 return BHERR_READCOM;
00896 }
00897
00898
00899 unsigned int readTotalTimeout = readTotalTimeoutMultiplier * rxNumBytes + readTotalTimeoutConstant;
00900 if (readTotalTimeout == 0)
00901 readTotalTimeout = wxTIMEOUT_INFINITY;
00902
00903 int numRead = dev->Readv(rxBuf, rxNumBytes, readTotalTimeout);
00904
00905
00906 return (rxNumBytes != numRead) ? BHERR_READCOM : 0;
00907 }
00908
00909 int BHandSerialDriver::ComWrite(const char* txBuf, int txNumBytes)
00910 {
00911 if (!ComIsOpen())
00912 return BHERR_WRITECOM;
00913
00914
00915 unsigned int writeTotalTimeout = writeTotalTimeoutMultiplier * txNumBytes + writeTotalTimeoutConstant;
00916 if (writeTotalTimeout == 0)
00917 writeTotalTimeout = wxTIMEOUT_INFINITY;
00918
00919 int numWritten = dev->Writev((char *)txBuf, txNumBytes, writeTotalTimeout);
00920
00921
00922
00923
00924
00925 return (txNumBytes != numWritten) ? BHERR_WRITECOM : 0;
00926 }
00927
00928 const char * BHandSerialDriver::Response()
00929 {
00930
00931 return (const char*)inbuf;
00932 }
00933
00934 const char * BHandSerialDriver::Buffer()
00935 {
00936
00937 return (const char*)outbuf;
00938 }
00939
00940
00941
00942
00944
00946
00947 int BHandSerialDriver::RTStart(const char *motor, BHMotorProtection motorProtection)
00948 {
00949
00950 rtFlags[0][0] = (strrchr(motor,'1') || strrchr(motor,'G'));
00951 rtFlags[1][0] = (strrchr(motor,'2') || strrchr(motor,'G'));
00952 rtFlags[2][0] = (strrchr(motor,'3') || strrchr(motor,'G'));
00953 rtFlags[3][0] = (strrchr(motor,'4') || strrchr(motor,'S'));
00954
00955
00956 for (int m = 0; m < 4; m++)
00957 {
00958 char motor[] = "1";
00959 motor[0] += m;
00960 if (m_bhand->Get(motor, "LCV LCPG LCT LFV LFS LFAP LFDP LFBP LFAIN", &rtFlags[m][1]))
00961 return BHERR_NOTINITIALIZED;
00962
00963
00964 if ((rtFlags[m][1] || rtFlags[m][2]) && rtFlags[m][3])
00965 return BHERR_NOTINITIALIZED;
00966 }
00967
00968 if (PGet("LFT", &rtGlobalFlags[0]))
00969 return BHERR_NOTINITIALIZED;
00970
00971
00972
00973 int myFeedbackPos = 0;
00974 int myControlPos = 0;
00975 for (int m = 0; m < 4; m++)
00976 {
00977
00978
00979
00980
00981 rtControl[m][0] = myControlPos; if (rtFlags[m][0]*rtFlags[m][1]) myControlPos++;
00982 rtControl[m][1] = myControlPos; if (rtFlags[m][0]*rtFlags[m][2]) myControlPos++;
00983 rtControl[m][2] = myControlPos; if (rtFlags[m][0]*rtFlags[m][3]) myControlPos+=2;
00984
00985
00986 rtFeedback[m][0] = myFeedbackPos; if (rtFlags[m][0]*rtFlags[m][4]) myFeedbackPos++;
00987 rtFeedback[m][1] = myFeedbackPos; if (rtFlags[m][0]*rtFlags[m][5]) myFeedbackPos++;
00988 rtFeedback[m][2] = myFeedbackPos; if (rtFlags[m][0]*rtFlags[m][6]) myFeedbackPos+=2;
00989 rtFeedback[m][3] = myFeedbackPos; if (rtFlags[m][0]*rtFlags[m][7]) myFeedbackPos++;
00990 rtFeedback[m][4] = myFeedbackPos; if (rtFlags[m][0]*rtFlags[m][8]) myFeedbackPos+=2;
00991 rtFeedback[m][5] = myFeedbackPos; if (rtFlags[m][0]*rtFlags[m][9]) myFeedbackPos++;
00992
00993
00994 RTSetVelocity('1'+m, 0);
00995 RTSetGain('1'+m, 0);
00996 RTSetTorque('1'+m, 0);
00997 }
00998
00999 rtGlobalFeedback[0] = myFeedbackPos; if (rtGlobalFlags[0]) myFeedbackPos+=1;
01000
01001
01002 memset(rtIn, 0, 4*8+1);
01003
01004
01005 nSend = myControlPos;
01006 nReceive = myFeedbackPos;
01007
01008
01009 sprintf(outbuf, "%sLOOP\r", motor);
01010 nout = strlen(outbuf);
01011
01012
01013 return m_moduleSuperReal->ComRequest(BHREQ_SUPERVISE);
01014 }
01015
01016
01017 int BHandSerialDriver::RTUpdate(bool control, bool feedback)
01018 {
01019
01020 if (control)
01021 {
01022 outbuf[0] = (feedback ? 'C' : 'c');
01023 memcpy(outbuf + 1, rtOut, nSend);
01024 nout = nSend + 1;
01025 }
01026 else
01027 {
01028 outbuf[0] = (feedback ? 'A' : 'a');
01029 nout = 1;
01030 }
01031
01032
01033 nin = (feedback ? 1 + nReceive : 1);
01034
01035 return m_moduleSuperReal->ComRequest(BHREQ_REALTIME);
01036 }
01037
01038 int BHandSerialDriver::RTAbort()
01039 {
01040
01041 outbuf[0] = 3;
01042 outbuf[1] = 0;
01043 nout = 1;
01044
01045
01046 return m_moduleSuperReal->ComRequest(BHREQ_SUPERVISE);
01047 }
01048
01049 int BHandSerialDriver::RTSetFlags(const char *motor, bool LCV, int LCVC, bool LCPG,
01050 bool LFV, int LFVC, bool LFS, bool LFAP, bool LFDP, int LFDPC)
01051 {
01052
01053 sprintf(outbuf, "%sFSET LCV %d LCVC %d LCPG %d LFV %d LFVC %d LFS %d LFAP %d LFDP %d LFDPC %d\r",
01054 motor, LCV, LCVC, LCPG, LFV, LFVC, LFS, LFAP, LFDP, LFDPC);
01055 nout = strlen(outbuf);
01056
01057
01058 return m_moduleSuperReal->ComRequest(BHREQ_SUPERVISE);
01059 }
01060
01061 int BHandSerialDriver::RTSetFlags(const char *motor, bool LCV, int LCVC, bool LCPG, bool LCT,
01062 bool LFV, int LFVC, bool LFS, bool LFAP, bool LFDP, int LFDPC,
01063 bool LFBP, bool LFAIN, bool LFDPD, bool LFT)
01064 {
01065 int err;
01066
01067
01068 sprintf(outbuf, "%sFSET LCV %d LCVC %d LCPG %d LCT %d LFV %d LFVC %d LFS %d LFAP %d LFDP %d LFDPC %d LFBP %d LFAIN %d\r",
01069 motor, LCV, LCVC, LCPG, LCT, LFV, LFVC, LFS, LFAP, LFDP, LFDPC, LFBP, LFAIN);
01070 nout = strlen( outbuf );
01071
01072
01073 err = m_moduleSuperReal->ComRequest(BHREQ_SUPERVISE);
01074
01075
01076 sprintf(outbuf, "PSET LFT %d LFDPD %d\r", LFT, LFDPD);
01077 nout = strlen(outbuf);
01078
01079
01080 err |= m_moduleSuperReal->ComRequest(BHREQ_SUPERVISE);
01081
01082 return err;
01083 }
01084
01085 int BHandSerialDriver::RTUpdate(const char *motor, const char *property, int *values)
01086 {
01087
01088 return BHERR_NOTCOMPLETED;
01089 }
01090
01091
01092
01093
01094
01095
01096 int BHandSerialDriver::RTSetVelocity(const char motor, int velocity)
01097 {
01098
01099 int m = motor - '1';
01100 if (m < 0)
01101 m = 0;
01102 else if (m > 3)
01103 m = 3;
01104
01105
01106 if (!rtFlags[m][0])
01107 return BHERR_MOTORINACTIVE;
01108
01109
01110 if (velocity > 127)
01111 velocity = 127;
01112 else if (velocity < -127)
01113 velocity = -127;
01114
01115
01116 rtOut[rtControl[m][0]] = velocity;
01117 return 0;
01118 }
01119
01120 int BHandSerialDriver::RTSetGain(const char motor, int gain)
01121 {
01122
01123 int m = motor - '1';
01124 if (m < 0)
01125 m = 0;
01126 else if (m > 3)
01127 m = 3;
01128
01129
01130 if (!rtFlags[m][0])
01131 return BHERR_MOTORINACTIVE;
01132
01133
01134 if (gain > 255)
01135 gain = 255;
01136 else if (gain < 0)
01137 gain = 0;
01138
01139
01140 rtOut[rtControl[m][1]] = (unsigned char)(gain & 0xFF);
01141 return 0;
01142 }
01143
01144 int BHandSerialDriver::RTSetTorque(const char motor, int torque)
01145 {
01146
01147 int m = motor - '1';
01148 if (m < 0)
01149 m = 0;
01150 else if (m > 3)
01151 m = 3;
01152
01153
01154 if (!rtFlags[m][0])
01155 return BHERR_MOTORINACTIVE;
01156
01157
01158 if (torque > 32767)
01159 torque = 32767;
01160 else if (torque < -32768)
01161 torque = -32768;
01162
01163
01164 rtOut[rtControl[m][2]] = (char)((torque >> 8) & 0x00FF);
01165 rtOut[rtControl[m][2]+1] = (char)((torque) & 0x00FF);
01166 return 0;
01167 }
01168
01169 int BHandSerialDriver::RTSetPosition(const char motor, int position)
01170 {
01171
01172 return 0;
01173 }
01174
01175 char BHandSerialDriver::RTGetVelocity(const char motor)
01176 {
01177
01178 int m = motor - '1';
01179 if (m < 0)
01180 m = 0;
01181 else if (m > 3)
01182 m = 3;
01183
01184
01185 if (!rtFlags[m][0] || !rtFlags[m][4])
01186 return 0;
01187 else
01188 return rtIn[rtFeedback[m][0]];
01189 }
01190
01191 unsigned char BHandSerialDriver::RTGetStrain(const char motor)
01192 {
01193
01194 int m = motor - '1';
01195 if (m < 0)
01196 m = 0;
01197 else if (m > 3)
01198 m = 3;
01199
01200
01201 if (!rtFlags[m][0] || !rtFlags[m][5])
01202 return 0;
01203 else
01204 return (unsigned char)rtIn[rtFeedback[m][1]];
01205 }
01206
01207 int BHandSerialDriver::RTGetPosition(const char motor)
01208 {
01209
01210 int m = motor - '1';
01211 if (m < 0)
01212 m = 0;
01213 else if (m > 3)
01214 m = 3;
01215
01216
01217 if (!rtFlags[m][0] || !rtFlags[m][6])
01218 return 0;
01219 else
01220 return ( (((int)rtIn[rtFeedback[m][2]])<<8) + (int)(unsigned char)rtIn[rtFeedback[m][2]+1] );
01221 }
01222
01223 char BHandSerialDriver::RTGetDeltaPos(const char motor)
01224 {
01225
01226 int m = motor - '1';
01227 if (m < 0)
01228 m = 0;
01229 else if (m > 3)
01230 m = 3;
01231
01232
01233 if (!rtFlags[m][0] || !rtFlags[m][7])
01234 return 0;
01235 else
01236 return rtIn[rtFeedback[m][3]];
01237 }
01238
01239 int BHandSerialDriver::RTGetBreakawayPosition(const char motor)
01240 {
01241
01242 int m = motor - '1';
01243 if (m < 0)
01244 m = 0;
01245 else if (m > 3)
01246 m = 3;
01247
01248
01249 if (!rtFlags[m][0] || !rtFlags[m][8])
01250 return 0;
01251 else
01252 return ( (((int)rtIn[rtFeedback[m][4]])<<8) + (int)(unsigned char)rtIn[rtFeedback[m][4]+1] );
01253 }
01254
01255 int BHandSerialDriver::RTGetTemp()
01256 {
01257
01258 if (!rtGlobalFlags[0])
01259 return 0;
01260 else
01261 return rtIn[rtGlobalFeedback[0]];
01262 }
01263
01264 unsigned char BHandSerialDriver::RTGetAIN(const char motor)
01265 {
01266
01267
01268 int m = motor - '1';
01269 if (m < 0)
01270 m = 0;
01271 else if (m > 3)
01272 m = 3;
01273
01274
01275 if (!rtFlags[m][0] || !rtFlags[m][9])
01276 return 0;
01277 else
01278 return (unsigned char)rtIn[rtFeedback[m][5]];
01279 }
01280
01281 void BHandSerialDriver::RTGetPPS(const char motor, int *pps, int ppsElements)
01282 {
01283
01284
01285
01286
01287 static float x = 0.3f;
01288 static float y = 0.6f;
01289 static int dirX = 1;
01290 static int dirY = 1;
01291
01292 unsigned int GRIDX = (motor <= '3') ? 3 : 7;
01293 unsigned int GRIDY = (motor <= '3') ? 8 : 4;
01294
01295 int i = 0;
01296 for (unsigned int r = 0; r < GRIDY; r++)
01297 {
01298 for (unsigned int c = 0; c < GRIDX; c++)
01299 {
01300 if (i >= ppsElements)
01301 continue;
01302 float x0 = (c + 0.5f) / GRIDX;
01303 float y0 = (r + 0.5f) / GRIDY;
01304 float dist = sqrtf((x-x0)*(x-x0) + (y-y0)*(y-y0));
01305 pps[i] = (int)(4095.0f * (1 - dist /1.42f));
01306
01307 if (pps[i] > 4095)
01308 pps[i] = 4095;
01309 else if (pps[i] < 0)
01310 pps[i] = 0;
01311
01312 if (dirX > 0)
01313 {
01314 x += 0.0003f;
01315 if (x > 1)
01316 dirX = -1;
01317 }
01318 else
01319 {
01320 x -= 0.0003f;
01321 if (x < 0)
01322 dirX = 1;
01323 }
01324
01325 if (dirY > 0)
01326 {
01327 y += 0.0003f;
01328 if (y > 1)
01329 dirY = -1;
01330 }
01331 else
01332 {
01333 y -= 0.0003f;
01334 if (y < 0)
01335 dirY = 1;
01336 }
01337
01338 if (motor > '3' && (r == 0 || r == (GRIDY - 1)) && (c == 0 || c == (GRIDX - 1)))
01339 {
01340
01341 }
01342 else
01343 i++;
01344
01345 }
01346 }
01347 }
01348
01349
01350
01351
01352
01353
01354
01355
01356
01357
01358
01359
01360
01361
01362
01363
01364
01365
01366
01367
01368
01369
01370
01371
01372
01373
01374
01375
01376
01377
01378