RobotArm.cpp

00001 // Copyright (c) Microsoft. All rights reserved.
00002 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
00003 
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 
00007 #include <vector>
00008 
00009 #include "DynamixelBus.h"
00010 #include "NodeAX12.h"
00011 #include "NodeEmul.h"
00012 #include "RobotArm.h"
00013 
00014 using namespace std;
00015 
00016 // create the bus interface for this device
00017 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 );
00018 
00019 
00020 // default to move every 25 ms
00021 #define StepPeriodMs        25
00022 
00023 // Thresholds
00024 // allow 3 degrees plus requested move diff for position error
00025 #define PositionErrorAllow  3.0f
00026 // time for continuous position error
00027 #define FailMsLimit         250
00028 // load level allowance
00029 #define MaxLoadLimit        950.0f
00030 // Temperature limit
00031 #define MaxTemp             69
00032 // Voltage limits
00033 #define MaxVoltLimit        13
00034 #define MinVoltLimit        10
00035 
00036 // should arm test for positions ?
00037 #define TestPositions       false
00038 
00039 
00040 RobotArm::RobotArm()
00041 {
00042     // build arm
00043     for (int ix = 0; ix < NUMJOINTS; ix++)
00044     {
00045         NodePartType nt = ArmJoints[ix].JointType;
00046         switch (nt)
00047         {
00048             case NT_AX12:
00049                 _armParts[ix] = dynamic_cast<RobotNode*>(new NodeAX12(&dynbus, ArmJoints[ix].JointId));
00050                 break;
00051                 
00052             case NT_Emul:
00053                 _armParts[ix] = dynamic_cast<RobotNode*>(new NodeEmul(ArmJoints[ix].JointId));
00054                 break;
00055                 
00056             default:
00057                 printf("Error! Unknown node type defined");
00058                 _armParts[ix] = NULL;
00059                 break;
00060         }
00061         
00062         _armParts[ix]->DoAction(NA_Init, 0.0f);
00063     }
00064     
00065     _numParts = NUMJOINTS;
00066 
00067     _numsteps = 0;
00068     _stepms = StepPeriodMs;             
00069 }
00070 
00071 
00072 void RobotArm::ClearErrorState()
00073 {
00074     for (int ix = 0; ix < _numParts; ix++)
00075     {
00076         _armParts[ix]->DoAction(NA_ClearError, 0.0f);
00077         _failms[ix] = 0;
00078     }
00079 }
00080 
00081 // move all parts to specified postions in ms time
00082 bool RobotArm::MoveArmPositionsStart(float positions[], int totms)
00083 {
00084     _lastErrorPart = 0;
00085 
00086     MoveArmPositionsEnd();
00087     
00088     GetArmPositions(_lastpos);
00089     
00090     _numsteps = totms / _stepms;
00091     if (_numsteps == 0) _numsteps = 1;
00092     
00093     for (int ix = 0; ix < _numParts; ix++)
00094     {
00095         if (positions[ix] > 0.0f)
00096         {
00097             _endgoals[ix] = positions[ix];
00098             float difference = (positions[ix] - _lastpos[ix]) / (float)_numsteps;
00099             _differentials[ix] = difference;
00100         }
00101         else
00102         {
00103             // negative goal. Treat as don't move
00104             _differentials[ix] = 0.0f;
00105         }
00106         _failms[ix] = 0;
00107     }
00108 
00109     _curstep = 1;
00110     
00111     _delayms = _stepms;
00112     
00113     _elapseTimer.start();
00114     _expDelay = (int)_elapseTimer.read_ms() + _delayms;
00115     
00116     return true;
00117 }
00118 
00119 // continue interrupted action
00120 bool RobotArm::MoveArmPositionsResume()
00121 {
00122     if (_curstep > _numsteps)
00123     {
00124         // no more steps 
00125         MoveArmPositionsEnd();
00126         return true;
00127     }
00128     GetArmPositions(_lastpos);
00129     
00130     // reset numsteps to be what was remaining
00131     _numsteps = _numsteps - _curstep + 1;
00132     
00133     for (int ix = 0; ix < _numParts; ix++)
00134     {
00135         if (_endgoals[ix] > 0.0f)
00136         {
00137             float difference = (_endgoals[ix] - _lastpos[ix]) / (float)_numsteps;
00138             _differentials[ix] = difference;
00139         }
00140         else
00141         {
00142             // negative goal. Treat as don't move
00143             _differentials[ix] = 0.0f;
00144         }
00145         _failms[ix] = 0;
00146     }
00147 
00148     _curstep = 1;
00149     
00150     _delayms = _stepms;
00151     
00152     _elapseTimer.start();
00153     _expDelay = (int)_elapseTimer.read_ms() + _delayms;
00154     
00155     return true;
00156 }
00157 
00158 bool RobotArm::MoveArmPositionsHasNext()
00159 {
00160     return (_curstep <= _numsteps);
00161 }
00162 
00163 bool RobotArm::MoveArmPositionsNext()
00164 {
00165     _lastErrorPart = 0;
00166 
00167     if (_curstep > _numsteps)
00168     {
00169         // no more steps 
00170         MoveArmPositionsEnd();
00171         return true;
00172     }
00173     
00174     bool ok = true;
00175 
00176     for (int ix = 0; ix < _numParts; ix++)
00177     {
00178         if (_armParts[ix]->HasAction(NA_Rotate))
00179         {
00180             float goal = (_curstep == _numsteps || _differentials[ix] == 0.0f) ?
00181                             _endgoals[ix] :  // last step - use actual goal
00182                             (_lastpos[ix] + (_differentials[ix] * (float)_curstep));
00183             _lastgoals[ix] = goal;
00184             if (_differentials[ix] != 0.0f)
00185             {
00186                 bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
00187                 if (!ok)
00188                 {
00189                     _lastErrorPart = ix;
00190                     _lastError = _armParts[_lastErrorPart]->GetLastError();
00191                     _lastPosDiff = 0;
00192                     break;
00193                 }
00194             }
00195         }
00196     }
00197     
00198     if (!ok)
00199     {
00200         return false;
00201     }
00202     
00203     _curstep++;
00204     if (_curstep > _numsteps)
00205     {
00206         MoveArmPositionsEnd();
00207     }
00208     
00209     return true;
00210 }
00211 
00212 // calculate actual delay until expDelay
00213 bool RobotArm::MoveArmPositionsDelay(int& nextdelay)
00214 {
00215     if (_curstep <= _numsteps)
00216     {
00217         int elapsed = (int)_elapseTimer.read_ms();
00218     
00219         if (elapsed <= _expDelay)
00220         {
00221             if (_expDelay - elapsed < _delayms)
00222                 nextdelay = _expDelay - elapsed;
00223             else
00224                 nextdelay = _delayms;
00225             // set next expected time by adding step delay
00226             _expDelay += _delayms;
00227         }
00228         else
00229         {
00230             nextdelay = _delayms;
00231            // set next expected time to now plus step delay
00232             _expDelay = elapsed + _delayms;
00233         }
00234     }
00235     else
00236     {
00237         nextdelay = _delayms;
00238     }
00239     
00240     return true;
00241 }
00242 
00243 // set goal to current position
00244 // prevents jump when obstruction is removed
00245 void RobotArm::MoveArmPositionsStop()
00246 {
00247     float curpos[NUMJOINTS];
00248     GetArmPositions(curpos);
00249     
00250     for (int ix = 0; ix < _numParts; ix++)
00251     {
00252         (void)_armParts[ix]->DoAction(NA_Rotate, curpos[ix]);
00253     }
00254 }
00255 
00256 bool RobotArm::MoveArmPositionsEnd()
00257 {
00258     if (_numsteps > 0)
00259     {
00260         _elapseTimer.stop();
00261         _numsteps = 0;
00262     }
00263     return true;
00264 }
00265 
00266 // clear cache to force a read
00267 void RobotArm::ArmMeasuresTestStart()
00268 {
00269     for (int ix = 0; ix < _numParts; ix++)
00270     {
00271         _armParts[ix]->ClearMeasureCache();
00272     }
00273 }
00274 
00275 // test values without clearing cache
00276 int RobotArm::ArmMeasuresTest(int measureId)
00277 {
00278     float curvals[NUMJOINTS];
00279     
00280     if (!GetArmLastMeasure(measureId, curvals))
00281     {
00282         return -2;
00283     }
00284     
00285     int rc = 0;
00286     
00287     switch (measureId)
00288     {
00289         case NM_Temperature:
00290             for (int ix = 0; ix < _numParts; ix++)
00291             {
00292                 float val = curvals[ix];
00293                 if (val > MaxTemp)
00294                 {
00295                     _lastErrorPart = ix;
00296                     rc = -1;
00297                     break;
00298                 }
00299             }
00300             break;
00301             
00302         case NM_Degrees:
00303             if (TestPositions)
00304             {
00305                 // positions will frequently be off while arm is moving
00306                 // logic checks if position is outside expected threshold for a period of time
00307                 // may get false positives if goal is changed frequently
00308                 for (int ix = 0; ix < _numParts; ix++)
00309                 {
00310                     float val = curvals[ix];
00311                     if (val > 0.0f)
00312                     {
00313                         float diff = fabs(val  - _lastgoals[ix]);
00314                         if (diff > (fabs(_differentials[ix] * 2.0f) + PositionErrorAllow))
00315                         {
00316                             int elapsed = (int)_elapseTimer.read_ms();
00317                             if (_failms[ix] > 0)
00318                             {
00319                                 if (elapsed - _failms[ix] > FailMsLimit)
00320                                 {
00321                                     // continuous failure for time period
00322                                     // report failure
00323                                     _lastPosDiff = diff;
00324                                     _lastErrorPart = ix;
00325             
00326                                     _failms[ix] = 0;
00327                                     rc = -1;
00328                                 }
00329                             }
00330                             else
00331                             {
00332                                 // first failure after success
00333                                 // remember first fail time.
00334                                 _failms[ix] = elapsed;
00335                             }
00336                         }
00337                         else
00338                         {
00339                             // within allowable range - clear time
00340                             _failms[ix] = 0;
00341                         }
00342                     }
00343                 }
00344             }
00345             break;
00346         
00347         case NM_Voltage:
00348             for (int ix = 0; ix < _numParts; ix++)
00349             {
00350                 float val = curvals[ix];
00351                 if (val > MaxVoltLimit || val < MinVoltLimit)
00352                 {
00353                     _lastErrorPart = ix;
00354                     rc = -1;
00355                     break;
00356                 }
00357             }
00358             break;
00359             
00360         case NM_Load:
00361             for (int ix = 0; ix < _numParts; ix++)
00362             {
00363                 float val = curvals[ix];
00364                 if (val > MaxLoadLimit)
00365                 {
00366                     _lastErrorPart = ix;
00367                     rc = -1;
00368                     break;
00369                 }
00370             }
00371             break;
00372 
00373             
00374         default:
00375             break;
00376     }
00377 
00378     return rc;
00379 }
00380 
00381 // get all parts positions
00382 bool RobotArm::GetArmPositions(float outPos[])
00383 {
00384     bool ok = true;
00385     for (int ix = 0; ix < _numParts; ix++)
00386     {
00387         _armParts[ix]->ClearMeasureCache();
00388         float pos = _armParts[ix]->GetMeasure(NM_Degrees);
00389         outPos[ix] = pos;
00390         if (_armParts[ix]->HasError())
00391         {
00392             _lastErrorPart = ix;
00393             _lastError = _armParts[ix]->GetLastError();
00394             ok = false;
00395         }
00396     }
00397     return ok;
00398 }
00399 
00400 // get all parts last measured positions
00401 bool RobotArm::GetArmLastPositions(float outPos[])
00402 {
00403     bool ok = true;
00404     for (int ix = 0; ix < _numParts; ix++)
00405     {
00406         float pos = _armParts[ix]->GetMeasure(NM_Degrees);
00407         outPos[ix] = pos;
00408         if (_armParts[ix]->HasError())
00409         {
00410             _lastErrorPart = ix;
00411             _lastError = _armParts[ix]->GetLastError();
00412             ok = false;
00413         }
00414     }
00415     return ok;
00416 }
00417 
00418 // get all parts measurements
00419 bool RobotArm::GetArmMeasure(int measureId, float outVals[])
00420 {
00421     bool ok = true;
00422     for (int ix = 0; ix < _numParts; ix++)
00423     {
00424         _armParts[ix]->ClearMeasureCache();
00425         float val = _armParts[ix]->GetMeasure(measureId);
00426         outVals[ix] = val;
00427         if (_armParts[ix]->HasError())
00428         {
00429             _lastErrorPart = ix;
00430             _lastError = _armParts[ix]->GetLastError();
00431             ok = false;
00432         }
00433     }
00434     return ok;
00435 }
00436 
00437 // get all parts last measurements
00438 bool RobotArm::GetArmLastMeasure(int measureId, float outVals[])
00439 {
00440     bool ok = true;
00441     for (int ix = 0; ix < _numParts; ix++)
00442     {
00443         float val = _armParts[ix]->GetMeasure(measureId);
00444         outVals[ix] = val;
00445         if (_armParts[ix]->HasError())
00446         {
00447             _lastErrorPart = ix;
00448             _lastError = _armParts[ix]->GetLastError();
00449             ok = false;
00450         }
00451     }
00452     return ok;
00453 }
00454 
00455 int RobotArm::GetNumParts()
00456 {
00457     return _numParts;
00458 }
00459 
00460 void RobotArm::SetStepMs(int stepms)
00461 {
00462     if (stepms > 0 && stepms < 5000)
00463         _stepms = stepms;
00464 }
00465 
00466 void RobotArm::SetThreadId(osThreadId tid)
00467 {
00468     _tid = tid;
00469 }
00470 
00471 // get part by position
00472 RobotNode* RobotArm::GetArmPart(int partIx)
00473 {
00474     return _armParts[partIx];
00475 }
00476 
00477 int RobotArm::GetLastError()
00478 {
00479     return _lastError;
00480 }
00481 
00482 float RobotArm::GetLastPosDiff()
00483 {
00484     return _lastPosDiff;
00485 }
00486 
00487 int RobotArm::GetLastErrorPart()
00488 {
00489     return _lastErrorPart;
00490 }
00491