Diff: RobotArm.cpp

Revision:

18:224289104fc0

Parent:

15:4bd10f531cdc

Child:

19:2f0ec9ac1238

--- a/RobotArm.cpp	Fri Jan 22 01:35:07 2016 +0000
+++ b/RobotArm.cpp	Sat Jan 23 00:08:30 2016 +0000
@@ -1,21 +1,19 @@
 #include "mbed.h"
 #include "rtos.h"
 
-#include <DynamixelBus.h>
-#include <NodeAX12.h>
-//#include <NodeEmul.h>
-#include <Terminal.h>
 #include <vector>
-#include <RobotArm.h>
+
+#include "DynamixelBus.h"
+#include "NodeAX12.h"
+#include "NodeEmul.h"
+#include "RobotArm.h"
 
 using namespace std;
 
+// create the bus interface for this device
 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 );
 
 
-// set the id for each part in the chain, in order
-int PartIds[] = { 2, 3, 4, 6, 1 };
-
 // default to move every 25 ms
 #define StepPeriodMs        25
 
@@ -32,20 +30,38 @@
 #define MaxVoltLimit        13
 #define MinVoltLimit        10
 
+// should arm test for positions ?
+#define TestPositions       false
+
 
 RobotArm::RobotArm()
 {
     // build arm
     for (int ix = 0; ix < NUMJOINTS; ix++)
     {
-        NodeAX12* servo = new NodeAX12(&dynbus, PartIds[ix]);
-        //NodeEmul* servo = new NodeEmul(PartIds[i]);
-        servo->DoAction(NA_Init, 0.0f);
-        _armParts[ix] = dynamic_cast<RobotNode*>(servo);
+        NodePartType nt = ArmJoints[ix].JointType;
+        switch (nt)
+        {
+            case NT_AX12:
+                _armParts[ix] = dynamic_cast<RobotNode*>(new NodeAX12(&dynbus, ArmJoints[ix].JointId));
+                break;
+                
+            case NT_Emul:
+                _armParts[ix] = dynamic_cast<RobotNode*>(new NodeEmul(ArmJoints[ix].JointId));
+                break;
+                
+            default:
+                printf("Error! Unknown node type defined");
+                _armParts[ix] = NULL;
+                break;
+        }
+        
+        _armParts[ix]->DoAction(NA_Init, 0.0f);
     }
+    
     _numParts = NUMJOINTS;
 
-    numsteps = 0;
+    _numsteps = 0;
     _stepms = StepPeriodMs;             
 }
 
@@ -55,7 +71,7 @@
     for (int ix = 0; ix < _numParts; ix++)
     {
         _armParts[ix]->DoAction(NA_ClearError, 0.0f);
-        failms[ix] = 0;
+        _failms[ix] = 0;
     }
 }
 
@@ -66,33 +82,33 @@
 
     MoveArmPositionsEnd();
     
-    GetArmPositions(lastpos);
+    GetArmPositions(_lastpos);
     
-    numsteps = totms / _stepms;
-    if (numsteps == 0) numsteps = 1;
+    _numsteps = totms / _stepms;
+    if (_numsteps == 0) _numsteps = 1;
     
     for (int ix = 0; ix < _numParts; ix++)
     {
         if (positions[ix] > 0.0f)
         {
-            endgoals[ix] = positions[ix];
-            float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
-            differentials[ix] = difference;
+            _endgoals[ix] = positions[ix];
+            float difference = (positions[ix] - _lastpos[ix]) / (float)_numsteps;
+            _differentials[ix] = difference;
         }
         else
         {
             // negative goal. Treat as don't move
-            differentials[ix] = 0.0f;
+            _differentials[ix] = 0.0f;
         }
-        failms[ix] = 0;
+        _failms[ix] = 0;
     }
 
-    curstep = 1;
+    _curstep = 1;
     
-    delayms = _stepms;
+    _delayms = _stepms;
     
-    elapseTimer.start();
-    expDelay = (int)elapseTimer.read_ms() + delayms;
+    _elapseTimer.start();
+    _expDelay = (int)_elapseTimer.read_ms() + _delayms;
     
     return true;
 }
@@ -100,52 +116,52 @@
 // continue interrupted action
 bool RobotArm::MoveArmPositionsResume()
 {
-    if (curstep > numsteps)
+    if (_curstep > _numsteps)
     {
         // no more steps 
         MoveArmPositionsEnd();
         return true;
     }
-    GetArmPositions(lastpos);
+    GetArmPositions(_lastpos);
     
     // reset numsteps to be what was remaining
-    numsteps = numsteps - curstep + 1;
+    _numsteps = _numsteps - _curstep + 1;
     
     for (int ix = 0; ix < _numParts; ix++)
     {
-        if (endgoals[ix] > 0.0f)
+        if (_endgoals[ix] > 0.0f)
         {
-            float difference = (endgoals[ix] - lastpos[ix]) / (float)numsteps;
-            differentials[ix] = difference;
+            float difference = (_endgoals[ix] - _lastpos[ix]) / (float)_numsteps;
+            _differentials[ix] = difference;
         }
         else
         {
             // negative goal. Treat as don't move
-            differentials[ix] = 0.0f;
+            _differentials[ix] = 0.0f;
         }
-        failms[ix] = 0;
+        _failms[ix] = 0;
     }
 
-    curstep = 1;
+    _curstep = 1;
     
-    delayms = _stepms;
+    _delayms = _stepms;
     
-    elapseTimer.start();
-    expDelay = (int)elapseTimer.read_ms() + delayms;
+    _elapseTimer.start();
+    _expDelay = (int)_elapseTimer.read_ms() + _delayms;
     
     return true;
 }
 
 bool RobotArm::MoveArmPositionsHasNext()
 {
-    return (curstep <= numsteps);
+    return (_curstep <= _numsteps);
 }
 
 bool RobotArm::MoveArmPositionsNext()
 {
     _lastErrorPart = 0;
 
-    if (curstep > numsteps)
+    if (_curstep > _numsteps)
     {
         // no more steps 
         MoveArmPositionsEnd();
@@ -158,11 +174,11 @@
     {
         if (_armParts[ix]->HasAction(NA_Rotate))
         {
-            float goal = (curstep == numsteps || differentials[ix] == 0.0f) ?
-                            endgoals[ix] :  // last step - use actual goal
-                            (lastpos[ix] + (differentials[ix] * (float)curstep));
-            lastgoals[ix] = goal;
-            if (differentials[ix] != 0.0f)
+            float goal = (_curstep == _numsteps || _differentials[ix] == 0.0f) ?
+                            _endgoals[ix] :  // last step - use actual goal
+                            (_lastpos[ix] + (_differentials[ix] * (float)_curstep));
+            _lastgoals[ix] = goal;
+            if (_differentials[ix] != 0.0f)
             {
                 bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
                 if (!ok)
@@ -181,8 +197,8 @@
         return false;
     }
     
-    curstep++;
-    if (curstep > numsteps)
+    _curstep++;
+    if (_curstep > _numsteps)
     {
         MoveArmPositionsEnd();
     }
@@ -193,29 +209,29 @@
 // calculate actual delay until expDelay
 bool RobotArm::MoveArmPositionsDelay(int& nextdelay)
 {
-    if (curstep <= numsteps)
+    if (_curstep <= _numsteps)
     {
-        int elapsed = (int)elapseTimer.read_ms();
+        int elapsed = (int)_elapseTimer.read_ms();
     
-        if (elapsed <= expDelay)
+        if (elapsed <= _expDelay)
         {
-            if (expDelay - elapsed < delayms)
-                nextdelay = expDelay - elapsed;
+            if (_expDelay - elapsed < _delayms)
+                nextdelay = _expDelay - elapsed;
             else
-                nextdelay = delayms;
+                nextdelay = _delayms;
             // set next expected time by adding step delay
-            expDelay += delayms;
+            _expDelay += _delayms;
         }
         else
         {
-            nextdelay = delayms;
+            nextdelay = _delayms;
            // set next expected time to now plus step delay
-            expDelay = elapsed + delayms;
+            _expDelay = elapsed + _delayms;
         }
     }
     else
     {
-        nextdelay = delayms;
+        nextdelay = _delayms;
     }
     
     return true;
@@ -236,10 +252,10 @@
 
 bool RobotArm::MoveArmPositionsEnd()
 {
-    if (numsteps > 0)
+    if (_numsteps > 0)
     {
-        elapseTimer.stop();
-        numsteps = 0;
+        _elapseTimer.stop();
+        _numsteps = 0;
     }
     return true;
 }
@@ -281,40 +297,46 @@
             break;
             
         case NM_Degrees:
-            for (int ix = 0; ix < _numParts; ix++)
+            if (TestPositions)
             {
-                float val = curvals[ix];
-                if (val > 0.0f)
+                // positions will frequently be off while arm is moving
+                // logic checks if position is outside expected threshold for a period of time
+                // may get false positives if goal is changed frequently
+                for (int ix = 0; ix < _numParts; ix++)
                 {
-                    float diff = fabs(val  - lastgoals[ix]);
-                    if (diff > (fabs(differentials[ix] * 2.0f) + PositionErrorAllow))
+                    float val = curvals[ix];
+                    if (val > 0.0f)
                     {
-                        int elapsed = (int)elapseTimer.read_ms();
-                        if (failms[ix] > 0)
+                        float diff = fabs(val  - _lastgoals[ix]);
+                        if (diff > (fabs(_differentials[ix] * 2.0f) + PositionErrorAllow))
                         {
-                            if (elapsed - failms[ix] > FailMsLimit)
+                            int elapsed = (int)_elapseTimer.read_ms();
+                            if (_failms[ix] > 0)
                             {
-                                // continuous failure for time period
-                                // report failure
-                                _lastPosDiff = diff;
-                                _lastErrorPart = ix;
-        
-                                failms[ix] = 0;
-                                rc = -1;
+                                if (elapsed - _failms[ix] > FailMsLimit)
+                                {
+                                    // continuous failure for time period
+                                    // report failure
+                                    _lastPosDiff = diff;
+                                    _lastErrorPart = ix;
+            
+                                    _failms[ix] = 0;
+                                    rc = -1;
+                                }
+                            }
+                            else
+                            {
+                                // first failure after success
+                                // remember first fail time.
+                                _failms[ix] = elapsed;
                             }
                         }
                         else
                         {
-                            // first failure after success
-                            // remember first fail time.
-                            failms[ix] = elapsed;
+                            // within allowable range - clear time
+                            _failms[ix] = 0;
                         }
                     }
-                    else
-                    {
-                        // within allowable range - clear time
-                        failms[ix] = 0;
-                    }
                 }
             }
             break;