Diff: RobotArm.cpp

Revision:

7:6723f6887d00

Parent:

5:36916b1c5a06

Child:

8:d98e2dec0f40

--- a/RobotArm.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/RobotArm.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -10,41 +10,42 @@
 using namespace std;
 
 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 );
-Terminal* RobotArmPc = NULL;
 
 
 // set the id for each part in the chain, in order
 int PartIds[] = { 2, 3, 4, 6, 1 };
 
+#define FailMsLimit  200
+
 RobotArm::RobotArm()
 {
     // build arm
     int i = 0;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); 
+    printf( "id[%d] = %d\r\n", i, PartIds[i]); 
     AX12* servo1 = new AX12( &dynbus, PartIds[i] );
     servo1->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo1);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo2 = new AX12( &dynbus, PartIds[i] );
     servo2->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo2);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo3 = new AX12( &dynbus, PartIds[i] );
     servo3->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo3);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo4 = new AX12( &dynbus, PartIds[i] );
     servo4->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo4);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo6 = new AX12( &dynbus, PartIds[i] );
     servo6->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo6);
@@ -54,65 +55,10 @@
 
     numsteps = 0;
     _stepms = 20; // move every 20 ms
+    allowance = 2.0f;       // allow 2 degree fudge factor
+    failms = 0;
 }
 
-// move all parts to specified postions in ms time
-bool RobotArm::MoveArmPositions(vector<float> positions, int ms, int steps)
-{
-    _lastErrorPart = 0;
-    
-    GetArmLastPositions(lastpos);
-
-    for( int ix = 0; ix < _numParts; ix++)
-    {
-        float difference = positions[ix]- lastpos[ix];
-        differentials.push_back( difference );
-    }
-
-    if (steps > 1000) steps = 1000;
-    if (ms <= 0) ms = 1;
-    
-    delayms = ms / steps;
-    expDelay = delayms;
-    
-    elapseTimer.start();
-    bool ok = true;
-    
-    // move arm to new position in steps
-    for( int step = 1; step <= steps; step++)
-    {
-        //pc.foreground(Red);
-        //pc.background(Black);
-        //pc.locate( 0 , 24 );
-        float incr = (float)step / (float)steps;
-        for( int ix = 0; ix < _numParts; ix++ )
-        {
-            //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
-            if (_armParts[ix]->HasAction(NA_Rotate))
-            {
-                float goal = lastpos[ix] + (differentials[ix] * incr);
-                bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
-                if (!ok)
-                {
-                    _lastErrorPart = ix;
-                    break;
-                }
-            }
-        }
-        if (!ok)
-            break;
-        // adjust delay
-        int elapsed = (int)elapseTimer.read_ms();
-        expDelay += delayms;
-        if (elapsed > expDelay && elapsed - expDelay < delayms)
-            wait_ms(elapsed - expDelay);
-        else
-            wait_ms(delayms);
-    }
-    
-    elapseTimer.stop();
-    return ok;
-}
 
 // move all parts to specified postions in ms time
 bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
@@ -121,24 +67,33 @@
 
     MoveArmPositionsEnd();
     GetArmPositions(lastpos);
+    lastgoals.clear();
 
+    numsteps = totms / _stepms;
+    if (numsteps == 0) numsteps = 1;
+    
     differentials.clear();
     for( int ix = 0; ix < _numParts; ix++)
     {
-        float difference = positions[ix] - lastpos[ix];
-        differentials.push_back( difference );
-        RobotArmPc->printf( "diff %d = %f\r\n", ix, difference);
+        if (positions[ix] > 0.0f)
+        {
+            float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
+            differentials.push_back( difference );
+        }
+        else
+        {
+            // negative goal. Treat as don't move
+            differentials.push_back(0.0f);
+        }
     }
 
-    numsteps = totms / _stepms;
-    if (numsteps == 0) numsteps = 1;
-
     curstep = 1;
     
     delayms = _stepms;
     
     elapseTimer.start();
     expDelay = (int)elapseTimer.read_ms() + delayms;
+    failms = 0;
     
     return true;
 }
@@ -148,7 +103,7 @@
     return (curstep <= numsteps);
 }
 
-bool RobotArm::MoveArmPositionsNext(int& nextdelay)
+bool RobotArm::MoveArmPositionsNext()
 {
     _lastErrorPart = 0;
 
@@ -160,19 +115,24 @@
     }
     
     bool ok = true;
-    float incr = (float)curstep / (float)numsteps;
+
     lastgoals.clear();
     for( int ix = 0; ix < _numParts; ix++ )
     {
         if (_armParts[ix]->HasAction(NA_Rotate))
         {
-            float goal = lastpos[ix] + (differentials[ix] * incr);
+            float goal = lastpos[ix] + (differentials[ix] * (float)curstep);
             lastgoals.push_back(goal);
-            bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
-            if (!ok)
+            if (differentials[ix] != 0.0f)
             {
-                _lastErrorPart = ix;
-                break;
+                bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
+                if (!ok)
+                {
+                    _lastErrorPart = ix;
+                    _lastError = _armParts[_lastErrorPart]->GetLastError();
+                    _lastPosDiff = 0;
+                    break;
+                }
             }
         }
     }
@@ -187,43 +147,83 @@
     {
         MoveArmPositionsEnd();
     }
-    else
-    {    
-        // adjust delay
+    
+    return true;
+}
+
+// calculate actual delay until expDelay
+bool RobotArm::MoveArmPositionsDelay(int& nextdelay)
+{
+    if (curstep <= numsteps)
+    {
         int elapsed = (int)elapseTimer.read_ms();
     
-        if (elapsed < expDelay)
+        if (elapsed <= expDelay)
         {
             if (expDelay - elapsed < delayms)
                 nextdelay = expDelay - elapsed;
             else
                 nextdelay = delayms;
+            // set next expected time by adding step delay
+            expDelay += delayms;
         }
         else
         {
-            // no delay before next step
-            nextdelay = 0;
+            nextdelay = delayms;
+           // set next expected time to now plus step delay
+            expDelay = elapsed + delayms;
         }
-        expDelay += delayms;
+    }
+    else
+    {
+        nextdelay = 0;
     }
     
     return true;
 }
 
-bool RobotArm::TestArmPosition(int& partix, float& diffpos)
+bool RobotArm::MoveArmPositionTest()
 {
     vector<float> curpos;
     GetArmPositions(curpos);
     for( int ix = 0; ix < _numParts; ix++ )
     {
-        float diff = abs(curpos[ix]  - lastgoals[ix]);
-        if (diff > abs(differentials[ix]) + allowance)
+        if (curpos[ix] > 0 && lastgoals.size() > ix)
         {
-            diffpos = diff;
-            partix = ix;
-            return false;
+            float diff = fabs(curpos[ix]  - lastgoals[ix]);
+            if (diff > (fabs(differentials[ix] * 2.0f) + allowance))
+            {
+                printf("Bad position for %d. Expect %f - got %f\r\n", ix, lastgoals[ix], curpos[ix]);
+                _lastPosDiff = diff;
+                _lastErrorPart = ix;
+                _lastError = 0;
+
+                int elapsed = (int)elapseTimer.read_ms();
+                if (failms > 0)
+                {
+                    if (elapsed - failms > FailMsLimit)
+                    {
+                        // continuous failure for time period
+                        // return false
+                        lastgoals.clear();
+                        failms = 0;
+                        return false;
+                    }
+                    // within time period. Do not report failure
+                    return true;
+                }
+                else
+                {
+                    // first failure after success
+                    // remember time. Do not report failure
+                    failms = elapsed;
+                    return true;
+                }
+            }
         }
     }
+    // success
+    failms = 0;
     return true;
 }
     
@@ -237,45 +237,6 @@
     return true;
 }
 
-// move one part to specified postion in ms time
-bool RobotArm::MovePartPosition(int partIx, float position, int ms, int steps)
-{
-    _lastErrorPart = 0;
-    if (!_armParts[partIx]->HasAction(NA_Rotate))
-        return false;
-        
-    float lastpos = _armParts[partIx]->GetLastMeasure(NM_Degrees);
-
-    float difference = position- lastpos;
-
-    if (steps > 1000) steps = 1000;
-    if (ms <= 0) ms = 1;
-    
-    int delayms = ms / steps;
-    
-    bool ok = true;
-    
-    // move arm to new position in steps
-    for( int step = 1; step <= steps; step++)
-    {
-        //pc.foreground(Red);
-        //pc.background(Black);
-        //pc.locate( 0 , 24 );
-        float incr = (float)step / (float)steps;
-
-        //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
-        float goal = lastpos + (difference * incr);
-        bool ok = _armParts[partIx]->DoAction(NA_Rotate, goal);
-        if (!ok)
-        {
-            _lastErrorPart = partIx;
-            break;
-        }
-            
-        wait_ms(delayms);
-    }
-    return ok;
-}
 
 // get all parts positions
 bool RobotArm::GetArmPositions(vector<float>& outPos)
@@ -289,7 +250,7 @@
     return true;
 }
 
-// get all parts positions
+// get all parts last measured positions
 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
 {
     outPos.clear();
@@ -301,14 +262,7 @@
     return true;
 }
 
-// get one part position
-float RobotArm::GetPartPosition(int partIx)
-{
-    return _armParts[partIx]->GetMeasure(NM_Degrees);
-}
-
-
-// get all parts positions
+// get all parts measurements
 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
 {
     outPos.clear();
@@ -320,7 +274,7 @@
     return true;
 }
 
-// get all parts positions
+// get all parts last measurements
 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
 {
     outPos.clear();
@@ -332,12 +286,6 @@
     return true;
 }
 
-// get one part position
-float RobotArm::GetPartMeasure(int measureId, int partIx)
-{
-    return _armParts[partIx]->GetMeasure(measureId);
-}
-
 int RobotArm::GetNumParts()
 {
     return _numParts;
@@ -360,12 +308,22 @@
     return _armParts[partIx];
 }
 
-unsigned char RobotArm::GetLastError()
+int RobotArm::GetLastError()
 {
-    return _armParts[_lastErrorPart]->GetLastError();
+    return _lastError;
+}
+
+float RobotArm::GetLastPosDiff()
+{
+    return _lastPosDiff;
 }
 
 int RobotArm::GetLastErrorPart()
 {
     return _lastErrorPart;
 }
+
+void RobotArm::SetAllowance(float allow)
+{
+    allowance = allow;
+}