View source for DYNAMIXEL servos

= Dynamixel motor control (MKR Zero) =

* Product: https://emanual.robotis.com/docs/en/parts/interface/mkr_shield/

== Dependencies ==
* Install the library DynamixelShield in the Arduino IDE
** GitHub: https://github.com/ROBOTIS-GIT/DynamixelShield
** Documentation: https://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/

== Description ==

This script is based on the example code provided with the DynamixelShield Arduino library.  
Some small modifications were required to make it work with the MKR Zero board and the Dynamixel motor used in this setup.

The main changes are:

* The shield communicates through '''Serial1''' on the MKR Zero.
* The port baudrate had to be set to '''1,000,000''' for the motor to respond correctly.
* Debug output is sent to the Arduino Serial Monitor.

The script first checks if the motor is connected using '''ping()'''.  
If the motor responds, torque is enabled and the operating mode is set to '''position control'''.

In the loop the motor is commanded to move to different positions.  
The position is set once using a raw value and once using degrees, and the current motor position is printed to the Serial Monitor.

== Source code ==

<pre>
/*******************************************************************************
* ROBOTIS Example - Fixed for MKR Zero
*******************************************************************************/

#include <DynamixelShield.h>

#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;

DynamixelShield dxl(DXL_SERIAL);

//This namespace is required to use Control table item names
using namespace ControlTableItem;

void setup() {

  DEBUG_SERIAL.begin(115200);
  while(!DEBUG_SERIAL);

  // Set Port baudrate (motor responded at 1,000,000)
  dxl.begin(1000000);

  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  DEBUG_SERIAL.println("Pinging motor...");

  if(dxl.ping(DXL_ID))
    DEBUG_SERIAL.println("Motor connected!");
  else
    DEBUG_SERIAL.println("Motor NOT detected!");

  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
  dxl.torqueOn(DXL_ID);
}

void loop() {

  dxl.setGoalPosition(DXL_ID, 512);
  delay(1000);

  DEBUG_SERIAL.print("Present Position(raw) : ");
  DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID));

  delay(1000);

  dxl.setGoalPosition(DXL_ID, 90, UNIT_DEGREE);
  delay(1000);

  DEBUG_SERIAL.print("Present Position(degree) : ");
  DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID, UNIT_DEGREE));

  delay(2000);
}
</pre>