Diff: RobotArm.cpp

Revision:

10:9b21566a5ddb

Parent:

9:a0fb6c370dbb

Child:

11:3a2e6eb9fbb8

diff -r a0fb6c370dbb -r 9b21566a5ddb RobotArm.cpp
--- a/RobotArm.cpp	Thu Dec 31 20:02:58 2015 +0000
+++ b/RobotArm.cpp	Wed Jan 06 00:58:41 2016 +0000
@@ -13,6 +13,7 @@
 
 
 // set the id for each part in the chain, in order
+#define NUMPARTS    5
 int PartIds[] = { 2, 3, 4, 6, 1 };
 
 #define FailMsLimit  200
@@ -20,37 +21,18 @@
 RobotArm::RobotArm()
 {
     // build arm
-    int i = 0;
-    AX12* servo1 = new AX12(&dynbus, PartIds[i]);
-    servo1->TorqueEnable(true);
-    _armParts[i] = dynamic_cast<RobotNode*>(servo1);
-    
-    i++;
-    AX12* servo2 = new AX12(&dynbus, PartIds[i]);
-    servo2->TorqueEnable(true);
-    _armParts[i] = dynamic_cast<RobotNode*>(servo2);
-    
-    i++;
-    AX12* servo3 = new AX12(&dynbus, PartIds[i]);
-    servo3->TorqueEnable(true);
-    _armParts[i] = dynamic_cast<RobotNode*>(servo3);
-    
-    i++;
-    AX12* servo4 = new AX12(&dynbus, PartIds[i]);
-    servo4->TorqueEnable(true);
-    _armParts[i] = dynamic_cast<RobotNode*>(servo4);
-    
-    i++;
-    AX12* servo6 = new AX12(&dynbus, PartIds[i]);
-    servo6->TorqueEnable(true);
-    _armParts[i] = dynamic_cast<RobotNode*>(servo6);
-    
-    i++;
-    _numParts = i;
+    for (int i = 0; i < NUMPARTS; i++)
+    {
+        AX12* servo = new AX12(&dynbus, PartIds[i]);
+        servo->TorqueEnable(false);
+        servo->TorqueEnable(true);
+        _armParts[i] = dynamic_cast<RobotNode*>(servo);
+    }
+    _numParts = NUMPARTS;
 
     numsteps = 0;
     _stepms = 20; // move every 20 ms
-    allowance = 2.0f;       // allow 2 degree fudge factor
+    allowance = 2.0f;       // allow 2 degree diff for position error
     failms = 0;
 }