00001
00028 #include "BHandSupervisoryRealTime.h"
00029
00030 #include "BHand.h"
00031
00032 #ifdef BH8_280_HARDWARE
00033 #include "BHandCANDriver.h"
00034 #endif
00035 #ifdef BH8_262_HARDWARE
00036 #include "BHandSerialDriver.h"
00037 #include "BHandBH8_262.h"
00038 #endif
00039
00040 #include "bhand_parse.h"
00041
00043
00045
00046 BHandSupervisoryRealtime::BHandSupervisoryRealtime(BHand *bhand, int priority) :
00047 syncMode(BHMODE_SYNC), requestTimeout(INFINITE), pCallback(NULL),
00048 m_initialized(false), m_bhand(bhand), m_driver(NULL),
00049 m_requestPending(),
00050 m_requestComplete(false),
00051 m_request(0), m_comErr(0)
00052 {
00053 createThread(priority);
00054 }
00055
00056 BHandSupervisoryRealtime::~BHandSupervisoryRealtime()
00057 {
00058 while (ComIsPending());
00059
00060 syncMode = BHMODE_ASYNCWAIT;
00061 ComRequest(BHREQ_EXIT);
00062
00063
00064 m_thread.tryJoin(1000);
00065
00066
00067 if (m_driver != NULL)
00068 {
00069 m_driver->Close();
00070 delete m_driver;
00071 }
00072 }
00073
00074
00076
00078
00079
00080
00098 int BHandSupervisoryRealtime::InitSoftware(int port, int priority)
00099 {
00100
00101 int hwIndex = BHandHardware::getBHandHardwareIndex("BH8-262");
00102 if (hwIndex < 0)
00103 {
00104
00105 return BHERR_NOTINITIALIZED;
00106 }
00107 m_bhand->setHardwareDesc(hwIndex);
00108
00109
00110
00111
00112 return Init(port, priority, BH_SERIAL_COMMUNICATION);
00113 }
00114
00132 int BHandSupervisoryRealtime::Init(int port, int priority, BHCommunication comm, bool async)
00133 {
00134
00135 if (m_initialized)
00136 return BHERR_BHANDEXISTS;
00137
00139
00141
00142
00143 if (m_driver != NULL)
00144 {
00145
00146 m_driver->Close();
00147 delete m_driver;
00148 m_driver = NULL;
00149 }
00150
00151 switch (comm)
00152 {
00153 #ifdef BH8_280_HARDWARE
00154 case BH_CAN_COMMUNICATION:
00155 {
00156 m_driver = new BHandCANDriver(m_bhand, this);
00157 break;
00158 }
00159 #endif
00160 #ifdef BH8_262_HARDWARE
00161 case BH_SERIAL_COMMUNICATION:
00162 {
00163 m_driver = new BHandSerialDriver(m_bhand, this, port);
00164 break;
00165 }
00166 #endif
00167 default:
00168 {
00169
00170 return BHERR_NOTINITIALIZED;
00171 }
00172 }
00173
00174
00175
00177
00179
00180 m_bhand->setDeviceDriver(m_driver);
00181
00183
00185
00186 int result;
00187 if ((result = m_driver->Initialize()))
00188 {
00189 m_driver->Close();
00190 m_comErr = result;
00191 return result;
00192 }
00193
00194
00195
00196 #ifdef BH8_262_HARDWARE
00197
00198
00199 if (comm == BH_SERIAL_COMMUNICATION && (result = ComSetTimeouts(50, 2000, 50, 2000)) > 0)
00200 {
00201 m_driver->Close();
00202 m_comErr = result;
00203 return result;
00204 }
00205 #endif
00206
00207
00208
00209
00210 if (async)
00211 setSyncMode(BHMODE_ASYNCWAIT);
00212
00214
00216
00217
00218
00219 bool receivedResponse;
00220 result = Reset(&receivedResponse);
00221
00222 #ifdef BH8_262_HARDWARE
00223
00224 if (!async && comm == BH_SERIAL_COMMUNICATION && (result = ComSetTimeouts(50, 15000, 50, 5000)) > 0)
00225 {
00226 m_driver->Close();
00227 m_comErr = result;
00228 return result;
00229 }
00230 #endif
00231
00232
00233 m_initialized = receivedResponse;
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249 return receivedResponse ? 0 : BHERR_READCOM;
00250 }
00251
00252
00254
00256
00257 bool BHandSupervisoryRealtime::driverIsOpen() { return m_driver != NULL && m_driver->IsOpen(); }
00258 bool BHandSupervisoryRealtime::serialDriverIsOpen() { return driverIsOpen() && m_driver->getComm() == BH_SERIAL_COMMUNICATION; }
00259
00260 void BHandSupervisoryRealtime::createThread(int priority)
00261 {
00262
00263 m_requestComplete.set();
00264 m_request = 0;
00265
00266
00267 m_thread.setName("BHand Async");
00268 switch (priority)
00269 {
00270 case THREAD_PRIORITY_TIME_CRITICAL: { m_thread.setPriority(Poco::Thread::PRIO_HIGHEST); break; }
00271 case THREAD_PRIORITY_HIGHEST: { m_thread.setPriority(Poco::Thread::PRIO_HIGHEST); break; }
00272 case THREAD_PRIORITY_ABOVE_NORMAL: { m_thread.setPriority(Poco::Thread::PRIO_HIGH); break; }
00273 case THREAD_PRIORITY_NORMAL: { m_thread.setPriority(Poco::Thread::PRIO_NORMAL); break; }
00274 case THREAD_PRIORITY_BELOW_NORMAL: { m_thread.setPriority(Poco::Thread::PRIO_LOW); break; }
00275 case THREAD_PRIORITY_LOWEST: { m_thread.setPriority(Poco::Thread::PRIO_LOWEST); break; }
00276 case THREAD_PRIORITY_IDLE: { m_thread.setPriority(Poco::Thread::PRIO_LOWEST); break; }
00277 default: { m_thread.setPriority(Poco::Thread::PRIO_HIGHEST); break; }
00278 }
00279
00280 m_thread.start(*this);
00281
00282
00283
00284
00285 }
00286
00287
00288
00289
00291
00293
00294 int BHandSupervisoryRealtime::validate(BHMotors motors, const char *propertyName, int value)
00295 {
00296
00297 if (!motors)
00298 return 0;
00299
00300
00301
00302
00303 BHandHardware *hw = m_bhand->getHardwareDesc();
00304
00305 BHandProperty *p = hw->getBHandProperty(propertyName);
00306
00307
00308 if (p->getNumExtendedAttributes() != 4)
00309 return BHERR_OUTOFRANGE;
00310
00311 for (unsigned int i = 0; i < 4; i++)
00312 if ((motors >> i) & 1)
00313 {
00314 int minValue = p->getMinValue(i);
00315 int maxValue = p->getMaxValue(i);
00316
00317
00318
00319
00320 if (value < minValue || value > maxValue)
00321 return BHERR_OUTOFRANGE;
00322 }
00323
00324 return 0;
00325 }
00326
00327 int BHandSupervisoryRealtime::validate(const char *propertyName, int value)
00328 {
00329
00330 BHandHardware *hw = m_bhand->getHardwareDesc();
00331 BHandProperty *p = hw->getBHandProperty(propertyName);
00332
00333
00334 if (p->getNumExtendedAttributes() != 1)
00335 return BHERR_OUTOFRANGE;
00336
00337 int minValue = p->getMinValue();
00338 int maxValue = p->getMaxValue();
00339
00340
00341 if (value < minValue || value > maxValue)
00342 return BHERR_OUTOFRANGE;
00343
00344 return 0;
00345 }
00346
00347
00348
00349
00351
00353
00354
00363 int BHandSupervisoryRealtime::ComRequest(int requestNumber)
00364 {
00365
00366 if (syncMode == BHMODE_ASYNCNOW)
00367 {
00368
00369 if (!m_requestComplete.tryWait(0))
00370 return BHERR_NOTCOMPLETED;
00371 }
00372 else
00373 {
00374 if (requestTimeout < INFINITE)
00375 m_requestComplete.wait(requestTimeout);
00376 else
00377 {
00378 m_requestComplete.wait();
00379 }
00380 }
00381
00382
00383 if (syncMode == BHMODE_RETURN)
00384 {
00385
00386 m_comErr = 0;
00387 if (pCallback)
00388 (*(pCallback))(getBHand());
00389
00390 return 0;
00391 }
00392
00393
00394 m_requestComplete.reset();
00395
00396
00397 m_request = requestNumber;
00398 m_requestPending.set();
00399
00400
00401 if (syncMode == BHMODE_SYNC)
00402 return ComWaitForCompletion(requestTimeout);
00403 else
00404 return BHERR_PENDING;
00405 }
00406
00407
00417 int BHandSupervisoryRealtime::ComWaitForCompletion(unsigned int timeout)
00418 {
00419 if (timeout < INFINITE)
00420 {
00421 bool wait = m_requestComplete.tryWait(timeout);
00422 if (wait)
00423 return m_comErr;
00424 else
00425 return BHERR_TIMEOUT;
00426 }
00427 else
00428 m_requestComplete.wait();
00429
00430 return m_comErr;
00431 }
00432
00433
00438 bool BHandSupervisoryRealtime::ComIsPending()
00439 {
00440 return !m_requestComplete.tryWait(0);
00441 }
00442
00455 int BHandSupervisoryRealtime::ComGetError()
00456 {
00457 return m_comErr;
00458 }
00459
00469 BHandSupervisoryResult * BHandSupervisoryRealtime::GetResult()
00470 {
00471 return m_driver->GetResult();
00472 }
00473
00474
00475
00476 #define THREADEND(err) \
00477 { \
00478 m_comErr = (err); \
00479 if (pCallback) \
00480 (*(pCallback))(m_bhand); \
00481 break; \
00482 }
00483
00484
00486
00488
00498 void BHandSupervisoryRealtime::run()
00499 {
00500 int result;
00501
00502
00503 while (1)
00504 {
00505
00506 m_requestComplete.set();
00507
00508
00509 m_requestPending.wait();
00510
00511 m_comErr = 0;
00512
00513
00514 if (!driverIsOpen() && (m_request == BHREQ_REALTIME || m_request == BHREQ_SUPERVISE))
00515 continue;
00516
00517
00518 switch (m_request)
00519 {
00520 case BHREQ_EXIT:
00521 {
00522
00523 m_requestComplete.set();
00524
00525
00526 return;
00527 }
00528 case BHREQ_REALTIME:
00529 {
00530
00531 result = m_driver->ExecuteRealtimeCall();
00532
00533 THREADEND(result);
00534 }
00535
00536 case BHREQ_SUPERVISE:
00537 {
00538
00539 result = m_driver->ExecuteSupervisoryCall();
00540
00541
00542
00543
00544 THREADEND(result);
00545 }
00546
00547 case BHREQ_CLEAR:
00548 {
00549 #ifdef BH8_262_HARDWARE
00550
00551 if (!ComClear())
00552 THREADEND(BHERR_CLEARBUFFER)
00553 #endif
00554 THREADEND(0)
00555 }
00556 default:
00557 {
00558 }
00559 }
00560 }
00561 }
00562
00563
00564
00565
00567
00569
00570 #ifdef BH8_262_HARDWARE
00571
00580 int BHandSupervisoryRealtime::ComInitialize(int comport, int priority)
00581 {
00582
00583 return (m_driver != NULL && m_driver->getComm() == BH_SERIAL_COMMUNICATION) ?
00584 ((BHandSerialDriver *)m_driver)->ComInitialize(comport) : BHERR_OPENCOMMPORT;
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597 }
00598
00599
00610 int BHandSupervisoryRealtime::ComOpen(int comport, int baudrate)
00611 {
00612
00613 if (m_initialized || m_driver != NULL)
00614 {
00615 printf("BHandSupervisoryRealtime: m_initialized || m_driver != NULL\n");
00616 if (m_driver != NULL && m_driver->getComm() == BH_SERIAL_COMMUNICATION)
00617 {
00618 printf("For the first time - using serial driver that already exists.\n");
00619 return ((BHandSerialDriver *)m_driver)->ComOpen(comport, baudrate);
00620 }
00621 else
00622 return BHERR_BHANDEXISTS;
00623 }
00624
00625
00626 printf("BHandSupervisoryRealtime::ComOpen\n");
00627 if (m_driver != NULL)
00628 printf("m_driver != NULL\n");
00629 else
00630 printf("m_driver = NULL\n");
00631
00632
00633 m_driver = new BHandSerialDriver(m_bhand, this, comport);
00634 m_bhand->setDeviceDriver(m_driver);
00635
00636
00637
00638 return (m_driver != NULL && m_driver->getComm() == BH_SERIAL_COMMUNICATION) ?
00639 ((BHandSerialDriver *)m_driver)->ComOpen(comport, baudrate) : BHERR_OPENCOMMPORT;
00640 }
00641
00642
00648 void BHandSupervisoryRealtime::ComClose()
00649 {
00650 if (serialDriverIsOpen())
00651 {
00652 ((BHandSerialDriver *)m_driver)->ComClose();
00653
00654 if (m_driver != NULL)
00655 {
00656 delete m_driver;
00657 m_driver = NULL;
00658 }
00659 m_initialized = false;
00660 }
00661 }
00662
00667 bool BHandSupervisoryRealtime::ComIsOpen()
00668 {
00669 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->ComIsOpen() : false;
00670 }
00671
00672
00682 bool BHandSupervisoryRealtime::ComClear(bool rxOnly)
00683 {
00684 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->ComClear(rxOnly) : false;
00685 }
00686
00687
00698 int BHandSupervisoryRealtime::ComRead(char* rxBuf, int rxNumBytes)
00699 {
00700 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->ComRead(rxBuf, rxNumBytes) : BHERR_OPENCOMMPORT;
00701 }
00702
00703
00714 int BHandSupervisoryRealtime::ComWrite(const char* txBuf, int txNumBytes)
00715 {
00716 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->ComWrite(txBuf, txNumBytes) : BHERR_OPENCOMMPORT;
00717 }
00718
00719
00729 void BHandSupervisoryRealtime::SetWaitCallbackFunc(BHCallback waitCallbackFunc)
00730 {
00731 if (driverIsOpen())
00732 m_driver->SetWaitCallbackFunc(waitCallbackFunc);
00733 }
00734
00735
00768 int BHandSupervisoryRealtime::ComSetTimeouts(unsigned int readInterval,
00769 unsigned int readMultiplier, unsigned int readConstant,
00770 unsigned int writeMultiplier, unsigned int writeConstant)
00771 {
00772 return ComSetTimeouts(readMultiplier, readConstant, writeMultiplier, writeConstant);
00773 }
00774
00775
00806 int BHandSupervisoryRealtime::ComSetTimeouts(unsigned int readMultiplier, unsigned int readConstant, unsigned int writeMultiplier, unsigned int writeConstant)
00807 {
00808 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->ComSetTimeouts(readMultiplier, readConstant, writeMultiplier, writeConstant) : BHERR_OPENCOMMPORT;
00809 }
00810
00811
00823 int BHandSupervisoryRealtime::ComSetBaudrate(int baudrate)
00824 {
00825 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->ComSetBaudrate(baudrate) : BHERR_OPENCOMMPORT;
00826 }
00827
00828 #endif
00829
00830
00831
00832
00834
00836
00855 int BHandSupervisoryRealtime::InitHand(const char *motor)
00856 {
00857 return driverIsOpen() ? m_driver->InitHand(motor) : BHERR_OPENCOMMPORT;
00858 }
00859
00860
00878 int BHandSupervisoryRealtime::Reset(bool *responseReceived)
00879 {
00880 return driverIsOpen() ? m_driver->Reset(responseReceived) : BHERR_OPENCOMMPORT;
00881 }
00882
00883
00902 int BHandSupervisoryRealtime::Close(const char *motor)
00903 {
00904 return driverIsOpen() ? m_driver->Close(motor) : BHERR_OPENCOMMPORT;
00905 }
00906
00907
00926 int BHandSupervisoryRealtime::Open(const char *motor)
00927 {
00928 return driverIsOpen() ? m_driver->Open(motor) : BHERR_OPENCOMMPORT;
00929 }
00930
00931
00948 int BHandSupervisoryRealtime::GoToDefault(const char* motor)
00949 {
00950 return driverIsOpen() ? m_driver->GoToDefault(motor) : BHERR_OPENCOMMPORT;
00951 }
00952
00953
00974 int BHandSupervisoryRealtime::GoToDifferentPositions(int value1, int value2, int value3, int value4)
00975 {
00976 if (!driverIsOpen()) return BHERR_OPENCOMMPORT;
00977
00978 if (validate(1, "DP", value1)) return BHERR_OUTOFRANGE;
00979 if (validate(2, "DP", value2)) return BHERR_OUTOFRANGE;
00980 if (validate(4, "DP", value3)) return BHERR_OUTOFRANGE;
00981 if (validate(8, "DP", value4)) return BHERR_OUTOFRANGE;
00982
00983
00984 int encoderPositions[4];
00985 encoderPositions[0] = value1;
00986 encoderPositions[1] = value2;
00987 encoderPositions[2] = value3;
00988 encoderPositions[3] = value4;
00989
00990 return m_driver->GoToDifferentPositions(encoderPositions, 4);
00991 }
00992
00993
01008 int BHandSupervisoryRealtime::GoToHome(const char *motor)
01009 {
01010 return driverIsOpen() ? m_driver->GoToHome(motor) : BHERR_OPENCOMMPORT;
01011 }
01012
01013
01032 int BHandSupervisoryRealtime::GoToPosition(const char *motor, int value)
01033 {
01034 if (!driverIsOpen())
01035 return BHERR_OPENCOMMPORT;
01036
01037 BHMotors bhMotors = toBHMotors(motor);
01038 if (validate(bhMotors, "DP", value))
01039 return BHERR_OUTOFRANGE;
01040
01041 return m_driver->GoToPosition(motor, value);
01042 }
01043
01044
01064 int BHandSupervisoryRealtime::StepClose(const char *motor, int stepAmount)
01065 {
01066 if (!driverIsOpen())
01067 return BHERR_OPENCOMMPORT;
01068
01069 BHMotors bhMotors = toBHMotors(motor);
01070 if (validate(bhMotors, "DS", stepAmount))
01071 return BHERR_OUTOFRANGE;
01072
01073 return m_driver->StepClose(motor, true, stepAmount);
01074 }
01075
01091 int BHandSupervisoryRealtime::StepClose(const char *motor)
01092 {
01093 if (!driverIsOpen())
01094 return BHERR_OPENCOMMPORT;
01095
01096 return m_driver->StepClose(motor);
01097 }
01098
01099
01119 int BHandSupervisoryRealtime::StepOpen(const char *motor, int stepAmount)
01120 {
01121 if (!driverIsOpen())
01122 return BHERR_OPENCOMMPORT;
01123
01124 BHMotors bhMotors = toBHMotors(motor);
01125 if (validate(bhMotors, "DS", stepAmount))
01126 return BHERR_OUTOFRANGE;
01127
01128 return m_driver->StepOpen(motor, true, stepAmount);
01129 }
01130
01131
01147 int BHandSupervisoryRealtime::StepOpen(const char *motor)
01148 {
01149 if (!driverIsOpen())
01150 return BHERR_OPENCOMMPORT;
01151
01152 return m_driver->StepOpen(motor);
01153 }
01154
01170 int BHandSupervisoryRealtime::TorqueClose(const char *motor)
01171 {
01172 return driverIsOpen() ? m_driver->TorqueClose(motor) : BHERR_OPENCOMMPORT;
01173 }
01174
01175
01191 int BHandSupervisoryRealtime::TorqueOpen(const char *motor)
01192 {
01193 return driverIsOpen() ? m_driver->TorqueOpen(motor) : BHERR_OPENCOMMPORT;
01194 }
01195
01196
01198
01200
01201
01226 int BHandSupervisoryRealtime::Get(const char *motor, const char *propertyName, int *result)
01227 {
01228 if (!driverIsOpen())
01229 return BHERR_OPENCOMMPORT;
01230
01231
01232 int r;
01233 if ((r = m_driver->Get(motor, propertyName, result)))
01234 return r;
01235
01236
01237
01238 return 0;
01239 }
01240
01241
01262 int BHandSupervisoryRealtime::Set(const char *motor, const char *propertyName, int value)
01263 {
01264 if (!driverIsOpen())
01265 return BHERR_OPENCOMMPORT;
01266
01267
01268
01269
01270 return m_driver->Set(motor, propertyName, value);
01271 }
01272
01273
01293 int BHandSupervisoryRealtime::PGet(const char *propertyName, int *result)
01294 {
01295 if (!driverIsOpen())
01296 return BHERR_OPENCOMMPORT;
01297
01298
01299 int r;
01300 if ((r = m_driver->PGet(propertyName, result)))
01301 return r;
01302
01303 return 0;
01304 }
01305
01306
01325 int BHandSupervisoryRealtime::PSet(const char *propertyName, int value)
01326 {
01327 if (!driverIsOpen())
01328 return BHERR_OPENCOMMPORT;
01329
01330 if (validate(propertyName, value))
01331 return BHERR_OUTOFRANGE;
01332
01333 return m_driver->PSet(propertyName, value);
01334 }
01335
01336
01355 int BHandSupervisoryRealtime::Default(const char* motor)
01356 {
01357 return driverIsOpen() ? m_driver->Default(motor) : BHERR_OPENCOMMPORT;
01358 }
01359
01360
01378 int BHandSupervisoryRealtime::Load(const char* motor)
01379 {
01380 return driverIsOpen() ? m_driver->Load(motor) : BHERR_OPENCOMMPORT;
01381 }
01382
01383
01405 int BHandSupervisoryRealtime::Save(const char *motor)
01406 {
01407 return driverIsOpen() ? m_driver->Save(motor) : BHERR_OPENCOMMPORT;
01408 }
01409
01410
01427 int BHandSupervisoryRealtime::Temperature(int *result)
01428 {
01429 return driverIsOpen() ? m_driver->Temperature(result) : BHERR_OPENCOMMPORT;
01430 }
01431
01432
01434
01436
01437 void allocate(COMMAND_RESULT *cResult)
01438 {
01439 if (cResult)
01440 {
01441
01442 cResult->result = new char[MAX_COMMAND_RESULT_LENGTH];
01443 cResult->result[0] = 0;
01444 }
01445 }
01446
01447
01473 int BHandSupervisoryRealtime::Command(const char *send, char *receive)
01474 {
01475 if (!driverIsOpen())
01476 return BHERR_OPENCOMMPORT;
01477
01478 #ifdef BH8_262_HARDWARE
01479 BHandHardware *hw = m_bhand->getHardwareDesc();
01480
01481
01482 if (strcmp(hw->getModelNumber(), BHAND_BH8_262_MODEL_NUM) == 0)
01483 return m_driver->Command(send, receive);
01484 #endif
01485
01486
01487
01488
01489 if (receive != NULL)
01490 *receive = 0;
01491
01492
01493 if (strlen(send) + 1 >= MAX_CMD_LEN)
01494 {
01495
01496
01497 return m_driver->Command(send, receive);
01498 }
01499
01500
01501
01502
01503 COMMAND command;
01504 COMMAND_RESULT command_result;
01505
01506
01507 COMMAND_RESULT *cResult = &command_result;
01508 cResult->result = NULL;
01509
01510
01511 strcpy(command.command, send);
01512
01513
01514
01515
01516 parseInput(&command, cResult);
01517
01518
01519 int cmd = command_result.command;
01520 BHMotors bhMotors = command_result.bhMotors;
01521
01522 bool valueIncluded = command_result.valueIncluded;
01523 int value = command_result.value;
01524
01525
01526
01527
01528
01529
01530
01531
01532
01533
01534
01535
01536
01537 char motors[10];
01538 toMotorChar(bhMotors, motors);
01539
01540
01541
01542
01543 switch (cmd)
01544 {
01545
01546 case CMD_HI: { return InitHand(motors); }
01547 case CMD_RESET: { bool reset; return Reset(&reset); }
01548
01549
01550 case CMD_C: { return Close(motors); }
01551 case CMD_O: { return Open(motors); }
01552
01553 case CMD_M: { return valueIncluded ? GoToPosition(motors, value) : GoToDefault(motors); }
01554 case CMD_HOME: { return GoToHome(motors); }
01555
01556 case CMD_IC: { return valueIncluded ? StepClose(motors, value) : StepClose(motors); }
01557 case CMD_IO: { return valueIncluded ? StepOpen(motors, value) : StepOpen(motors); }
01558
01559 case CMD_TC: { return TorqueClose(motors); break; }
01560 case CMD_TO: { return TorqueOpen(motors); break; }
01561
01562
01563 case CMD_SET: { return Set(motors, command_result.propertyName, value); }
01564 case CMD_GET:
01565 {
01566
01567 unsigned int nResults = countMotors(bhMotors);
01568
01569
01570
01571 int *results = new int[nResults];
01572
01573
01574
01575 Get(motors, command_result.propertyName, results);
01576
01577
01578 char *resultString = receive;
01579 char propValueString[64];
01580 for (unsigned int i = 0; i < nResults; i++)
01581 {
01582 sprintf(propValueString, "%d ", results[i]);
01583 strcat(resultString, propValueString);
01584 }
01585
01586
01587 if (strlen(resultString) > 0)
01588 resultString[strlen(resultString) - 1] = 0;
01589
01590
01591
01592
01593 delete[] results;
01594
01595 return 0;
01596
01597 }
01598
01599 case CMD_LOAD: { return Load(motors); }
01600 case CMD_SAVE: { return Save(motors); }
01601 case CMD_DEF: { return Default(motors); }
01602
01603
01604
01605
01606 case CMD_T: { return StopMotor(motors); }
01607
01608
01609 case CMD_LOOP: { return RTStart(motors); }
01610
01611 default:
01612 {
01613
01614 return BHERR_NOTCOMPLETED;
01615 }
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625 }
01626 }
01627
01628
01643 int BHandSupervisoryRealtime::Delay(unsigned int msec)
01644 {
01645 if (!driverIsOpen())
01646 return BHERR_OPENCOMMPORT;
01647
01648
01649 if (((int)msec) <= 0)
01650 return BHERR_OUTOFRANGE;
01651
01652 return m_driver->Delay(msec);
01653 }
01654
01655
01673 int BHandSupervisoryRealtime::StopMotor(const char *motor)
01674 {
01675 return driverIsOpen() ? m_driver->StopMotor(motor) : BHERR_OPENCOMMPORT;
01676 }
01677
01678
01679 #ifdef BH8_262_HARDWARE
01680
01688 const char* BHandSupervisoryRealtime::Response()
01689 {
01690 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->Response() : NULL;
01691 }
01692
01693
01700 const char* BHandSupervisoryRealtime::Buffer()
01701 {
01702 return serialDriverIsOpen() ? ((BHandSerialDriver *)m_driver)->Response() : NULL;
01703 }
01704
01705 #endif
01706
01707
01730 int BHandSupervisoryRealtime::Baud(unsigned int newbaud)
01731 {
01732 return driverIsOpen() ? m_driver->Baud(newbaud) : BHERR_OPENCOMMPORT;
01733 }
01734
01735
01736
01737
01739
01741
01742
01774 int BHandSupervisoryRealtime::RTStart(const char *motor, BHMotorProtection motorProtection)
01775 {
01776 return driverIsOpen() ? m_driver->RTStart(motor, motorProtection) : BHERR_OPENCOMMPORT;
01777 }
01778
01779
01809 int BHandSupervisoryRealtime::RTUpdate(bool control, bool feedback)
01810 {
01811 return driverIsOpen() ? m_driver->RTUpdate(control, feedback) : BHERR_OPENCOMMPORT;
01812 }
01813
01814
01827 int BHandSupervisoryRealtime::RTAbort()
01828 {
01829 return driverIsOpen() ? m_driver->RTAbort() : BHERR_OPENCOMMPORT;
01830 }
01831
01865 int BHandSupervisoryRealtime::RTUpdate(const char *motor, const char *property, int *values)
01866 {
01867 return driverIsOpen() ? m_driver->RTUpdate(motor, property, values) : BHERR_OPENCOMMPORT;
01868 }
01869
01916 int BHandSupervisoryRealtime::RTSetFlags(const char *motor, bool LCV, int LCVC, bool LCPG,
01917 bool LFV, int LFVC, bool LFS, bool LFAP, bool LFDP, int LFDPC)
01918 {
01919
01920
01921
01922
01923
01924
01925
01926
01927
01928
01929
01930
01931
01932
01933 return driverIsOpen() ? m_driver->RTSetFlags(motor, LCV, LCVC, LCPG,
01934 LFV, LFVC, LFS, LFAP, LFDP, LFDPC) : BHERR_OPENCOMMPORT;
01935 }
01936
01937
01995 int BHandSupervisoryRealtime::RTSetFlags(const char *motor, bool LCV, int LCVC, bool LCPG, bool LCT,
01996 bool LFV, int LFVC, bool LFS, bool LFAP, bool LFDP, int LFDPC,
01997 bool LFBP, bool LFAIN, bool LFDPD, bool LFT)
01998 {
01999 return driverIsOpen() ? m_driver->RTSetFlags(motor, LCV, LCVC, LCPG, LCT,
02000 LFV, LFVC, LFS, LFAP, LFDP, LFDPC, LFBP, LFAIN, LFDPD, LFT) : BHERR_OPENCOMMPORT;
02001 }
02002
02003
02027 char BHandSupervisoryRealtime::RTGetVelocity(const char motor)
02028 {
02029 return driverIsOpen() ? m_driver->RTGetVelocity(motor) : 0;
02030 }
02031
02032
02052 unsigned char BHandSupervisoryRealtime::RTGetStrain(const char motor)
02053 {
02054 return driverIsOpen() ? m_driver->RTGetStrain(motor) : 0;
02055 }
02056
02057
02073 int BHandSupervisoryRealtime::RTGetPosition(const char motor)
02074 {
02075 return driverIsOpen() ? m_driver->RTGetPosition(motor) : 0;
02076 }
02077
02078
02094 int BHandSupervisoryRealtime::RTGetTemp()
02095 {
02096 return driverIsOpen() ? m_driver->RTGetTemp() : 0;
02097 }
02098
02099
02114 unsigned char BHandSupervisoryRealtime::RTGetAIN(const char motor)
02115 {
02116 return driverIsOpen() ? m_driver->RTGetAIN(motor) : 0;
02117 }
02118
02119
02153 char BHandSupervisoryRealtime::RTGetDeltaPos(const char motor)
02154 {
02155 return driverIsOpen() ? m_driver->RTGetDeltaPos(motor) : 0;
02156 }
02157
02178 int BHandSupervisoryRealtime::RTGetBreakawayPosition(const char motor)
02179 {
02180 return driverIsOpen() ? m_driver->RTGetBreakawayPosition(motor) : 0;
02181 }
02182
02183 void BHandSupervisoryRealtime::RTGetPPS(const char motor, int *pps, int ppsElements)
02184 {
02185 if (driverIsOpen())
02186 m_driver->RTGetPPS(motor, pps, ppsElements);
02187 }
02188
02189
02231 int BHandSupervisoryRealtime::RTSetVelocity(const char motor, int velocity)
02232 {
02233 return driverIsOpen() ? m_driver->RTSetVelocity(motor, velocity) : 0;
02234 }
02235
02236
02264 int BHandSupervisoryRealtime::RTSetTorque(const char motor, int torque)
02265 {
02266 return driverIsOpen() ? m_driver->RTSetTorque(motor, torque) : 0;
02267 }
02268
02269
02292 int BHandSupervisoryRealtime::RTSetGain(const char motor, int gain)
02293 {
02294 return driverIsOpen() ? m_driver->RTSetGain(motor, gain) : 0;
02295 }
02296
02297
02318 int BHandSupervisoryRealtime::RTSetPosition(const char motor, int position)
02319 {
02320 return driverIsOpen() ? m_driver->RTSetPosition(motor, position) : 0;
02321 }
02322