Step 3: Leader and Follower Example

This tutorial shows you how to read data from one Dynamixel and write the read value to another Dynamixel using the Dynamixel Easy SDK.

NOTE: It assumes that you have already installed and built the SDK.

Make py file

Create a new Python source file Open it with your editor.
```
$ touch tutorial_step3.py
```

Check the Port Names

Before running the code, check the port name of the connected Dynamixel.
For General
- OpenRB-150: ttyACM0: USB ACM device
- U2D2: FTDI USB Serial Device converter now attached to ttyUSB0
```
$ sudo dmesg | grep tty
```

Source Code Description

from dynamixel_easy_sdk import *

def main():
    connector = Connector("/dev/ttyACM0", 57600)
    leader_motor = connector.createMotor(1)
    follower_motor = connector.createMotor(2)
    min_position = leader_motor.getMinPositionLimit() + 100
    max_position = leader_motor.getMaxPositionLimit() - 100

    leader_motor.disableTorque()
    follower_motor.disableTorque()
    follower_motor.setOperatingMode(OperatingMode.POSITION)
    follower_motor.enableTorque()

    while (True):
        present_position = leader_motor.getPresentPosition()
        print(f"Leader Position: {present_position}     ", end='\r')
        if (present_position < min_position):
            leader_motor.enableTorque()
            leader_motor.setGoalPosition(min_position)
            while (True):
                present_position = leader_motor.getPresentPosition()
                if (present_position >= min_position):
                    break
            leader_motor.disableTorque()
        elif (present_position > max_position):
            leader_motor.enableTorque()
            leader_motor.setGoalPosition(max_position)
            while (True):
                present_position = leader_motor.getPresentPosition()
                if (present_position <= max_position):
                    break
            leader_motor.disableTorque()
        follower_motor.setGoalPosition(present_position)

if __name__ == "__main__":
    main()

Import Library

Import dynamixel_easy_sdk.
```
from dynamixel_easy_sdk import *
```

Create Connector and Motor Object

Create a Connector object with port name and baud rate to manage the communication.
```
def main():
    connector = Connector("/dev/ttyACM0", 57600)
```
Create a Motor object for each Dynamixel servo you want to control with its ID, using the createMotor method of the Connector class.

Create motor objects while carefully distinguishing between the leader and follower IDs.

    leader_motor = connector.createMotor(1)
    follower_motor = connector.createMotor(2)

Set Position Limits

Get the position limits of the leader motor using the getMinPositionLimit and getMaxPositionLimit methods.

    min_position = leader_motor.getMinPositionLimit() + 100
    max_position = leader_motor.getMaxPositionLimit() - 100

Set Operating Mode to Position Control Mode

Use the methods provided by the Motor class to control the Dynamixel servo.

Set the operating mode of follower motor to position control mode.

    leader_motor.disableTorque()
    follower_motor.disableTorque()
    follower_motor.setOperatingMode(OperatingMode.POSITION)
    follower_motor.enableTorque()

Leader and Follower Control Loop

In a loop, read the present position of the leader motor using the getPresentPosition method.

    while (True):
        present_position = leader_motor.getPresentPosition()
        print(f"Leader Position: {present_position}     ", end='\r')

If the leader motor’s position exceeds the defined range, move it back within the range.

        if (present_position < min_position):
            leader_motor.enableTorque()
            leader_motor.setGoalPosition(min_position)
            while (True):
                present_position = leader_motor.getPresentPosition()
                if (present_position >= min_position):
                    break
            leader_motor.disableTorque()
        elif (present_position > max_position):
            leader_motor.enableTorque()
            leader_motor.setGoalPosition(max_position)
            while (True):
                present_position = leader_motor.getPresentPosition()
                if (present_position <= max_position):
                    break
            leader_motor.disableTorque()

Set the goal position of the follower motor to the present position of the leader motor.
```
    follower_motor.setGoalPosition(present_position)
```

Error Handling

When an error occurs, DxlRuntimeError is raised.

You can catch this error using a try-except block.

try:
    motor1.getPresentPosition()
except DxlRuntimeError as e:
    print(e)

DxlRuntimeError contains DxlError Enum that provides detailed information about the error.

try:
    motor1.getPresentPosition()
except DxlRuntimeError as e:
    if e.dxl_error == DxlError.SDK_COMM_RX_FAIL:
        print("Transmission failed.")
    elif e.dxl_error == DxlError.SDK_COMM_RX_FAIL:
        print("Receive failed.")

NOTE: It assumes that you have already installed and built the SDK.

Make cpp file

Create a new C++ source file Open it with your editor.
```
$ touch tutorial_step3.cpp
```