00001
00002
00004
00005 #include "BHandCANDriver.h"
00006
00007 #include "BHandSupervisoryRealTime.h"
00008 #include "BHandCommands.h"
00009
00010
00011 #include "bhand_misc.h"
00012 #include "bhand_parse.h"
00013 #include "bhand_motors.h"
00014
00015 #include "puck2.h"
00016
00017 #include <cstring>
00018
00019 #include <math.h>
00020
00021
00022 #ifndef MAX
00023 #define MAX(x, y) ((x > y) ? x : y)
00024 #endif
00025
00026
00027 enum BHRealTimeMotorProperties {
00028 REALTIME_LCV = 0,
00029 REALTIME_LCT,
00030 REALTIME_LCP,
00031 REALTIME_LFV,
00032 REALTIME_LFS,
00033 REALTIME_LFT,
00034 REALTIME_LFAP,
00035 REALTIME_LFPPS
00036 };
00037
00038 const struct textKey propBHRealTimeMotor[] =
00039 {
00040 { "LFPPS", REALTIME_LFPPS },
00041 { "LFAP", REALTIME_LFAP },
00042 { "LCP", REALTIME_LCP },
00043 { "LCT", REALTIME_LCT },
00044 { "LCV", REALTIME_LCV },
00045 { "LFS", REALTIME_LFS },
00046 { "LFT", REALTIME_LFT },
00047 { "LFV", REALTIME_LFV },
00048
00049 { "", NONE }
00050 };
00051
00052
00054
00056
00057 static const RealTimeGlobalParameters defGlobalParams = { 0, false, 0, BHMotorTSTOPProtect, 0 };
00058 static const RealTimeMotorParameters defMotorParams =
00059 { false, false, false, false, false, false, false, false, 0, 0, 0, 0, 0, 0, 0, { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, 0.0f };
00060
00061
00062 #ifdef PPS_DEMO
00063
00064 const int PPS_FILTER_SIZE = 32;
00065 int filterIndexPPS = 0;
00066 int FeedbackPresentPPS[4][24][PPS_FILTER_SIZE];
00067 int TaredPPS[4][24];
00068 #endif
00069
00070
00071 #define NUM_PUCKS_IN_HAND 4
00072
00073 #define PUCK2_ID_FINGER1 11
00074 #define PUCK2_ID_FINGER2 12
00075 #define PUCK2_ID_FINGER3 13
00076 #define PUCK2_ID_SPREAD 14
00077
00078
00080
00082
00083 BHandCANDriver::BHandCANDriver(BHand *bhand, BHandSupervisoryRealtime *moduleSuperReal)
00084 : BHandDriver(bhand, moduleSuperReal, BH_CAN_COMMUNICATION), pucksInHand(PUCK2_ID_PC),
00085 m_realtimeMode(false),
00086 realtimeGlobalParameters(defGlobalParams)
00087 {
00088 realtimeMotorParameters = new RealTimeMotorParameters[NUM_PUCKS_IN_HAND];
00089 for (int i = 0; i < NUM_PUCKS_IN_HAND; i++)
00090 realtimeMotorParameters[i] = defMotorParams;
00091 pucks = new BPuck2* [NUM_PUCKS_IN_HAND];
00092 }
00093
00094
00095 BHandCANDriver::~BHandCANDriver()
00096 {
00097 delete[] pucks;
00098 delete[] realtimeMotorParameters;
00099 }
00100
00101
00103
00105
00106 int BHandCANDriver::Initialize()
00107 {
00108 if (m_open)
00109 return BHERR_OPENCOMMPORT;
00110
00111
00112 if (pucksInHand.addPuck(PUCK2_ID_FINGER1) || pucksInHand.addPuck(PUCK2_ID_FINGER2) ||
00113 pucksInHand.addPuck(PUCK2_ID_FINGER3) || pucksInHand.addPuck(PUCK2_ID_SPREAD))
00114 return BHERR_OPENCOMMPORT;
00115
00116
00117 if (pucksInHand.init())
00118 return BHERR_OPENCOMMPORT;
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131 pucks[0] = pucksInHand.getPuck(PUCK2_ID_FINGER1);
00132 pucks[1] = pucksInHand.getPuck(PUCK2_ID_FINGER2);
00133 pucks[2] = pucksInHand.getPuck(PUCK2_ID_FINGER3);
00134 pucks[3] = pucksInHand.getPuck(PUCK2_ID_SPREAD);
00135
00136
00137 puckTwos.bhand = m_bhand;
00138 puckTwos.puckWithMotor = pucks;
00139 puckTwos.pucksWithMotors = NUM_PUCKS_IN_HAND;
00140 puckTwos.puckManager = &pucksInHand;
00141
00142 waitCallbackFunc = NULL;
00143
00144
00145 m_open = true;
00146
00147 return 0;
00148 }
00149
00150 void BHandCANDriver::Close()
00151 {
00152 pucksInHand.close();
00153 m_open = false;
00154 }
00155
00156
00157
00158
00159
00160
00161
00162 void BHandCANDriver::waitForStop(BHMotors bhMotors)
00163 {
00164
00165 int *tstop = new int[puckTwos.pucksWithMotors];
00166 bool *restoreTSTOP = new bool[puckTwos.pucksWithMotors];
00167 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00168 {
00169 restoreTSTOP[i] = false;
00170 if ((bhMotors >> i) & 1)
00171 {
00172 HandGet(1 << i, "TSTOP", &tstop[i]);
00173 if (tstop[i] == 0)
00174 {
00175 HandSet(1 << i, "TSTOP", BHAND_DEFAULT_TSTOP);
00176 restoreTSTOP[i] = true;
00177
00178 }
00179 }
00180 }
00181
00182
00183
00184
00185
00186
00187
00188 while (1)
00189 {
00190 int numMotors = 0;
00191 int numDone = 0;
00192
00193 if (waitCallbackFunc != NULL)
00194 {
00195 waitCallbackFunc(puckTwos.bhand);
00196 }
00197 else
00198 {
00199 DELAY(50);
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 }
00211
00212
00213
00214 unsigned int mode[4];
00215 BPuck2Manager *puckManager = &pucksInHand;
00216 puckManager->startGetPropertyBatch();
00217 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00218 if ((bhMotors >> i) & 1)
00219 puckManager->addToGetPropertyBatch(pucks[i]->getID(), &(mode[i]));
00220 int err = puckManager->getBatchPropertyUInt32(PROP_MODE);
00221
00222 if (err == 0)
00223 {
00224 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00225 if ((bhMotors >> i) & 1)
00226 {
00227 numMotors++;
00228
00229 if (mode[i] == 0 || mode[i] == 3)
00230 {
00231 numDone++;
00232
00233 }
00234 else
00235 {
00236
00237
00238
00239 }
00240
00241 }
00242 }
00243
00244 if (numDone == numMotors)
00245 {
00246 break;
00247 }
00248 }
00249
00250 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00251 {
00252 if (restoreTSTOP[i])
00253 {
00254 HandSet(1 << i, "TSTOP", tstop[i]);
00255
00256 }
00257 }
00258
00259 delete[] restoreTSTOP;
00260 delete[] tstop;
00261 }
00262
00263
00264
00265
00266
00267
00268
00269
00270 int BHandCANDriver::HandInit(BHMotors bhMotors)
00271 {
00272
00273 int temp[4];
00274 HandGet(15, "MODE", temp);
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292 if (bhMotors & 1) pucks[0]->setPropertyUInt8(PROP_CMD, CMD_HI);
00293 if (bhMotors & 2) pucks[1]->setPropertyUInt8(PROP_CMD, CMD_HI);
00294 if (bhMotors & 4) pucks[2]->setPropertyUInt8(PROP_CMD, CMD_HI);
00295
00296 if (bhMotors & 7)
00297 {
00298 for (int i = 0; i < 60; i++)
00299
00300 {
00301 int mode[3];
00302
00303 RTUpdate("G", "MODE", mode);
00304
00305
00306
00307 bool idle = true;
00308 if (bhMotors & 1 && mode[0] != 0) idle = false;
00309 if (bhMotors & 2 && mode[1] != 0) idle = false;
00310 if (bhMotors & 4 && mode[2] != 0) idle = false;
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322 if (idle)
00323 break;
00324
00325 DELAY(100);
00326
00327 }
00328 }
00329
00330
00331 if (bhMotors & 8)
00332 {
00333 pucks[3]->setPropertyUInt8(PROP_CMD, CMD_HI);
00334
00335
00336 for (int i = 0; i < 40; i++)
00337 {
00338 int mode;
00339 HandGet(8, "MODE", &mode);
00340
00341
00342
00343
00344
00345
00346
00347 if (mode == 0 || mode == 3)
00348 break;
00349
00350 DELAY(100);
00351
00352 }
00353 }
00354
00355 return 0;
00356 }
00357
00358 int BHandCANDriver::HandReset(BHMotors bhMotors, bool *responseReceived)
00359 {
00360 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00361 if ((bhMotors >> i) & 1)
00362 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_RESET);
00363
00364 DELAY(1000);
00365
00366 int awaken = pucksInHand.awaken();
00367
00368 if (awaken)
00369 {
00370 *responseReceived = false;
00371
00372
00373
00374
00375
00376 if (!pucks[0]->awake() || !pucks[1]->awake() || !pucks[2]->awake() || !pucks[3]->awake())
00377 {
00378
00379 return -9;
00380 }
00381 }
00382
00383
00384
00385
00386
00387
00388 HandSet(0x0f, "DP", 0);
00389 HandSet(0x0f, "TSTOP", BHAND_DEFAULT_TSTOP);
00390
00391
00392 *responseReceived = true;
00393
00394
00395 return (*responseReceived) ? 0 : -9;
00396 }
00397
00398 int BHandCANDriver::HandClose(BHMotors bhMotors)
00399 {
00400 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00401 if ((bhMotors >> i) & 1)
00402 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_C);
00403
00404 waitForStop(bhMotors);
00405
00406 return 0;
00407 }
00408
00409 int BHandCANDriver::HandOpen(BHMotors bhMotors)
00410 {
00411 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00412 if ((bhMotors >> i) & 1)
00413 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_O);
00414
00415 waitForStop(bhMotors);
00416 return 0;
00417 }
00418
00419 int BHandCANDriver::HandGoToDefault(BHMotors bhMotors, bool valueIncluded, int defaultPosition)
00420 {
00421
00422 if (valueIncluded)
00423 {
00424
00425 HandSet(bhMotors, "DP", defaultPosition);
00426
00427 }
00428
00429
00430
00431 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00432 if ((bhMotors >> i) & 1)
00433 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_M);
00434
00435
00436 waitForStop(bhMotors);
00437
00438 return 0;
00439 }
00440
00441 int BHandCANDriver::HandGoToDifferentPositionsHand(const int *encoderPositions, unsigned int numEncoderPositions)
00442 {
00443 if (numEncoderPositions > puckTwos.pucksWithMotors)
00444 return -1;
00445
00446 for (unsigned int i = 0; i < numEncoderPositions; i++)
00447 pucks[i]->setPropertyUInt32(PROP_M, (unsigned int)encoderPositions[i]);
00448 waitForStop(15);
00449
00450 return 0;
00451 }
00452
00453 int BHandCANDriver::HandGoToHome(BHMotors bhMotors)
00454 {
00455 if (bhMotors & 1) pucks[0]->setPropertyUInt8(PROP_CMD, CMD_HOME);
00456 if (bhMotors & 2) pucks[1]->setPropertyUInt8(PROP_CMD, CMD_HOME);
00457 if (bhMotors & 4) pucks[2]->setPropertyUInt8(PROP_CMD, CMD_HOME);
00458
00459 waitForStop(bhMotors & 7);
00460
00461 if (bhMotors & 8) pucks[3]->setPropertyUInt8(PROP_CMD, CMD_HOME);
00462
00463 waitForStop(bhMotors & 8);
00464
00465 return 0;
00466 }
00467
00468 int BHandCANDriver::HandGoToPosition(BHMotors bhMotors, unsigned int encoderPositionTickCount)
00469 {
00470
00471
00472 if (bhMotors & 1) pucks[0]->setPropertyUInt32(PROP_M, encoderPositionTickCount);
00473 if (bhMotors & 2) pucks[1]->setPropertyUInt32(PROP_M, encoderPositionTickCount);
00474 if (bhMotors & 4) pucks[2]->setPropertyUInt32(PROP_M, encoderPositionTickCount);
00475 if (bhMotors & 8) pucks[3]->setPropertyUInt32(PROP_M, encoderPositionTickCount);
00476
00477 waitForStop(bhMotors);
00478
00479 return 0;
00480 }
00481
00482 int BHandCANDriver::HandStepClose(BHMotors bhMotors, bool valueIncluded, int stepAmount)
00483 {
00484
00485
00486 if (valueIncluded)
00487
00488 HandSet(bhMotors, "DS", stepAmount);
00489
00490 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00491 if ((bhMotors >> i) & 1)
00492 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_IC);
00493
00494 waitForStop(bhMotors);
00495
00496 return 0;
00497 }
00498
00499 int BHandCANDriver::HandStepOpen(BHMotors bhMotors, bool valueIncluded, int stepAmount)
00500 {
00501
00502
00503 if (valueIncluded)
00504
00505 HandSet(bhMotors, "DS", stepAmount);
00506
00507 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00508 if ((bhMotors >> i) & 1)
00509 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_IO);
00510
00511 waitForStop(bhMotors);
00512
00513 return 0;
00514 }
00515
00516 int BHandCANDriver::HandStopMotor(BHMotors bhMotors)
00517 {
00518
00519
00520 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00521 if ((bhMotors >> i) & 1)
00522 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_T);
00523 DELAY(1);
00524
00525 return 0;
00526 }
00527
00528 int BHandCANDriver::HandTorqueClose(BHMotors bhMotors)
00529 {
00530 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00531 if ((bhMotors >> i) & 1)
00532 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_TC);
00533
00534 waitForStop(bhMotors);
00535
00536 return 0;
00537 }
00538
00539 int BHandCANDriver::HandTorqueOpen(BHMotors bhMotors)
00540 {
00541 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00542 if ((bhMotors >> i) & 1)
00543 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_TO);
00544
00545 waitForStop(bhMotors);
00546
00547 return 0;
00548 }
00549
00550 int BHandCANDriver::HandGet(BHMotors bhMotors, const char *property, int *propertyResult, int *nresults)
00551 {
00552
00553
00554 if (strcmp(property, "MCV") == 0 || strcmp(property, "MOV") == 0)
00555 return HandGet(bhMotors, "MV", propertyResult);
00556
00557
00558 unsigned int mCount = countMotors(bhMotors);
00559 for (unsigned m = 0; m < mCount; m++)
00560 propertyResult[m] = 0;
00561
00562
00563 int puckProperty = getPropertyValue(property);
00564 if (puckProperty == NONE)
00565 {
00566 pucksInHandRTGet(bhMotors, property, propertyResult);
00567 return -1;
00568 }
00569
00570
00571
00572
00573
00574 unsigned int pCount = 0;
00575
00576
00577 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00578 if ((bhMotors >> i) & 1)
00579 {
00580 pucks[i]->getPropertyUInt32(puckProperty, (unsigned int *)&propertyResult[pCount++]);
00581
00582
00583
00584 if (pCount == mCount)
00585 {
00586
00587 return 0;
00588 }
00589 }
00590
00591
00592
00593 return 0;
00594 }
00595
00596
00597 int BHandCANDriver::HandSet(BHMotors bhMotors, const char *property, int value)
00598 {
00599 if (strcmp(property, "MCV") == 0 || strcmp(property, "MOV") == 0)
00600 HandSet(bhMotors, "MV", value);
00601
00602
00603
00604
00605
00606 int puckProperty = getPropertyValue(property);
00607 if (puckProperty == NONE)
00608 {
00609 pucksInHandRTSet(bhMotors, property, value);
00610 return -1;
00611 }
00612
00613
00614 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00615 if ((bhMotors >> i) & 1)
00616 {
00617
00618 pucks[i]->setPropertyUInt32(puckProperty, value);
00619 }
00620
00621 return 0;
00622 }
00623
00624 int BHandCANDriver::HandPGet(const char *property, int *propertyResult)
00625 {
00626
00627 if (strcmp(property, "LFT") == 0)
00628 {
00629 pucksInHandRTGet(1, property, propertyResult);
00630 return 0;
00631 }
00632
00633
00634 *propertyResult = -9;
00635
00636 return 0;
00637 }
00638
00639 int BHandCANDriver::HandPSet(const char *property, int value)
00640 {
00641
00642
00643
00644 int puckProperty = getPropertyValue(property);
00645 if (puckProperty == NONE)
00646 {
00647 pucksInHandRTSet(0xf, property, value);
00648 return -1;
00649 }
00650
00651
00652
00653 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00654 pucks[i]->setPropertyUInt32(puckProperty, value);
00655
00656 return 0;
00657 }
00658
00659 int BHandCANDriver::HandDefault(BHMotors bhMotors)
00660 {
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697 int p, v;
00698
00699 int jidx[] = { 8, 9, 10, 11 };
00700 int pidx[] = { 3, 4, 1, 2 };
00701
00702 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00703 if ((bhMotors >> i) & 1)
00704 {
00705
00706
00707
00708
00709 for (int n = 0; n < pucksInHandPropertyNumDefault(); n++)
00710 {
00711 pucksInHandPropertyDefault(n, &p, &v);
00712 pucks[i]->setPropertyUInt32(p, v); DELAY(1);
00713 }
00714 if (i == 3)
00715 {
00716
00717 for (int n = 0; n < pucksInHandPropertyNumSpreadDefault(); n++)
00718 {
00719 pucksInHandPropertySpreadDefault(n, &p, &v);
00720 pucks[i]->setPropertyUInt32(p, v); DELAY(1);
00721 }
00722 }
00723
00724 pucks[i]->setPropertyUInt32(PROP_JIDX, jidx[i]); DELAY(1);
00725 pucks[i]->setPropertyUInt32(PROP_PIDX, pidx[i]); DELAY(1);
00726
00727
00728
00729 pucks[i]->setPropertyUInt32(PROP_SAVE, (unsigned int)-1);
00730
00731 printf("Puck %d saved default parameters\n", i);
00732
00733
00734
00735 }
00736
00737 DELAY(100);
00738
00739 return 0;
00740 }
00741
00742 int BHandCANDriver::HandLoad(BHMotors bhMotors)
00743 {
00744 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00745 if ((bhMotors >> i) & 1)
00746 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_LOAD);
00747 DELAY(1);
00748
00749 return 0;
00750 }
00751
00752 int BHandCANDriver::HandSave(BHMotors bhMotors)
00753 {
00754 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
00755 if ((bhMotors >> i) & 1)
00756 pucks[i]->setPropertyUInt8(PROP_CMD, CMD_SAVE);
00757 DELAY(100);
00758
00759 return 0;
00760 }
00761
00762 int BHandCANDriver::HandTemperature(BHMotors bhMotors, int *temperature, unsigned int numTemperatures)
00763 {
00764 int r = 0;
00765 for (unsigned int i = 0; i < numTemperatures; i++)
00766 if ((bhMotors >> i) & 1)
00767 if ((r = pucks[i]->getPropertyUInt32(PROP_TEMP, (unsigned int *)&temperature[i])))
00768 return r;
00769
00770 return 0;
00771 }
00772
00773 int BHandCANDriver::HandDelay(unsigned int msec)
00774 {
00775 BHiResTimer tmr;
00776 tmr.Start();
00777 while ((unsigned int)tmr.GetElapsedInMilliSecs() < msec)
00778 {
00779 if (waitCallbackFunc != NULL)
00780 waitCallbackFunc(puckTwos.bhand);
00781 DELAY(1);
00782 }
00783
00784 return 0;
00785 }
00786
00787 char * BHandCANDriver::HandCommand(const char *send, int *errorCode)
00788 {
00789
00790 *errorCode = 0;
00791 return NULL;
00792 }
00793
00794 int BHandCANDriver::HandBaud(unsigned int newbaud)
00795 {
00796
00797 return 0;
00798 }
00799
00800
00801
00802
00803 int BHandCANDriver::HandleSupervisoryCall()
00804 {
00805
00806 return 0;
00807 }
00808 int BHandCANDriver::ExecuteRealtimeCall()
00809 {
00810
00811
00812
00813 BPuck2Manager *puckManager = puckTwos.puckManager;
00814
00816
00818
00819 if (m_control)
00820 {
00821 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00822 {
00823 if (((realtimeGlobalParameters.MotorsInRealTime >> n) & 1) == 0)
00824 continue;
00825
00826 if (realtimeMotorParameters[n].ControlVelocity)
00827 pucks[n]->setPropertyUInt32(PROP_V, realtimeMotorParameters[n].ControlPresentVelocity);
00828 if (realtimeMotorParameters[n].ControlTorque)
00829 pucks[n]->setPropertyUInt32(PROP_T, realtimeMotorParameters[n].ControlPresentTorque);
00830 if (realtimeMotorParameters[n].ControlPosition)
00831 pucks[n]->setPropertyUInt32(PROP_P, realtimeMotorParameters[n].ControlPresentPosition);
00832 }
00833 }
00834
00836
00838
00839 bool someFeedback = false;
00840
00841 if (m_feedback)
00842 {
00843
00844 int positions[4] = { 0x7fffff, 0x7fffff, 0x7fffff, 0x7fffff};
00845 puckManager->startGetPropertyBatch();
00846 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00847 if (realtimeMotorParameters[n].FeedbackPosition)
00848 pucks[n]->addToGetPropertyBatch((unsigned int *)&positions[n]);
00849 puckManager->getBatchPropertyUInt32(PROP_P);
00850 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00851 {
00852 if (realtimeMotorParameters[n].FeedbackPosition)
00853 {
00854
00855 if (positions[n] != 0x7fffff)
00856 {
00857 bool negative = ((positions[n] & 0x800000) == 0x800000);
00858 positions[n] = negative ?
00859 (positions[n] | 0xff000000) :
00860 (positions[n] & 0xffffff);
00861 realtimeMotorParameters[n].FeedbackPresentPosition = positions[n];
00862 someFeedback = true;
00863 }
00864 }
00865 }
00866
00867
00868 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00869 {
00870 if (realtimeMotorParameters[n].FeedbackVelocity)
00871 {
00872 double elapsedMilliSeconds = realtimeMotorParameters[n].tmr.GetElapsedInMilliSecs();
00873 if (elapsedMilliSeconds >= 1)
00874 {
00875 int newPosition = realtimeMotorParameters[n].FeedbackPresentPosition;
00876 int lastPosition = realtimeMotorParameters[n].velLastPositionEncoderTicks;
00877 realtimeMotorParameters[n].velLastPositionEncoderTicks = newPosition;
00878
00879 realtimeMotorParameters[n].FeedbackPresentVelocity = (int)((double)(newPosition - lastPosition) / elapsedMilliSeconds);
00880
00881 realtimeMotorParameters[n].tmr.Start();
00882
00883
00884
00885 }
00886 someFeedback = true;
00887 }
00888 }
00889
00890
00891 int strain[4] = { 4096, 4096, 4096, 4096};
00892 puckManager->startGetPropertyBatch();
00893 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00894 if (realtimeMotorParameters[n].FeedbackStrain)
00895 pucks[n]->addToGetPropertyBatch((unsigned int *)&strain[n]);
00896 puckManager->getBatchPropertyUInt32(PROP_SG);
00897 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00898 {
00899 if (realtimeMotorParameters[n].FeedbackStrain &&
00900 strain[n] >= 0 && strain[n] < 4096)
00901 {
00902
00903 realtimeMotorParameters[n].FeedbackPresentStrain = strain[n];
00904 someFeedback = true;
00905 }
00906 }
00907
00908
00909 if (realtimeGlobalParameters.FeedbackTemperature)
00910 {
00911 int temps[4] = { 0, 0, 0, 0};
00912 puckManager->startGetPropertyBatch();
00913 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00914 pucks[n]->addToGetPropertyBatch((unsigned int *)&temps[n]);
00915 puckManager->getBatchPropertyUInt32(PROP_TEMP);
00916 realtimeGlobalParameters.Temperature = 0;
00917 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00918 realtimeGlobalParameters.Temperature = MAX(realtimeGlobalParameters.Temperature, temps[n]);
00919 }
00920
00921
00922 int pps = 0;
00923 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
00924 pps |= realtimeMotorParameters[n].FeedbackPPS ? (1 << n) : 0;
00925 if (pps)
00926 {
00927 RTUpdatePPS(pps);
00928 someFeedback = true;
00929 }
00930 }
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970 if (realtimeGlobalParameters.motorProtect & BHMotorTorqueLimitProtect)
00971 {
00972 RTHandProtectMotors();
00973 someFeedback = true;
00974 }
00975
00976
00977 if (!someFeedback)
00978 {
00979 DELAY(1);
00980 }
00981
00982 return 0;
00983
00984 }
00985
00986
00987
00988
00990
00992
00993 int BHandCANDriver::RTStart(const char *motor, BHMotorProtection motorProtection)
00994 {
00995 if (m_realtimeMode)
00996 return -1;
00997
00998
00999 BHMotors bhMotors = toBHMotors(motor);
01000 realtimeGlobalParameters.MotorsInRealTime = bhMotors;
01001
01002
01003 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01004 {
01005 if ((bhMotors >> n) & 1)
01006 {
01007 int nModes = 0;
01008 if (realtimeMotorParameters[n].ControlVelocity) nModes++;
01009 if (realtimeMotorParameters[n].ControlTorque) nModes++;
01010 if (realtimeMotorParameters[n].ControlPosition) nModes++;
01011
01012 if (nModes != 1)
01013 {
01014 printf("pucksInHandRTStart - nModes != 1\n");
01015 return -1;
01016 }
01017 }
01018 }
01019
01020
01021 realtimeGlobalParameters.motorProtect = motorProtection;
01022 int r;
01023 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01024 {
01025 if ((bhMotors >> n) & 1)
01026 {
01027 if (realtimeGlobalParameters.motorProtect & BHMotorTSTOPProtect)
01028 {
01029
01030 r = pucks[n]->setPropertyUInt32(PROP_TSTOP, BHAND_DEFAULT_TSTOP);
01031 if (r) return r;
01032 }
01033 else if (realtimeGlobalParameters.motorProtect & BHMotorTorqueLimitProtect)
01034 {
01035
01036 r = pucks[n]->setPropertyUInt32(PROP_TSTOP, 0); if (r) return r;
01037 }
01038 else
01039 {
01040
01041 r = pucks[n]->setPropertyUInt32(PROP_TSTOP, BHAND_DEFAULT_TSTOP);
01042 if (r) return r;
01043 }
01044 }
01045 }
01046
01047
01048 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01049 {
01050 realtimeMotorParameters[n].Stopped = true;
01051 if ((bhMotors >> n) & 1)
01052 {
01053 int numTries = 0;
01054 while (realtimeMotorParameters[n].Stopped)
01055 {
01056 if (numTries++ == 2)
01057 return -1;
01058 if (realtimeMotorParameters[n].ControlTorque)
01059 {
01060 realtimeMotorParameters[n].ControlPresentTorque = 0;
01061 r = pucks[n]->setPropertyUInt32(PROP_T, 0); if (r) return r;
01062 r = pucks[n]->setPropertyUInt32(PROP_MODE, 2); if (r) return r;
01063 }
01064 if (realtimeMotorParameters[n].ControlPosition)
01065 {
01066 r = pucks[n]->getPropertyUInt32(PROP_P, (unsigned int *)&(realtimeMotorParameters[n].ControlPresentPosition)); if (r) return r;
01067 r = pucks[n]->setPropertyUInt32(PROP_MODE, 3); if (r) return r;
01068 }
01069 if (realtimeMotorParameters[n].ControlVelocity)
01070 {
01071
01072 realtimeMotorParameters[n].ControlPresentVelocity = 0;
01073 r = pucks[n]->setPropertyUInt32(PROP_V, 0); if (r) return r;
01074 r = pucks[n]->setPropertyUInt32(PROP_MODE, 4); if (r) return r;
01075 }
01076
01077
01078 unsigned int mode;
01079 r = pucks[n]->getPropertyUInt32(PROP_MODE, &mode); if (r) return r;
01080 if ((realtimeMotorParameters[n].ControlTorque && mode == 2) ||
01081 (realtimeMotorParameters[n].ControlPosition && mode == 3) ||
01082 (realtimeMotorParameters[n].ControlVelocity && mode == 4))
01083 realtimeMotorParameters[n].Stopped = false;
01084
01085 if (realtimeMotorParameters[n].Stopped)
01086 printf("Inner Mode not set, trying again %d\n", n);
01087 }
01088 }
01089 }
01090
01091
01092
01093 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01094 {
01095 if ((bhMotors >> n) & 1)
01096 {
01097 if (realtimeMotorParameters[n].FeedbackVelocity)
01098 {
01099 realtimeMotorParameters[n].FeedbackPosition = true;
01100 realtimeMotorParameters[n].FeedbackPresentVelocity = 0;
01101
01102 r = pucks[n]->getPropertyUInt32(PROP_P, (unsigned int *)&(realtimeMotorParameters[n].velLastPositionEncoderTicks)); if (r) return r;
01103 realtimeMotorParameters[n].tmr.Start();
01104 }
01105 }
01106 }
01107
01108 updateTmr.Start();
01109
01110
01111 m_realtimeMode = true;
01112
01113 return 0;
01114 }
01115
01116 int BHandCANDriver::RTSetFlags(const char *motor, bool LCV, int LCVC, bool LCPG,
01117 bool LFV, int LFVC, bool LFS, bool LFAP, bool LFDP, int LFDPC)
01118 {
01119 BHMotors bhmotors = toBHMotors(motor);
01120
01121 if (pucksInHandRTSet(bhmotors, "LCV", LCV)) return -1;
01122
01123
01124 if (pucksInHandRTSet(bhmotors, "LFV", LFV)) return -1;
01125
01126 if (pucksInHandRTSet(bhmotors, "LFS", LFS)) return -1;
01127 if (pucksInHandRTSet(bhmotors, "LFAP", LFAP)) return -1;
01128
01129
01130
01131 return 0;
01132 }
01133
01134 int BHandCANDriver::RTSetFlags(const char *motor, bool LCV, int LCVC, bool LCPG, bool LCT,
01135 bool LFV, int LFVC, bool LFS, bool LFAP, bool LFDP, int LFDPC,
01136 bool LFBP, bool LFAIN, bool LFDPD, bool LFT)
01137 {
01138 BHMotors bhmotors = toBHMotors(motor);
01139
01140 if (pucksInHandRTSet(bhmotors, "LCV", LCV)) return -1;
01141
01142
01143 if (pucksInHandRTSet(bhmotors, "LCT", LCT)) return -1;
01144
01145 if (pucksInHandRTSet(bhmotors, "LFV", LFV)) return -1;
01146
01147 if (pucksInHandRTSet(bhmotors, "LFS", LFS)) return -1;
01148 if (pucksInHandRTSet(bhmotors, "LFAP", LFAP)) return -1;
01149
01150
01151
01152
01153 return 0;
01154 }
01155
01156
01157 void BHandCANDriver::RTHandProtectMotors()
01158 {
01159 BPuck2Manager *puckManager = puckTwos.puckManager;
01160
01161
01162 updateTmr.Stop();
01163 double elapsed = updateTmr.GetDurationInMilliSecs();
01164 updateTmr.Start();
01165
01166
01167 int torque[4] = { BHAND_FINGER_MAX_PEAK_TORQUE, BHAND_FINGER_MAX_PEAK_TORQUE, BHAND_FINGER_MAX_PEAK_TORQUE, BHAND_SPREAD_MAX_PEAK_TORQUE};
01168 puckManager->startGetPropertyBatch();
01169 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01170 pucks[n]->addToGetPropertyBatch((unsigned int *)&torque[n]);
01171 puckManager->getBatchPropertyUInt32(PROP_T);
01172
01173 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01174 {
01175
01176 if (torque[n] > BHAND_FINGER_MAX_PEAK_TORQUE)
01177 torque[n] = BHAND_FINGER_MAX_PEAK_TORQUE;
01178 else if (torque[n] < -BHAND_FINGER_MAX_PEAK_TORQUE)
01179 torque[n] = -BHAND_FINGER_MAX_PEAK_TORQUE;
01180
01181
01182 const float EXP = 0.99997f;
01183 float e = pow(EXP, (float)elapsed);
01184 realtimeMotorParameters[n].texp = (float)(realtimeMotorParameters[n].texp * e + (torque[n]*torque[n]) * (1 - e));
01185
01186 if (n < 3)
01187
01188 pucks[n]->setPropertyUInt32(PROP_MT,
01189 (realtimeMotorParameters[n].texp < BHAND_FINGER_MAX_CONT_TORQUE * BHAND_FINGER_MAX_CONT_TORQUE) ?
01190 BHAND_FINGER_MAX_PEAK_TORQUE : BHAND_FINGER_MAX_CONT_TORQUE);
01191 else
01192
01193 pucks[n]->setPropertyUInt32(PROP_MT,
01194 (realtimeMotorParameters[n].texp < BHAND_SPREAD_MAX_CONT_TORQUE * BHAND_SPREAD_MAX_CONT_TORQUE) ?
01195 BHAND_SPREAD_MAX_PEAK_TORQUE : BHAND_SPREAD_MAX_CONT_TORQUE);
01196 }
01197
01198
01199
01200
01201
01202
01203
01204 }
01205
01206
01207
01208 int BHandCANDriver::RTUpdate(const char *motor, const char *property, int *values)
01209 {
01210 BHMotors bhMotors = toBHMotors(motor);
01211
01212
01213 int puckProperty = getPropertyValue(property);
01214 if (puckProperty == NONE)
01215 return -1;
01216
01217 BPuck2Manager *puckManager = puckTwos.puckManager;
01218
01219
01220 puckManager->startGetPropertyBatch();
01221 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01222 if ((bhMotors >> n) & 1)
01223 pucks[n]->addToGetPropertyBatch((unsigned int *)&values[n]);
01224
01225
01226 return puckManager->getBatchPropertyUInt32(puckProperty);
01227 }
01228
01229 int BHandCANDriver::RTUpdate(bool control, bool feedback)
01230 {
01231 m_control = control;
01232 m_feedback = feedback;
01233 return m_moduleSuperReal->ComRequest(BHREQ_REALTIME);
01234 }
01235
01236 int BHandCANDriver::RTUpdatePPS(BHMotors bhMotors)
01237 {
01238 BPuck2Manager *puckManager = puckTwos.puckManager;
01239
01240 unsigned int numMotors = 0;
01241
01242
01243 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01244 if ((bhMotors >> n) & 1)
01245 {
01246 if (!realtimeMotorParameters[n].ControlPosition)
01247 {
01248 pucks[n]->setPropertyUInt32(PROP_TACT, 2);
01249 numMotors++;
01250 }
01251 }
01252
01253 for (unsigned int n = 0; n < numMotors * 5; n++)
01254 {
01255 int puckID;
01256 unsigned char data[8];
01257 int dataLength;
01258
01259
01260 BHiResTimer tmr;
01261 tmr.Start();
01262 int err = 1;
01263 while (err)
01264 {
01265 err = puckManager->readMsgAsync(&puckID, data, &dataLength);
01266 if (tmr.GetElapsedInMilliSecs() > 1000)
01267 return -1;
01268 }
01269
01270
01271 if (dataLength != 8)
01272 return -1;
01273
01274
01275 for (unsigned int m = 0; m < 4; m++)
01276 {
01277 if (pucks[m]->getID() != (unsigned int)puckID)
01278 continue;
01279
01280
01281 int i = (data[0] >> 4) * 5;
01282
01283 #ifdef PPS_DEMO
01284 const int scalePressure = 100;
01285
01286
01287 if (i < MAX_PPS_ELEMENTS)
01288 {
01289 FeedbackPresentPPS[m][i++][filterIndexPPS] = scalePressure * (((int)data[0]&0x000F)<<8 | ((int)data[1]&0x00FF));
01290 if (i < MAX_PPS_ELEMENTS)
01291 {
01292 FeedbackPresentPPS[m][i++][filterIndexPPS] = scalePressure * (((int)data[2]&0x00FF)<<4 | ((int)data[3]&0x00F0)>>4);
01293 if (i < MAX_PPS_ELEMENTS)
01294 {
01295 FeedbackPresentPPS[m][i++][filterIndexPPS] = scalePressure * (((int)data[3]&0x000F)<<8 | ((int)data[4]&0x00FF));
01296 if (i < MAX_PPS_ELEMENTS)
01297 {
01298 FeedbackPresentPPS[m][i++][filterIndexPPS] = scalePressure * (((int)data[5]&0x00FF)<<4 | ((int)data[6]&0x00F0)>>4);
01299 if (i < MAX_PPS_ELEMENTS)
01300 {
01301 FeedbackPresentPPS[m][i++][filterIndexPPS] = scalePressure * (((int)data[6]&0x000F)<<8 | ((int)data[7]&0x00FF));
01302 }
01303 }
01304 }
01305 }
01306 }
01307 #else
01308
01309 if (i < MAX_PPS_ELEMENTS)
01310 {
01311 realtimeMotorParameters[m].FeedbackPresentPPS[i++] = ((int)data[0]&0x000F)<<8 | ((int)data[1]&0x00FF);
01312 if (i < MAX_PPS_ELEMENTS)
01313 {
01314 realtimeMotorParameters[m].FeedbackPresentPPS[i++] = ((int)data[2]&0x00FF)<<4 | ((int)data[3]&0x00F0)>>4;
01315 if (i < MAX_PPS_ELEMENTS)
01316 {
01317 realtimeMotorParameters[m].FeedbackPresentPPS[i++] = ((int)data[3]&0x000F)<<8 | ((int)data[4]&0x00FF);
01318 if (i < MAX_PPS_ELEMENTS)
01319 {
01320 realtimeMotorParameters[m].FeedbackPresentPPS[i++] = ((int)data[5]&0x00FF)<<4 | ((int)data[6]&0x00F0)>>4;
01321 if (i < MAX_PPS_ELEMENTS)
01322 {
01323 realtimeMotorParameters[m].FeedbackPresentPPS[i++] = ((int)data[6]&0x000F)<<8 | ((int)data[7]&0x00FF);
01324 }
01325 }
01326 }
01327 }
01328 }
01329 #endif
01330
01331 }
01332
01333 }
01334
01335 #ifdef PPS_DEMO
01336
01337
01338 static int ncount = 0;
01339
01340 for (unsigned int m = 0; m < 4; m++)
01341 {
01342 for (int i = 0; i < MAX_PPS_ELEMENTS; i++)
01343 {
01344 realtimeMotorParameters[m].FeedbackPresentPPS[i] = 0;
01345 for (int n = 0; n < PPS_FILTER_SIZE; n++)
01346 {
01347 realtimeMotorParameters[m].FeedbackPresentPPS[i] += FeedbackPresentPPS[m][i][n];
01348 }
01349 realtimeMotorParameters[m].FeedbackPresentPPS[i] /= PPS_FILTER_SIZE;
01350 }
01351
01352 if (ncount++ == 300 * 3)
01353 {
01354 for (int i = 0; i < MAX_PPS_ELEMENTS; i++)
01355 {
01356 TaredPPS[m][i] = realtimeMotorParameters[m].FeedbackPresentPPS[i];
01357 ncount = 0;
01358 }
01359 }
01360
01361
01362 for (int i = 0; i < MAX_PPS_ELEMENTS; i++)
01363 {
01364 realtimeMotorParameters[m].FeedbackPresentPPS[i] -= TaredPPS[m][i];
01365 }
01366
01367
01368 if (m == 3)
01369 {
01370 printf("\n");
01371 for (int i = 0; i < 15; i++)
01372 {
01373 printf("%4d ", realtimeMotorParameters[m].FeedbackPresentPPS[i]);
01374 }
01375 printf("\r");
01376 }
01377 }
01378
01379 if (++filterIndexPPS == PPS_FILTER_SIZE)
01380 filterIndexPPS = 0;
01381 #endif
01382
01383 return 0;
01384 }
01385
01386 int BHandCANDriver::RTAbort()
01387 {
01388 int result;
01389 for (unsigned int n = 0; n < puckTwos.pucksWithMotors; n++)
01390 {
01391 if (((realtimeGlobalParameters.MotorsInRealTime >> n) & 1) == 0)
01392 continue;
01393
01394
01395 result = pucks[n]->setPropertyUInt32(PROP_TSTOP, BHAND_DEFAULT_TSTOP);
01396 result = pucks[n]->setPropertyUInt32(PROP_MODE, 0);
01397 result = pucks[n]->setPropertyUInt32(PROP_MT, (n < 3) ? BHAND_FINGER_MAX_PEAK_TORQUE : BHAND_SPREAD_MAX_PEAK_TORQUE);
01398 }
01399
01400 m_realtimeMode = false;
01401 return 0;
01402 }
01403
01404 int BHandCANDriver::pucksInHandRTSet(BHMotors bhMotors, const char *property, int value)
01405 {
01406
01407
01408 if (m_realtimeMode)
01409 return -1;
01410
01411
01412 int rtMotorProperty = getPropertyValue(property, propBHRealTimeMotor);
01413 if (rtMotorProperty == NONE)
01414 return -1;
01415
01416
01417
01418
01419 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
01420 {
01421 if ((bhMotors >> i) & 1)
01422 {
01423 switch (rtMotorProperty)
01424 {
01425 case REALTIME_LCV: { realtimeMotorParameters[i].ControlVelocity = (value ? 1 : 0); break; }
01426 case REALTIME_LCT: { realtimeMotorParameters[i].ControlTorque = (value ? 1 : 0); break; }
01427 case REALTIME_LCP: { realtimeMotorParameters[i].ControlPosition = (value ? 1 : 0); break; }
01428 case REALTIME_LFV: { realtimeMotorParameters[i].FeedbackVelocity = (value ? 1 : 0); break; }
01429 case REALTIME_LFS: { realtimeMotorParameters[i].FeedbackStrain = (value ? 1 : 0); break; }
01430 case REALTIME_LFAP: { realtimeMotorParameters[i].FeedbackPosition = (value ? 1 : 0); break; }
01431 case REALTIME_LFPPS: { realtimeMotorParameters[i].FeedbackPPS = (value ? 1 : 0); break; }
01432 case REALTIME_LFT: { realtimeGlobalParameters.FeedbackTemperature = (value ? 1 : 0); break; }
01433 }
01434 }
01435 }
01436
01437 return 0;
01438 }
01439
01440 int BHandCANDriver::pucksInHandRTGet(BHMotors bhMotors, const char *property, int *value)
01441 {
01442
01443
01444 if (m_realtimeMode)
01445 return -1;
01446
01447
01448 int rtMotorProperty = getPropertyValue(property, propBHRealTimeMotor);
01449 if (rtMotorProperty == NONE)
01450 return -1;
01451
01452
01453
01454 if (rtMotorProperty == REALTIME_LFT)
01455 {
01456 value[0] = realtimeGlobalParameters.FeedbackTemperature ? 1 : 0;
01457 return 0;
01458 }
01459
01460
01461 unsigned int pCount = 0;
01462 for (unsigned int i = 0; i < puckTwos.pucksWithMotors; i++)
01463 {
01464 if ((bhMotors >> i) & 1)
01465 {
01466 switch (rtMotorProperty)
01467 {
01468 case REALTIME_LCV: { value[pCount] = realtimeMotorParameters[i].ControlVelocity ? 1 : 0; break; }
01469 case REALTIME_LCT: { value[pCount] = realtimeMotorParameters[i].ControlTorque ? 1 : 0; break; }
01470 case REALTIME_LCP: { value[pCount] = realtimeMotorParameters[i].ControlPosition ? 1 : 0; break; }
01471 case REALTIME_LFV: { value[pCount] = realtimeMotorParameters[i].FeedbackVelocity ? 1 : 0; break; }
01472 case REALTIME_LFS: { value[pCount] = realtimeMotorParameters[i].FeedbackStrain ? 1 : 0; break; }
01473 case REALTIME_LFAP: { value[pCount] = realtimeMotorParameters[i].FeedbackPosition ? 1 : 0; break; }
01474 case REALTIME_LFPPS: { value[pCount] = realtimeMotorParameters[i].FeedbackPPS ? 1 : 0; break; }
01475 }
01476 pCount++;
01477 }
01478 }
01479 return 0;
01480 }
01481
01482
01483
01484 int BHandCANDriver::RTSetVelocity(const char motor, int velocity)
01485 {
01486 int n = motor - '1';
01487
01488 if (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors)
01489 {
01490 realtimeMotorParameters[n].ControlPresentVelocity = velocity;
01491 return 0;
01492 }
01493 else
01494 return -1;
01495 }
01496 int BHandCANDriver::RTSetGain(const char motor, int gain)
01497 {
01498 return 0;
01499 }
01500 int BHandCANDriver::RTSetTorque(const char motor, int torque)
01501 {
01502 int n = motor - '1';
01503
01504 if (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors)
01505 {
01506 realtimeMotorParameters[n].ControlPresentTorque = torque;
01507 return 0;
01508 }
01509 else
01510 return -1;
01511 }
01512 int BHandCANDriver::RTSetPosition(const char motor, int position)
01513 {
01514 int n = motor - '1';
01515
01516 if (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors)
01517 {
01518 realtimeMotorParameters[n].ControlPresentPosition = position;
01519 return 0;
01520 }
01521 else
01522 return -1;
01523 }
01524
01525
01526 char BHandCANDriver::RTGetVelocity(const char motor)
01527 {
01528 int n = motor - '1';
01529 if (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors)
01530 {
01531
01532 if (realtimeMotorParameters[n].FeedbackPresentVelocity > 127)
01533 return 127;
01534 else if (realtimeMotorParameters[n].FeedbackPresentVelocity < -127)
01535 return -127;
01536
01537 return realtimeMotorParameters[n].FeedbackPresentVelocity;
01538 }
01539 return 0;
01540 }
01541 unsigned char BHandCANDriver::RTGetStrain(const char motor)
01542 {
01543 int n = motor - '1';
01544 return (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors) ? (realtimeMotorParameters[n].FeedbackPresentStrain >> 4) : 0;
01545 }
01546 int BHandCANDriver::RTGetPosition(const char motor)
01547 {
01548 int n = motor - '1';
01549 return (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors) ? realtimeMotorParameters[n].FeedbackPresentPosition : 0;
01550 }
01551 char BHandCANDriver::RTGetDeltaPos(const char motor)
01552 {
01553 return 0;
01554 }
01555 int BHandCANDriver::RTGetBreakawayPosition(const char motor)
01556 {
01557 return 0;
01558 }
01559 int BHandCANDriver::RTGetTemp()
01560 {
01561 return realtimeGlobalParameters.Temperature;
01562 }
01563 unsigned char BHandCANDriver::RTGetAIN(const char motor)
01564 {
01565 return 0;
01566 }
01567 void BHandCANDriver::RTGetPPS(const char motor, int *pps, int ppsElements)
01568 {
01569 int n = motor - '1';
01570 if (n >= 0 && (unsigned int)n < puckTwos.pucksWithMotors)
01571 {
01572 for (int i = 0; i < ppsElements; i++)
01573 pps[i] = realtimeMotorParameters[n].FeedbackPresentPPS[i];
01574 }
01575 }
01576
01577
01578 void BHandCANDriver::SetWaitCallbackFunc(BHCallback waitCallbackFunc) { this->waitCallbackFunc = waitCallbackFunc; }