demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
Diff: RobotArm.cpp
- Revision:
- 10:9b21566a5ddb
- Parent:
- 9:a0fb6c370dbb
- Child:
- 11:3a2e6eb9fbb8
--- 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; }