Difference between revisions of "ROBOTIS DYNAMIXEL Shield for Arduino MKR Series Examples"

From Lofaro Lab Wiki
Jump to: navigation, search
(Include the DYNAMIXEL Protocol Headers)
(Full Code)
 
(22 intermediate revisions by the same user not shown)
Line 27: Line 27:
 
=== Code ===
 
=== Code ===
 
==== Include the DYNAMIXEL Protocol Headers ====
 
==== Include the DYNAMIXEL Protocol Headers ====
If you have not installed the DYNAMIXEL Protocol Headers for Arduino please install them now.  This example used the library found here: [ http://files.lofarolabs.com/software/Dynamixel2Arduino-master.zip Dynamixel2Arduino-master.zip ]
+
If you have not installed the DYNAMIXEL Protocol Headers for Arduino please install them now.  This example used the library found here: [http://files.lofarolabs.com/software/Dynamixel2Arduino-master.zip Dynamixel2Arduino-master.zip]
 
   #include <Dynamixel2Arduino.h>
 
   #include <Dynamixel2Arduino.h>
  
Line 66: Line 66:
  
  
===== Inside your loop() function ====
+
==== Inside your loop() function ====
 
The next part is inside your loop function, here you will ping the ID specified above and print out if you get a response or not.  The system will delay 500ms (0.5sec) between pings.
 
The next part is inside your loop function, here you will ping the ID specified above and print out if you get a response or not.  The system will delay 500ms (0.5sec) between pings.
  
   if(dxl.ping(DXL_ID) == true) DEBUG_SERIAL.print("success");
+
   if(dxl.ping(DXL_ID) == true) DEBUG_SERIAL.println("success");
   else DEBUG_SERIAL("error");
+
   else DEBUG_SERIAL.println("error");
 
   delay(500);
 
   delay(500);
 +
 +
==== Full Code ====
 +
This will ping the actuator with ID=1 and a baud of 57600.  It will return "success" if it see the actuator and "error" if it does not.
 +
 +
<syntaxhighlight lang="cpp" line='line'>
 +
 +
#include <Dynamixel2Arduino.h>
 +
 +
#define DXL_SERIAL  Serial1
 +
#define DEBUG_SERIAL Serial
 +
const uint8_t DXL_DIR_PIN = A6;
 +
 +
const uint8_t DXL_ID = 1;
 +
const float DXL_PROTOCOL_VERSION = 2.0;
 +
const int DXL_BAUD = 57600;
 +
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 +
using namespace ControlTableItem;
 +
 +
void setup() {
 +
  DEBUG_SERIAL.begin(115200);
 +
  dxl.begin(DXL_BAUD);
 +
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 +
}
 +
 +
void loop() {
 +
  if(dxl.ping(DXL_ID) == true) DEBUG_SERIAL.println("success");
 +
  else DEBUG_SERIAL.println("error");
 +
  delay(500);
 +
}
 +
 +
</syntaxhighlight>
 +
 +
== Set Position ==
 +
This example shows you how to set the position of the actuator.  This example assumes you have the base setup from the "ping" example.  Everything from "ping" is needed except for what is in the loop() function.
 +
 +
=== Code ===
 +
==== Enable the actuator in the setup ====
 +
before we can move the actuator we need to enable the actuator, set the control mode to position, turn on the torque, and limit the maximum velocity.  Put the following at the END of the setup() function.  This means any place in the setup function after you have started the actuator object and set the protocol version.
 +
 +
Turn off torque before modifying the paramaters:
 +
  dxl.torqueOff(DXL_ID);
 +
 +
Put into position control mode:
 +
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
 +
 +
Enable Torque
 +
  dxl.torqueOn(DXL_ID);
 +
 +
Limit maximum speed (note set 0 for max speed - not recommended):
 +
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
 +
 +
 +
==== Set the motors position using "ticks" ====
 +
Within the loop() function set the position to 512 "ticks" then delay for 1500 ms (1.5 sec).  Note that the XL330 actuator has 4096 "ticks" per revolution and is continuous rotation.  This means that 2048 is the center position:
 +
  dxl.setGoalPosition(DXL_ID, 512);
 +
  delay(1500);
 +
 +
Then set the position to 1023 "ticks" then delay for 1500 ms (1.5 sec):
 +
  dxl.setGoalPosition(DXL_ID, 1023);
 +
  delay(1500);
 +
 +
Then set the position to 0 "ticks" then delay for 1500 ms (1.5 sec):
 +
  dxl.setGoalPosition(DXL_ID, 0);
 +
  delay(1500);
 +
 +
 +
==== Set the motors position using "degrees" ====
 +
Within the loop() function set the position to 0 degrees then delay for 3000 ms (3 sec).  Please note that 180 degrees is center position. 
 +
  dxl.setGoalPosition(DXL_ID, 0, UNIT_DEGREE);
 +
  delay(3000);
 +
 +
Then set the position to 1023 degrees then delay for 3000 ms (3 sec):
 +
  dxl.setGoalPosition(DXL_ID, 180, UNIT_DEGREE);
 +
  delay(3000);
 +
 +
Then set the position to 0 degrees then delay for 3000 ms (3 sec):
 +
  dxl.setGoalPosition(DXL_ID, 270, UNIT_DEGREE);
 +
  delay(3000);
 +
 +
==== Full Code (using "ticks") ====
 +
<syntaxhighlight lang="cpp" line='line'>
 +
#include <Dynamixel2Arduino.h>
 +
 +
#define DXL_SERIAL  Serial1
 +
#define DEBUG_SERIAL Serial
 +
const uint8_t DXL_DIR_PIN = A6;
 +
 +
const uint8_t DXL_ID = 1;
 +
const float DXL_PROTOCOL_VERSION = 2.0;
 +
const int DXL_BAUD = 57600;
 +
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 +
using namespace ControlTableItem;
 +
 +
void setup() {
 +
  DEBUG_SERIAL.begin(115200);
 +
  dxl.begin(DXL_BAUD);
 +
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 +
 +
  dxl.torqueOff(DXL_ID);
 +
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
 +
  dxl.torqueOn(DXL_ID);
 +
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
 +
}
 +
 +
void loop() {
 +
  dxl.setGoalPosition(DXL_ID, 512);
 +
  delay(1500);
 +
  dxl.setGoalPosition(DXL_ID, 1023);
 +
  delay(1500);
 +
  dxl.setGoalPosition(DXL_ID, 0);
 +
  delay(1500);
 +
}
 +
</syntaxhighlight>
 +
 +
==== Full Code (using "degrees") ====
 +
<syntaxhighlight lang="cpp" line='line'>
 +
#include <Dynamixel2Arduino.h>
 +
 +
#define DXL_SERIAL  Serial1
 +
#define DEBUG_SERIAL Serial
 +
const uint8_t DXL_DIR_PIN = A6;
 +
 +
const uint8_t DXL_ID = 1;
 +
const float DXL_PROTOCOL_VERSION = 2.0;
 +
const int DXL_BAUD = 57600;
 +
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 +
using namespace ControlTableItem;
 +
 +
void setup() {
 +
  DEBUG_SERIAL.begin(115200);
 +
  dxl.begin(DXL_BAUD);
 +
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 +
 +
  dxl.torqueOff(DXL_ID);
 +
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
 +
  dxl.torqueOn(DXL_ID);
 +
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
 +
}
 +
 +
void loop() {
 +
  dxl.setGoalPosition(DXL_ID, 0.0, UNIT_DEGREE);
 +
  delay(3000);
 +
  dxl.setGoalPosition(DXL_ID, 180.0, UNIT_DEGREE);
 +
  delay(3000);
 +
  dxl.setGoalPosition(DXL_ID, 270.0, UNIT_DEGREE);
 +
  delay(3000);
 +
}
 +
</syntaxhighlight>
 +
 +
== Get Position ==
 +
This example shows you how to get the position of the actuator.  This example assumes you have the base setup from the "set position in degrees" example.  Everything from the latter example is needed including what is in the loop() function.
 +
 +
=== Code ===
 +
 +
==== Get the motors position using "ticks" ====
 +
Within the loop() function, after you have set the position but before the delay add the following:
 +
  int pos_t = dxl.getPresentPosition(DXL_ID);
 +
  DEBUG_SERIAL.println(pos_t);
 +
 +
==== Get the motors position using "degrees" ====
 +
Within the loop() function, after you have set the position but before the delay add the following:
 +
  float pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
 +
  DEBUG_SERIAL.println(pos_d);
 +
 +
==== Full Code (using "ticks" and "degrees") ====
 +
<syntaxhighlight lang="cpp" line='line'>
 +
#include <Dynamixel2Arduino.h>
 +
 +
#define DXL_SERIAL  Serial1
 +
#define DEBUG_SERIAL Serial
 +
const uint8_t DXL_DIR_PIN = A6;
 +
 +
const uint8_t DXL_ID = 1;
 +
const float DXL_PROTOCOL_VERSION = 2.0;
 +
const int DXL_BAUD = 57600;
 +
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 +
using namespace ControlTableItem;
 +
 +
void setup() {
 +
  DEBUG_SERIAL.begin(115200);
 +
  dxl.begin(DXL_BAUD);
 +
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 +
 +
  dxl.torqueOff(DXL_ID);
 +
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
 +
  dxl.torqueOn(DXL_ID);
 +
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
 +
}
 +
 +
void loop() {
 +
  dxl.setGoalPosition(DXL_ID, 512);
 +
  delay(1500);
 +
  dxl.setGoalPosition(DXL_ID, 1023);
 +
  delay(1500);
 +
  dxl.setGoalPosition(DXL_ID, 0);
 +
  delay(1500);
 +
}
 +
</syntaxhighlight>
 +
 +
==== Full Code (using "ticks" and "degrees") ====
 +
Note that slight changes were made below to make sure it can compile and print properly.  The resulting code will print the current actuator position in ticks and degrees.
 +
<syntaxhighlight lang="cpp" line='line'>
 +
#include <Dynamixel2Arduino.h>
 +
 +
#define DXL_SERIAL  Serial1
 +
#define DEBUG_SERIAL Serial
 +
const uint8_t DXL_DIR_PIN = A6;
 +
 +
const uint8_t DXL_ID = 1;
 +
const float DXL_PROTOCOL_VERSION = 2.0;
 +
const int DXL_BAUD = 57600;
 +
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 +
using namespace ControlTableItem;
 +
 +
void setup() {
 +
  DEBUG_SERIAL.begin(115200);
 +
  dxl.begin(DXL_BAUD);
 +
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 +
 +
  dxl.torqueOff(DXL_ID);
 +
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
 +
  dxl.torqueOn(DXL_ID);
 +
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
 +
}
 +
 +
void loop() {
 +
  /*
 +
  dxl.setGoalPosition(DXL_ID, 512);
 +
  delay(1500);
 +
  dxl.setGoalPosition(DXL_ID, 1023);
 +
  delay(1500);
 +
  dxl.setGoalPosition(DXL_ID, 0);
 +
  delay(1500);
 +
  */
 +
  dxl.setGoalPosition(DXL_ID, 0.0, UNIT_DEGREE);
 +
  int pos_t = dxl.getPresentPosition(DXL_ID);
 +
  DEBUG_SERIAL.print(pos_t);
 +
  DEBUG_SERIAL.print(" ");
 +
  float pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
 +
  DEBUG_SERIAL.println(pos_d);
 +
  delay(3000);
 +
 
 +
  dxl.setGoalPosition(DXL_ID, 180.0, UNIT_DEGREE);
 +
  pos_t = dxl.getPresentPosition(DXL_ID);
 +
  DEBUG_SERIAL.print(pos_t);
 +
  DEBUG_SERIAL.print(" ");
 +
  pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
 +
  DEBUG_SERIAL.println(pos_d);
 +
  delay(3000);
 +
 
 +
  dxl.setGoalPosition(DXL_ID, 270.0, UNIT_DEGREE);
 +
  pos_t = dxl.getPresentPosition(DXL_ID);
 +
  DEBUG_SERIAL.print(pos_t);
 +
  DEBUG_SERIAL.print(" ");
 +
  pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
 +
  DEBUG_SERIAL.println(pos_d);
 +
  delay(3000);
 +
}
 +
</syntaxhighlight>
 +
 +
== Read position fast example ==
 +
In this example we are reading the position quickly and plotting out set point for our actuator and our actual position in degrees.  This is a slight modification of the "Get Position" example above.  The full code is listed below for this example.  It is important to note that we are reading the position at approximately 50hz (note the delay 20 ms at the bottom of the loop).  We are also alternating the set point of the actuator between 90 degrees and 270 degrees.  The animated plots below shows what happens when we change between a 90 degrees set point and a 270 degree set point with one (1) second between set points and with three (3) seconds between set points.
 +
 +
 +
=== Change between a 90 degrees set point and a 270 degree set point with three (3) second between set points ===
 +
[[File:mkr-robotis-step-slow.gif]]
 +
 +
 +
=== Change between a 90 degrees set point and a 270 degree set point with one (1) second between set points ===
 +
[[File:mkr-robotis-step-fast.gif]]
 +
 +
Note how if we change the set point too fast the actuator does not reach it's goal position.
 +
 +
=== Full Code ===
 +
 +
==== Change Set Point Timing ====
 +
For an approximate one (1) second delay between change in set points:
 +
  GOAL_POS_I_MAX 50
 +
 +
For an approximate three (3) second delay between change in set points:
 +
  GOAL_POS_I_MAX 150
 +
 +
 +
==== Code ====
 +
<syntaxhighlight lang="cpp" line='line'>
 +
#include <Dynamixel2Arduino.h>
 +
 +
#define DXL_SERIAL  Serial1
 +
#define DEBUG_SERIAL Serial
 +
const uint8_t DXL_DIR_PIN = A6;
 +
 +
const uint8_t DXL_ID = 1;
 +
const float DXL_PROTOCOL_VERSION = 2.0;
 +
const int DXL_BAUD = 57600;
 +
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
 +
using namespace ControlTableItem;
 +
 +
void setup() {
 +
  DEBUG_SERIAL.begin(115200);
 +
  dxl.begin(DXL_BAUD);
 +
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 +
 +
  dxl.torqueOff(DXL_ID);
 +
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
 +
  dxl.torqueOn(DXL_ID);
 +
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
 +
}
 +
 +
float goal_pos_0 = 90.0;
 +
float goal_pos_1 = 270.0;
 +
float goal_pos = goal_pos_0;
 +
int goal_pos_i = 0;
 +
#define GOAL_POS_I_MAX 50
 +
void loop() {
 +
  if(goal_pos_i > GOAL_POS_I_MAX)
 +
  {
 +
    if(goal_pos == goal_pos_0) goal_pos = goal_pos_1;
 +
    else goal_pos = goal_pos_0;
 +
    dxl.setGoalPosition(DXL_ID, goal_pos, UNIT_DEGREE);
 +
    goal_pos_i = 0;
 +
  }
 +
 
 +
  float pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
 +
  DEBUG_SERIAL.print(goal_pos);
 +
  DEBUG_SERIAL.print(" ");
 +
  DEBUG_SERIAL.println(pos_d);
 +
  goal_pos_i++;
 +
  delay(20);
 +
}
 +
</syntaxhighlight>

Latest revision as of 23:49, 21 April 2021

Prerequisites

This tutorial assumes that you have the following:

  • 1x DYNAMIXEL Shield for Arduino MKR Series
  • 1x Arduino MKR compatible device. Note: This tutorial uses the Arduino MKR1000
  • 1x Compatible 1S LiPo battery with a JST2.0 connector. (Please check for proper polarity)
  • Nx DYNAMIXEL XL330-M288-T ROBOTIS actuators where N >= 1

Examples

This shows examples on how to use the DYNAMIXEL Shield for MKR with the DYNAMIXEL XL330-M288-T ROBOTIS actuators.

Ping

The first step is to make sure you can ping your actuator. This will let you know if you can connect to your actuator.

Wiring

Do the following steps to hook up the system for this test:

  1. Firmly place the DYNAMIXEL Shield on top or below the Arduino MKR1000. Note there may be about 0.5cm (0.2in) of headers still showing.
  2. Attach one end of the wire that came in the XL330's box wo one of the three ports on the shield that it fits into. Please noted that it is keyed. If it does not fit in easily you may be trying it backwards.
  3. Attach the other end of the wire from the previous step to the XL330 actuator.
  4. Attach the fully charged 1S battery to the JST2.0 battery port on the MKR shield. This will power the actuator. Do NOT hook it up to the JST2.0 port on the Arduino MKR1000. Please note that if you hook it up to the battery port on the Arduino side it will NOT power the actuator.
  5. If the actuator has factory setting it will blink one when it is powered on. Additionally it will have the following attributes:
ID: 1
Protocol: DYNAMIXEL Protocol 2.0
BAUD: 57600

To program the device you need to plug in the USB cable into your computer.

Code

Include the DYNAMIXEL Protocol Headers

If you have not installed the DYNAMIXEL Protocol Headers for Arduino please install them now. This example used the library found here: Dynamixel2Arduino-master.zip

 #include <Dynamixel2Arduino.h>

Setup your serial ports

Note that the DXL_DIR_PIN is on pin A6, this allows the system to turn the full-duplex communication of the Arduino into the required half-duplex of the actuator.

 #define DXL_SERIAL   Serial1
 #define DEBUG_SERIAL Serial
 const uint8_t DXL_DIR_PIN = A6; // DYNAMIXEL Shield DIR PIN 

Define protocol, baud, and ID

This step assumes that your actuator is at the default settings.

 const uint8_t DXL_ID = 1;
 const float DXL_PROTOCOL_VERSION = 2.0;
 const int DXL_BAUD = 57600;

Create the DYNAMIXEL communications object

 Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

Setup the control table

This namespace is required to use Control table item names

 using namespace ControlTableItem;

Inside your setup() function

The next part is inside your setup function, you will start your serial for the actuators and the serial for debugging as well as set the DYNAMIXEL Protocol version

Debug Serial:

 DEBUG_SERIAL.begin(115200);

DYNAMIXEL Com port:

 dxl.begin(DXL_BAUD);


Set Protocol for DYNAMIXEL:

 dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);


Inside your loop() function

The next part is inside your loop function, here you will ping the ID specified above and print out if you get a response or not. The system will delay 500ms (0.5sec) between pings.

 if(dxl.ping(DXL_ID) == true) DEBUG_SERIAL.println("success");
 else DEBUG_SERIAL.println("error");
 delay(500);

Full Code

This will ping the actuator with ID=1 and a baud of 57600. It will return "success" if it see the actuator and "error" if it does not.

  1. #include <Dynamixel2Arduino.h>
  2.  
  3. #define DXL_SERIAL   Serial1
  4. #define DEBUG_SERIAL Serial
  5. const uint8_t DXL_DIR_PIN = A6;
  6.  
  7. const uint8_t DXL_ID = 1;
  8. const float DXL_PROTOCOL_VERSION = 2.0;
  9. const int DXL_BAUD = 57600;
  10. Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
  11. using namespace ControlTableItem;
  12.  
  13. void setup() {
  14.   DEBUG_SERIAL.begin(115200);
  15.   dxl.begin(DXL_BAUD);
  16.   dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  17. }
  18.  
  19. void loop() {
  20.   if(dxl.ping(DXL_ID) == true) DEBUG_SERIAL.println("success");
  21.   else DEBUG_SERIAL.println("error");
  22.   delay(500);
  23. }

Set Position

This example shows you how to set the position of the actuator. This example assumes you have the base setup from the "ping" example. Everything from "ping" is needed except for what is in the loop() function.

Code

Enable the actuator in the setup

before we can move the actuator we need to enable the actuator, set the control mode to position, turn on the torque, and limit the maximum velocity. Put the following at the END of the setup() function. This means any place in the setup function after you have started the actuator object and set the protocol version.

Turn off torque before modifying the paramaters:

 dxl.torqueOff(DXL_ID);

Put into position control mode:

 dxl.setOperatingMode(DXL_ID, OP_POSITION);

Enable Torque

 dxl.torqueOn(DXL_ID);

Limit maximum speed (note set 0 for max speed - not recommended):

 dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);


Set the motors position using "ticks"

Within the loop() function set the position to 512 "ticks" then delay for 1500 ms (1.5 sec). Note that the XL330 actuator has 4096 "ticks" per revolution and is continuous rotation. This means that 2048 is the center position:

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

Then set the position to 1023 "ticks" then delay for 1500 ms (1.5 sec):

 dxl.setGoalPosition(DXL_ID, 1023);
 delay(1500);

Then set the position to 0 "ticks" then delay for 1500 ms (1.5 sec):

 dxl.setGoalPosition(DXL_ID, 0);
 delay(1500);


Set the motors position using "degrees"

Within the loop() function set the position to 0 degrees then delay for 3000 ms (3 sec). Please note that 180 degrees is center position.

 dxl.setGoalPosition(DXL_ID, 0, UNIT_DEGREE);
 delay(3000);

Then set the position to 1023 degrees then delay for 3000 ms (3 sec):

 dxl.setGoalPosition(DXL_ID, 180, UNIT_DEGREE);
 delay(3000);

Then set the position to 0 degrees then delay for 3000 ms (3 sec):

 dxl.setGoalPosition(DXL_ID, 270, UNIT_DEGREE);
 delay(3000);

Full Code (using "ticks")

  1. #include <Dynamixel2Arduino.h>
  2.  
  3. #define DXL_SERIAL   Serial1
  4. #define DEBUG_SERIAL Serial
  5. const uint8_t DXL_DIR_PIN = A6;
  6.  
  7. const uint8_t DXL_ID = 1;
  8. const float DXL_PROTOCOL_VERSION = 2.0;
  9. const int DXL_BAUD = 57600;
  10. Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
  11. using namespace ControlTableItem;
  12.  
  13. void setup() {
  14.   DEBUG_SERIAL.begin(115200);
  15.   dxl.begin(DXL_BAUD);
  16.   dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  17.  
  18.   dxl.torqueOff(DXL_ID);
  19.   dxl.setOperatingMode(DXL_ID, OP_POSITION);
  20.   dxl.torqueOn(DXL_ID);
  21.   dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
  22. }
  23.  
  24. void loop() {
  25.   dxl.setGoalPosition(DXL_ID, 512);
  26.   delay(1500);
  27.   dxl.setGoalPosition(DXL_ID, 1023);
  28.   delay(1500);
  29.   dxl.setGoalPosition(DXL_ID, 0);
  30.   delay(1500);
  31. }

Full Code (using "degrees")

  1. #include <Dynamixel2Arduino.h>
  2.  
  3. #define DXL_SERIAL   Serial1
  4. #define DEBUG_SERIAL Serial
  5. const uint8_t DXL_DIR_PIN = A6;
  6.  
  7. const uint8_t DXL_ID = 1;
  8. const float DXL_PROTOCOL_VERSION = 2.0;
  9. const int DXL_BAUD = 57600;
  10. Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
  11. using namespace ControlTableItem;
  12.  
  13. void setup() {
  14.   DEBUG_SERIAL.begin(115200);
  15.   dxl.begin(DXL_BAUD);
  16.   dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  17.  
  18.   dxl.torqueOff(DXL_ID);
  19.   dxl.setOperatingMode(DXL_ID, OP_POSITION);
  20.   dxl.torqueOn(DXL_ID);
  21.   dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
  22. }
  23.  
  24. void loop() {
  25.   dxl.setGoalPosition(DXL_ID, 0.0, UNIT_DEGREE);
  26.   delay(3000);
  27.   dxl.setGoalPosition(DXL_ID, 180.0, UNIT_DEGREE);
  28.   delay(3000);
  29.   dxl.setGoalPosition(DXL_ID, 270.0, UNIT_DEGREE);
  30.   delay(3000);
  31. }

Get Position

This example shows you how to get the position of the actuator. This example assumes you have the base setup from the "set position in degrees" example. Everything from the latter example is needed including what is in the loop() function.

Code

Get the motors position using "ticks"

Within the loop() function, after you have set the position but before the delay add the following:

 int pos_t = dxl.getPresentPosition(DXL_ID);
 DEBUG_SERIAL.println(pos_t);

Get the motors position using "degrees"

Within the loop() function, after you have set the position but before the delay add the following:

 float pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
 DEBUG_SERIAL.println(pos_d);

Full Code (using "ticks" and "degrees")

  1. #include <Dynamixel2Arduino.h>
  2.  
  3. #define DXL_SERIAL   Serial1
  4. #define DEBUG_SERIAL Serial
  5. const uint8_t DXL_DIR_PIN = A6;
  6.  
  7. const uint8_t DXL_ID = 1;
  8. const float DXL_PROTOCOL_VERSION = 2.0;
  9. const int DXL_BAUD = 57600;
  10. Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
  11. using namespace ControlTableItem;
  12.  
  13. void setup() {
  14.   DEBUG_SERIAL.begin(115200);
  15.   dxl.begin(DXL_BAUD);
  16.   dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  17.  
  18.   dxl.torqueOff(DXL_ID);
  19.   dxl.setOperatingMode(DXL_ID, OP_POSITION);
  20.   dxl.torqueOn(DXL_ID);
  21.   dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
  22. }
  23.  
  24. void loop() {
  25.   dxl.setGoalPosition(DXL_ID, 512);
  26.   delay(1500);
  27.   dxl.setGoalPosition(DXL_ID, 1023);
  28.   delay(1500);
  29.   dxl.setGoalPosition(DXL_ID, 0);
  30.   delay(1500);
  31. }

Full Code (using "ticks" and "degrees")

Note that slight changes were made below to make sure it can compile and print properly. The resulting code will print the current actuator position in ticks and degrees.

  1. #include <Dynamixel2Arduino.h>
  2.  
  3. #define DXL_SERIAL   Serial1
  4. #define DEBUG_SERIAL Serial
  5. const uint8_t DXL_DIR_PIN = A6;
  6.  
  7. const uint8_t DXL_ID = 1;
  8. const float DXL_PROTOCOL_VERSION = 2.0;
  9. const int DXL_BAUD = 57600;
  10. Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
  11. using namespace ControlTableItem;
  12.  
  13. void setup() {
  14.   DEBUG_SERIAL.begin(115200);
  15.   dxl.begin(DXL_BAUD);
  16.   dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  17.  
  18.   dxl.torqueOff(DXL_ID);
  19.   dxl.setOperatingMode(DXL_ID, OP_POSITION);
  20.   dxl.torqueOn(DXL_ID);
  21.   dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
  22. }
  23.  
  24. void loop() {
  25.   /*
  26.   dxl.setGoalPosition(DXL_ID, 512);
  27.   delay(1500);
  28.   dxl.setGoalPosition(DXL_ID, 1023);
  29.   delay(1500);
  30.   dxl.setGoalPosition(DXL_ID, 0);
  31.   delay(1500);
  32.   */
  33.   dxl.setGoalPosition(DXL_ID, 0.0, UNIT_DEGREE);
  34.   int pos_t = dxl.getPresentPosition(DXL_ID);
  35.   DEBUG_SERIAL.print(pos_t);
  36.   DEBUG_SERIAL.print(" ");
  37.   float pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
  38.   DEBUG_SERIAL.println(pos_d);
  39.   delay(3000);
  40.  
  41.   dxl.setGoalPosition(DXL_ID, 180.0, UNIT_DEGREE);
  42.   pos_t = dxl.getPresentPosition(DXL_ID);
  43.   DEBUG_SERIAL.print(pos_t);
  44.   DEBUG_SERIAL.print(" ");
  45.   pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
  46.   DEBUG_SERIAL.println(pos_d);
  47.   delay(3000);
  48.  
  49.   dxl.setGoalPosition(DXL_ID, 270.0, UNIT_DEGREE);
  50.   pos_t = dxl.getPresentPosition(DXL_ID);
  51.   DEBUG_SERIAL.print(pos_t);
  52.   DEBUG_SERIAL.print(" ");
  53.   pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
  54.   DEBUG_SERIAL.println(pos_d);
  55.   delay(3000);
  56. }

Read position fast example

In this example we are reading the position quickly and plotting out set point for our actuator and our actual position in degrees. This is a slight modification of the "Get Position" example above. The full code is listed below for this example. It is important to note that we are reading the position at approximately 50hz (note the delay 20 ms at the bottom of the loop). We are also alternating the set point of the actuator between 90 degrees and 270 degrees. The animated plots below shows what happens when we change between a 90 degrees set point and a 270 degree set point with one (1) second between set points and with three (3) seconds between set points.


Change between a 90 degrees set point and a 270 degree set point with three (3) second between set points

Mkr-robotis-step-slow.gif


Change between a 90 degrees set point and a 270 degree set point with one (1) second between set points

Mkr-robotis-step-fast.gif

Note how if we change the set point too fast the actuator does not reach it's goal position.

Full Code

Change Set Point Timing

For an approximate one (1) second delay between change in set points:

 GOAL_POS_I_MAX 50

For an approximate three (3) second delay between change in set points:

 GOAL_POS_I_MAX 150


Code

  1. #include <Dynamixel2Arduino.h>
  2.  
  3. #define DXL_SERIAL   Serial1
  4. #define DEBUG_SERIAL Serial
  5. const uint8_t DXL_DIR_PIN = A6;
  6.  
  7. const uint8_t DXL_ID = 1;
  8. const float DXL_PROTOCOL_VERSION = 2.0;
  9. const int DXL_BAUD = 57600;
  10. Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
  11. using namespace ControlTableItem;
  12.  
  13. void setup() {
  14.   DEBUG_SERIAL.begin(115200);
  15.   dxl.begin(DXL_BAUD);
  16.   dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  17.  
  18.   dxl.torqueOff(DXL_ID);
  19.   dxl.setOperatingMode(DXL_ID, OP_POSITION);
  20.   dxl.torqueOn(DXL_ID);
  21.   dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 60);
  22. }
  23.  
  24. float goal_pos_0 = 90.0;
  25. float goal_pos_1 = 270.0;
  26. float goal_pos = goal_pos_0;
  27. int goal_pos_i = 0;
  28. #define GOAL_POS_I_MAX 50
  29. void loop() {
  30.   if(goal_pos_i > GOAL_POS_I_MAX)
  31.   {
  32.     if(goal_pos == goal_pos_0) goal_pos = goal_pos_1;
  33.     else goal_pos = goal_pos_0;
  34.     dxl.setGoalPosition(DXL_ID, goal_pos, UNIT_DEGREE);
  35.     goal_pos_i = 0;
  36.   }
  37.  
  38.   float pos_d = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
  39.   DEBUG_SERIAL.print(goal_pos);
  40.   DEBUG_SERIAL.print(" ");
  41.   DEBUG_SERIAL.println(pos_d);
  42.   goal_pos_i++;
  43.   delay(20);
  44. }