Diff: RobotArm.cpp

Revision:

8:d98e2dec0f40

Parent:

7:6723f6887d00

Child:

9:a0fb6c370dbb

--- a/RobotArm.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/RobotArm.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -21,32 +21,27 @@
 {
     // build arm
     int i = 0;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]); 
-    AX12* servo1 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo1 = new AX12(&dynbus, PartIds[i]);
     servo1->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo1);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo2 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo2 = new AX12(&dynbus, PartIds[i]);
     servo2->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo2);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo3 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo3 = new AX12(&dynbus, PartIds[i]);
     servo3->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo3);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo4 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo4 = new AX12(&dynbus, PartIds[i]);
     servo4->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo4);
     
     i++;
-    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
-    AX12* servo6 = new AX12( &dynbus, PartIds[i] );
+    AX12* servo6 = new AX12(&dynbus, PartIds[i]);
     servo6->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo6);
     
@@ -68,21 +63,24 @@
     MoveArmPositionsEnd();
     GetArmPositions(lastpos);
     lastgoals.clear();
-
+    endgoals.clear();
+    
     numsteps = totms / _stepms;
     if (numsteps == 0) numsteps = 1;
     
     differentials.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    for (int ix = 0; ix < _numParts; ix++)
     {
         if (positions[ix] > 0.0f)
         {
+            endgoals.push_back(positions[ix]);
             float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
-            differentials.push_back( difference );
+            differentials.push_back(difference);
         }
         else
         {
             // negative goal. Treat as don't move
+            endgoals.push_back(lastpos[ix]);
             differentials.push_back(0.0f);
         }
     }
@@ -117,11 +115,13 @@
     bool ok = true;
 
     lastgoals.clear();
-    for( int ix = 0; ix < _numParts; ix++ )
+    for (int ix = 0; ix < _numParts; ix++)
     {
         if (_armParts[ix]->HasAction(NA_Rotate))
         {
-            float goal = lastpos[ix] + (differentials[ix] * (float)curstep);
+            float goal = (curstep == numsteps) ?
+                            endgoals[ix] :  // last step - use actual goal
+                            (lastpos[ix] + (differentials[ix] * (float)curstep));
             lastgoals.push_back(goal);
             if (differentials[ix] != 0.0f)
             {
@@ -176,7 +176,7 @@
     }
     else
     {
-        nextdelay = 0;
+        nextdelay = delayms;
     }
     
     return true;
@@ -186,7 +186,7 @@
 {
     vector<float> curpos;
     GetArmPositions(curpos);
-    for( int ix = 0; ix < _numParts; ix++ )
+    for (int ix = 0; ix < _numParts; ix++)
     {
         if (curpos[ix] > 0 && lastgoals.size() > ix)
         {
@@ -242,10 +242,10 @@
 bool RobotArm::GetArmPositions(vector<float>& outPos)
 {
     outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    for (int ix = 0; ix < _numParts; ix++)
     {
         float pos = _armParts[ix]->GetMeasure(NM_Degrees);
-        outPos.push_back( pos );
+        outPos.push_back(pos);
     }
     return true;
 }
@@ -254,34 +254,34 @@
 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
 {
     outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    for (int ix = 0; ix < _numParts; ix++)
     {
         float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
-        outPos.push_back( pos );
+        outPos.push_back(pos);
     }
     return true;
 }
 
 // get all parts measurements
-bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
+bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals)
 {
-    outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    outVals.clear();
+    for (int ix = 0; ix < _numParts; ix++)
     {
-        float pos = _armParts[ix]->GetMeasure(measureId);
-        outPos.push_back( pos );
+        float val = _armParts[ix]->GetMeasure(measureId);
+        outVals.push_back(val);
     }
     return true;
 }
 
 // get all parts last measurements
-bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
+bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals)
 {
-    outPos.clear();
-    for( int ix = 0; ix < _numParts; ix++)
+    outVals.clear();
+    for (int ix = 0; ix < _numParts; ix++)
     {
-        float pos = _armParts[ix]->GetLastMeasure(measureId);
-        outPos.push_back( pos );
+        float val = _armParts[ix]->GetLastMeasure(measureId);
+        outVals.push_back(val);
     }
     return true;
 }