Diff: RobotArm.cpp

Revision:

5:36916b1c5a06

Parent:

4:36a4eceb1b7f

Child:

7:6723f6887d00

--- a/RobotArm.cpp	Wed Dec 23 18:34:06 2015 +0000
+++ b/RobotArm.cpp	Mon Dec 28 17:19:37 2015 +0000
@@ -12,15 +12,9 @@
 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 );
 Terminal* RobotArmPc = NULL;
 
-// move arm thread timer routine
-void NextMove(void const * tid)
-{
-    osSignalSet((osThreadId)tid, AS_NextStep);
-}
-
 
 // set the id for each part in the chain, in order
-int PartIds[] = { 1, 2, 3, 4, 6 };
+int PartIds[] = { 2, 3, 4, 6, 1 };
 
 RobotArm::RobotArm()
 {
@@ -126,7 +120,7 @@
     _lastErrorPart = 0;
 
     MoveArmPositionsEnd();
-    GetArmLastPositions(lastpos);
+    GetArmPositions(lastpos);
 
     differentials.clear();
     for( int ix = 0; ix < _numParts; ix++)
@@ -142,9 +136,9 @@
     curstep = 1;
     
     delayms = _stepms;
-    expDelay = _stepms;
     
     elapseTimer.start();
+    expDelay = (int)elapseTimer.read_ms() + delayms;
     
     return true;
 }
@@ -167,11 +161,13 @@
     
     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);
+            lastgoals.push_back(goal);
             bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
             if (!ok)
             {
@@ -195,12 +191,11 @@
     {    
         // adjust delay
         int elapsed = (int)elapseTimer.read_ms();
-        expDelay += delayms;
     
-        if (elapsed > expDelay)
+        if (elapsed < expDelay)
         {
-            if (elapsed - expDelay < delayms)
-                nextdelay = elapsed - expDelay;
+            if (expDelay - elapsed < delayms)
+                nextdelay = expDelay - elapsed;
             else
                 nextdelay = delayms;
         }
@@ -209,11 +204,29 @@
             // no delay before next step
             nextdelay = 0;
         }
+        expDelay += delayms;
     }
     
     return true;
 }
 
+bool RobotArm::TestArmPosition(int& partix, float& diffpos)
+{
+    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)
+        {
+            diffpos = diff;
+            partix = ix;
+            return false;
+        }
+    }
+    return true;
+}
+    
 bool RobotArm::MoveArmPositionsEnd()
 {
     if (numsteps > 0)