Dynamixel Actuators Code Overview - Carleton-SRCL/SPOT GitHub Wiki
This section of the Wiki will cover the low-level code for some of the Dynamixel custom device drivers. The code for the Dynamixel blocks is located under /Custom_Library/Dynamixel_Actuators/. Inside this directory, you will find a src folder and an include folder, as well as a device driver file called Dynamixel_Controller.m. This device driver acts as the universal controller for the manipulator. You will only ever have ONE of these in your diagram; you can change the control mode by sending a 0, 1, 2, or 3 to the device driver (more on that below). Depending on which control mode you are in, the expected input for each joint changes.
NOTE: In order to switch modes, you must go from your current mode to DISABLED (mode 0); after you have disabled the actuators, you can set the new mode (for example, if you want to do torque control in phase #1, and then switch to the position controllers in phase #2).
The first section from the device driver to understand is the properties section:
properties
POSITION_P_GAIN = 400;
POSITION_I_GAIN = 0;
POSITION_D_GAIN = 200;
MAX_POSITION = 3072;
MIN_POSITION = 1024;
MOVE_TIME = 0;
CURRENT_LIMIT = 850;
SPEED_P_GAIN = 100;
SPEED_I_GAIN = 1920;
VELOCITY_LIMIT = 1023;
ACCELERATION_TIME = 0;
end
This section lists the properties of the Simulink block that you will be bale to edit, as well as their default values. The definitions for the properties are as follows:
POSITION_P_GAIN: This sets the proportional gain for the POSITION CONTROL MODE.
POSITION_I_GAIN: This sets the integral gain for the POSITION CONTROL MODE.
POSITION_D_GAIN: This sets the derivative gain for the POSITION CONTROL MODE.
MAX_POSITION: This sets the maximum allowable position for the actuators. The range is 0 to 4096; this corresponds to -PI to PI. So the default limit is -PI/2 to PI/2, which is the hardware limit for the manipulator joints.
MIN_POSITION: This sets the minimum allowable position for the actuators.
MOVE_TIME: This controls the period of time over which the joints will move to their commanded positions.
CURRENT_LIMIT: This defines the maximum current (and therefore the maximum torque) of the actuators when in TORQUE CONTROL MODE. The range is 0 to 1023, where at 1023 the actuators will use 100% of their maximum torque. At 512, they will use 50% of their maximum torque. The stall torque for these actuators is 6 Nm.
SPEED_P_GAIN: This defines the proportional gain for the SPEED CONTROL MODE.
SPEED_I_GAIN: This defines the integral gain for the SPEED CONTROL MODE.
VELOCITY LIMIT: This sets the maximum angular velocity of the joints when in SPEED CONTROL MODE. The range is 0 to 1023 and the units for each increment are 0.229 RPM (so the maximum speed setting is 234 RPM). By default, we leave this essentially unlimited (1023).
ACCELERATION_TIME: This parameter defines the time to achieve the desired velocity in SPEED CONTROL MODE.
The next section of import is the setupImpl method, defined as:
function setupImpl(obj)
if isempty(coder.target)
% Place simulation setup code here
else
coder.cinclude('dynamixel_sdk.h');
coder.cinclude('dynamixel_functions.h');
coder.ceval('initialize_dynamixel');
end
end
This function is executed ONCE on startup and simply initializes the actuators. It does this by running the initialize_dynamixel C++ function, which will be covered later in this Wiki entry. This function is then followed by the stepImpl method:
function stepImpl(obj,Control_Mode, Joint1_Command, Joint2_Command, Joint3_Command)
if isempty(coder.target)
% Place simulation output code here
else
% Call C-function implementing device output
% include the dynamicel functions
coder.cinclude('dynamixel_sdk.h');
coder.cinclude('dynamixel_functions.h');
% Run the main controller code. If the switch state is
% true then this code will initialize the parameters and
% then start the actuator, and THEN run the command. If the
% switch state is false, it will not reinitialize the
% motor
coder.ceval('dynamixel_controller',Control_Mode, obj.POSITION_P_GAIN, obj.POSITION_I_GAIN, obj.POSITION_D_GAIN, obj.MAX_POSITION,...
obj.MIN_POSITION, obj.MOVE_TIME, Joint1_Command, ...
Joint2_Command, Joint3_Command, obj.CURRENT_LIMIT, Joint1_Command, Joint2_Command, Joint3_Command, obj.SPEED_P_GAIN,...
obj.SPEED_I_GAIN, obj.VELOCITY_LIMIT, Joint1_Command, Joint2_Command, Joint3_Command, ...
obj.ACCELERATION_TIME);
end
end
This function is run every time step during the execution of the Simulink diagram. It calls the C++ function dynamixel_controller - covered later - which sets the parameters and send the commands to the actuators. The last function to look at is the releaseImpl function:
function releaseImpl(obj) %#ok<MANU>
if isempty(coder.target)
% Place simulation termination code here
else
coder.cinclude('dynamixel_sdk.h');
coder.cinclude('dynamixel_functions.h');
coder.ceval('terminate_dynamixel');
end
end
This function is run only once at the END of the experiment, after Stop Experiment has been pressed. More specifically, it is executed when the command kill -15 <PID> is sent in Linux - this corresponds to the TERMINATE command in Linux.
Having covered the main contents of the device driver for the Dynamixel actuators, we can now dig into the C++ functions which are called in the device driver. The C++ functions are contained within the src directory, in the dynamixel_functions.cpp file.
The first portion of the C++ file contains all of the necessary includes and defines:
// Get methods and members of PortHandlerLinux or PortHandlerWindows #include <fcntl.h> #include <termios.h> #include <stdlib.h> #include <stdio.h> #include <tgmath.h>
// To do the modulus operator on doubles
#include <cmath>
// Include the dynamixel headers
#include "dynamixel_sdk.h"
#include "dynamixel_functions.h"
// Define the device name, baudrate, and protocol
#define DEVICENAME "/dev/ttyUSB0"
#define BAUDRATE 1000000
#define PROTOCOL_VERSION 2.0
// Control table addresses
#define ADDR_MX_GOAL_POSITION 116
#define ADDR_MX_PRESENT_POSITION 132
#define ADDR_MX_GOAL_SPEED 104
#define ADDR_MX_GOAL_CURRENT 102
#define ADDR_MX_PRESENT_SPEED 128
#define ADDR_MX_PRESENT_LOAD 126
#define ADDR_MX_POSITION_P_GAIN 84
#define ADDR_MX_POSITION_I_GAIN 82
#define ADDR_MX_POSITION_D_GAIN 80
#define ADDR_MX_VELOCITY_P_GAIN 78
#define ADDR_MX_VELOCITY_I_GAIN 76
#define ADDR_MX_TORQUE_ENABLE 64
#define ADDR_MX_MAX_POSITION 48
#define ADDR_MX_MIN_POSITION 52
#define ADDR_MX_VELOCITY_LIMIT 44
#define ADDR_MX_VELOCITY_PROFILE 112
#define ADDR_MX_CURRENT_LIMIT 38
#define ADDR_MX_RETURN_DELAY 9
#define ADDR_MX_DRIVE_MODE 10
#define ADDR_MX_OPERATING_MODE 11
#define ADDR_MX_PWM_LIMIT 36
#define ADDR_MX_GOAL_PWM 100
#define ADDR_MX_PROFILE_ACCELERATION 108
// Define the port handler and packet handler variables
dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Initialize GroupBulkWrite instance
dynamixel::GroupBulkWrite groupBulkWrite(portHandler, packetHandler);
// Initialize GroupBulkRead instance
dynamixel::GroupBulkRead groupBulkRead(portHandler, packetHandler);
// Initialize Groupsyncread instance for Present Speed & Position [first 4 are speed, next 4 are position, luckily the values are right beside each other in the control table]
dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_MX_PRESENT_SPEED, 8);
int run_once_flag = 0;
The user should almost never need to change anything here. Unless you are setting up new hardware, the only case where something may need to be changed is the #define DEVICENAME, which might be different depending on how many devices you have connected to the Xavier.
The function initialize_dynamixel() is defined as follows:
void initialize_dynamixel()
{
// Open COM port for serial communication with the actuators
portHandler->openPort();
// Set port baudrate
portHandler->setBaudRate(BAUDRATE);
}
This function opens a COM port based on the device name, and then sets the baud rate for the device.
The main controller is defined in the dynamixel_controller() function. This is a large function so we will break it down into smaller chunks. First, let's take a look at the inputs for the function:
void dynamixel_controller(int CONTROL_MODE, double POSITION_P_GAIN, double POSITION_I_GAIN, double POSITION_D_GAIN, double MAX_POSITION,
double MIN_POSITION, double MOVE_TIME, double JOINT1_POS_RAD,
double JOINT2_POS_RAD, double JOINT3_POS_RAD, double CURRENT_LIMIT, int JOINT1_BITS, int JOINT2_BITS, int JOINT3_BITS, double SPEED_P_GAIN,
double SPEED_I_GAIN, double VELOCITY_LIMIT, double JOINT1_SPD_RAD, double JOINT2_SPD_RAD, double JOINT3_SPD_RAD,
double ACCELERATION_TIME)
These inputs are the same as the ones described earlier in this wiki entry, under the properties section. Inside the function, the first chunk of code is as follows:
// Define the transmission failure code
int dxl_comm_result = COMM_TX_FAIL;
// Initialize the dxl_error variable
uint8_t dxl_error = 0;
if (CONTROL_MODE == 0 && run_once_flag == 0) {
terminate_dynamixel();
run_once_flag = 1;
// Set the actuators settings
}
This code initializes a variable that is used to check for communication failures. It also initializes a variable used for checking for errors. Next is an IF statement, which checks if the control mode is 0 (reset mode) AND if the run_once_flag is 0. The run_once_flag is used to determine if a reset has occurred, in which case there are some functions that need to be run before any commands can be sent to the actuators in their new mode of operation. If this IF statement is TRUE, then the dynamixel is terminated and the flag is set to be 1. The next chunk of code checks contains more initialization code. If the run_once_flag is 1 (meaning a reset has occured), then these statements check which CONTROL_MODE has been selected and then sets the properties accordingly. The writeXByteTxRx() function is a part of the Dynamixel API, and won't be covered here. For most people, this simply sends information to the actuators in bytes. If an errors is returned by the function, it is passed to the dxl_error variable.
if (CONTROL_MODE == 1 && run_once_flag == 1) { // Set up motors for position control
initialize_dynamixel();
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_OPERATING_MODE, 3, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_DRIVE_MODE, 4, &dxl_error); // Velocity_profile yields the time required to reach goal position
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 1, ADDR_MX_POSITION_P_GAIN, POSITION_P_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 1, ADDR_MX_POSITION_I_GAIN, POSITION_I_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 1, ADDR_MX_POSITION_D_GAIN, POSITION_D_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 1, ADDR_MX_MAX_POSITION, MAX_POSITION, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 1, ADDR_MX_MIN_POSITION, MIN_POSITION, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 1, ADDR_MX_VELOCITY_PROFILE, MOVE_TIME, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 1, ADDR_MX_PROFILE_ACCELERATION, 0, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_OPERATING_MODE, 3, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_DRIVE_MODE, 4, &dxl_error); // Velocity_profile yields the time required to reach goal position
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 2, ADDR_MX_POSITION_P_GAIN, POSITION_P_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 2, ADDR_MX_POSITION_I_GAIN, POSITION_I_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 2, ADDR_MX_POSITION_D_GAIN, POSITION_D_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 2, ADDR_MX_MAX_POSITION, MAX_POSITION, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 2, ADDR_MX_MIN_POSITION, MIN_POSITION, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 2, ADDR_MX_VELOCITY_PROFILE, MOVE_TIME, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 2, ADDR_MX_PROFILE_ACCELERATION, 0, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_OPERATING_MODE, 3, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_DRIVE_MODE, 4, &dxl_error); // Velocity_profile yields the time required to reach goal position
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 3, ADDR_MX_POSITION_P_GAIN, POSITION_P_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 3, ADDR_MX_POSITION_I_GAIN, POSITION_I_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 3, ADDR_MX_POSITION_D_GAIN, POSITION_D_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 3, ADDR_MX_MAX_POSITION, MAX_POSITION, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 3, ADDR_MX_MIN_POSITION, MIN_POSITION, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 3, ADDR_MX_VELOCITY_PROFILE, MOVE_TIME, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 3, ADDR_MX_PROFILE_ACCELERATION, 0, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
run_once_flag = 0;
} else if (CONTROL_MODE == 2 && run_once_flag == 1) { // Set up motors for current control
initialize_dynamixel();
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_OPERATING_MODE, 0, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 1, ADDR_MX_CURRENT_LIMIT, CURRENT_LIMIT, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_OPERATING_MODE, 0, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 2, ADDR_MX_CURRENT_LIMIT, CURRENT_LIMIT, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_OPERATING_MODE, 0, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 3, ADDR_MX_CURRENT_LIMIT, CURRENT_LIMIT, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
run_once_flag = 0;
} else if (CONTROL_MODE == 3 && run_once_flag == 1) { // Set up motors for speed control
initialize_dynamixel();
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_OPERATING_MODE, 1, &dxl_error); // Velocity mode
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_DRIVE_MODE, 4, &dxl_error); // Acceleration_profile yields the time required to reach goal velocity
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 1, ADDR_MX_VELOCITY_P_GAIN, SPEED_P_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 1, ADDR_MX_VELOCITY_I_GAIN, SPEED_I_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 1, ADDR_MX_VELOCITY_LIMIT, VELOCITY_LIMIT, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_OPERATING_MODE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_DRIVE_MODE, 4, &dxl_error); // Acceleration_profile yields the time required to reach goal velocity
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 2, ADDR_MX_VELOCITY_P_GAIN, SPEED_P_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 2, ADDR_MX_VELOCITY_I_GAIN, SPEED_I_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 2, ADDR_MX_VELOCITY_LIMIT, VELOCITY_LIMIT, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_OPERATING_MODE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_DRIVE_MODE, 4, &dxl_error); // Acceleration_profile yields the time required to reach goal velocity
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 3, ADDR_MX_VELOCITY_P_GAIN, SPEED_P_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, 3, ADDR_MX_VELOCITY_I_GAIN, SPEED_I_GAIN, &dxl_error);
dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, 3, ADDR_MX_VELOCITY_LIMIT, VELOCITY_LIMIT, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_TORQUE_ENABLE, 1, &dxl_error);
run_once_flag = 0;
}
After this code is run, the run_once_flag is set to 0, and so the initialization code is not run again until a reset is detected. The next code snippet is the controller; first, we have the position controller, which takes the joint position command in radians and then convers it into a bit. This is then broken down into low and high bytes and stored into a 4 byte array. The Dynamixel functions addParam and txPacket are then used to send the command to the actuators, which will then move to the commanded position.
// Run the controller
if (CONTROL_MODE == 1) { // Set up motors for position control
// Define the transmission failure code
int dxl_comm_result = COMM_TX_FAIL;
bool dxl_addparam_result = false;
uint8_t param_goal_position_1[4];
uint8_t param_goal_position_2[4];
uint8_t param_goal_position_3[4];
// Convert the commanded position from radians to raw bits
double JOINT1_POS_BITS = nearbyint(651.74*JOINT1_POS_RAD + 2048);
double JOINT2_POS_BITS = nearbyint(651.74*JOINT2_POS_RAD + 2048);
double JOINT3_POS_BITS = nearbyint(651.74*JOINT3_POS_RAD + 2048);
// Allocate goal position value into byte array
param_goal_position_1[0] = DXL_LOBYTE(DXL_LOWORD(JOINT1_POS_BITS));
param_goal_position_1[1] = DXL_HIBYTE(DXL_LOWORD(JOINT1_POS_BITS));
param_goal_position_1[2] = DXL_LOBYTE(DXL_HIWORD(JOINT1_POS_BITS));
param_goal_position_1[3] = DXL_HIBYTE(DXL_HIWORD(JOINT1_POS_BITS));
param_goal_position_2[0] = DXL_LOBYTE(DXL_LOWORD(JOINT2_POS_BITS));
param_goal_position_2[1] = DXL_HIBYTE(DXL_LOWORD(JOINT2_POS_BITS));
param_goal_position_2[2] = DXL_LOBYTE(DXL_HIWORD(JOINT2_POS_BITS));
param_goal_position_2[3] = DXL_HIBYTE(DXL_HIWORD(JOINT2_POS_BITS));
param_goal_position_3[0] = DXL_LOBYTE(DXL_LOWORD(JOINT3_POS_BITS));
param_goal_position_3[1] = DXL_HIBYTE(DXL_LOWORD(JOINT3_POS_BITS));
param_goal_position_3[2] = DXL_LOBYTE(DXL_HIWORD(JOINT3_POS_BITS));
param_goal_position_3[3] = DXL_HIBYTE(DXL_HIWORD(JOINT3_POS_BITS));
// Initialize the dxl_error variable
uint8_t dxl_error = 0;
// Send the raw (0->4096) initial position value.
dxl_addparam_result = groupBulkWrite.addParam(1, ADDR_MX_GOAL_POSITION, 4, param_goal_position_1);
dxl_addparam_result = groupBulkWrite.addParam(2, ADDR_MX_GOAL_POSITION, 4, param_goal_position_2);
dxl_addparam_result = groupBulkWrite.addParam(3, ADDR_MX_GOAL_POSITION, 4, param_goal_position_3);
dxl_comm_result = groupBulkWrite.txPacket();
groupBulkWrite.clearParam();
}
If the control mode is 2 instead, then the current control mode is used. This is much the same as the position controller, except that there is no conversion. Future users may want to add a direct conversion from torque to bits here, but for now the inputs are simply in bits.
} else if (CONTROL_MODE == 2) { // Set up motors for current control
// Define the transmission failure code
int dxl_comm_result = COMM_TX_FAIL;
bool dxl_addparam_result = false;
uint8_t param_goal_TAU_1[2];
uint8_t param_goal_TAU_2[2];
uint8_t param_goal_TAU_3[2];
// Allocate goal position value into byte array
param_goal_TAU_1[0] = DXL_LOBYTE((JOINT1_BITS));
param_goal_TAU_1[1] = DXL_HIBYTE((JOINT1_BITS));
param_goal_TAU_2[0] = DXL_LOBYTE((JOINT2_BITS));
param_goal_TAU_2[1] = DXL_HIBYTE((JOINT2_BITS));
param_goal_TAU_3[0] = DXL_LOBYTE((JOINT3_BITS));
param_goal_TAU_3[1] = DXL_HIBYTE((JOINT3_BITS));
uint8_t dxl_error = 0;
// Send the raw (0->4096) initial position value.
dxl_addparam_result = groupBulkWrite.addParam(1, ADDR_MX_GOAL_CURRENT, 2, param_goal_TAU_1);
dxl_addparam_result = groupBulkWrite.addParam(2, ADDR_MX_GOAL_CURRENT, 2, param_goal_TAU_2);
dxl_addparam_result = groupBulkWrite.addParam(3, ADDR_MX_GOAL_CURRENT, 2, param_goal_TAU_3);
dxl_comm_result = groupBulkWrite.txPacket();
groupBulkWrite.clearParam();
}
The last segment of code covers the speed control section. Once again, this works the same as the other controllers. In this case there is a conversion from rad/s to bits that happens in the function itself. Also, if there are any negative commands, then you must take their two's compliment to get the 4 byte number.
} else if (CONTROL_MODE == 3) { // Set up motors for speed control
// Define the transmission failure code
int dxl_comm_result = COMM_TX_FAIL;
bool dxl_addparam_result = false;
uint8_t param_goal_speed_1[4];
uint8_t param_goal_speed_2[4];
uint8_t param_goal_speed_3[4];
uint8_t acceleration_time_bits[4];
// Initialize the dxl_error variable
uint8_t dxl_error = 0;
// Convert to RPM
double JOINT1_SPD_RPM = JOINT1_SPD_RAD*9.549296585513702;
double JOINT2_SPD_RPM = JOINT2_SPD_RAD*9.549296585513702;
double JOINT3_SPD_RPM = JOINT3_SPD_RAD*9.549296585513702;
// Convert to bits (max speed of 234.27 corresponds to bit 1023)
double JOINT1_SPD_BITS;
double JOINT2_SPD_BITS;
double JOINT3_SPD_BITS;
JOINT1_SPD_BITS = nearbyint(JOINT1_SPD_RPM * 4.366756307);
JOINT2_SPD_BITS = nearbyint(JOINT2_SPD_RPM * 4.366756307);
JOINT3_SPD_BITS = nearbyint(JOINT3_SPD_RPM * 4.366756307);
// If any commands are negative, take their two's compliment (for the 4 byte [32 bit] number)
if (JOINT1_SPD_BITS < 0)
{
JOINT1_SPD_BITS = JOINT1_SPD_BITS + pow(2,32);
}
if (JOINT2_SPD_BITS < 0)
{
JOINT2_SPD_BITS = JOINT2_SPD_BITS + pow(2,32);
}
if (JOINT3_SPD_BITS < 0)
{
JOINT3_SPD_BITS = JOINT3_SPD_BITS + pow(2,32);
}
// Allocate these into the byte array
param_goal_speed_1[0] = DXL_LOBYTE(DXL_LOWORD(JOINT1_SPD_BITS));
param_goal_speed_1[1] = DXL_HIBYTE(DXL_LOWORD(JOINT1_SPD_BITS));
param_goal_speed_1[2] = DXL_LOBYTE(DXL_HIWORD(JOINT1_SPD_BITS));
param_goal_speed_1[3] = DXL_HIBYTE(DXL_HIWORD(JOINT1_SPD_BITS));
param_goal_speed_2[0] = DXL_LOBYTE(DXL_LOWORD(JOINT2_SPD_BITS));
param_goal_speed_2[1] = DXL_HIBYTE(DXL_LOWORD(JOINT2_SPD_BITS));
param_goal_speed_2[2] = DXL_LOBYTE(DXL_HIWORD(JOINT2_SPD_BITS));
param_goal_speed_2[3] = DXL_HIBYTE(DXL_HIWORD(JOINT2_SPD_BITS));
param_goal_speed_3[0] = DXL_LOBYTE(DXL_LOWORD(JOINT3_SPD_BITS));
param_goal_speed_3[1] = DXL_HIBYTE(DXL_LOWORD(JOINT3_SPD_BITS));
param_goal_speed_3[2] = DXL_LOBYTE(DXL_HIWORD(JOINT3_SPD_BITS));
param_goal_speed_3[3] = DXL_HIBYTE(DXL_HIWORD(JOINT3_SPD_BITS));
acceleration_time_bits[0] = DXL_LOBYTE(DXL_LOWORD(nearbyint(ACCELERATION_TIME)));
acceleration_time_bits[1] = DXL_HIBYTE(DXL_LOWORD(nearbyint(ACCELERATION_TIME)));
acceleration_time_bits[2] = DXL_LOBYTE(DXL_HIWORD(nearbyint(ACCELERATION_TIME)));
acceleration_time_bits[3] = DXL_HIBYTE(DXL_HIWORD(nearbyint(ACCELERATION_TIME)));
// Send the goal velocity command
dxl_addparam_result = groupBulkWrite.addParam(1, ADDR_MX_GOAL_SPEED, 4, param_goal_speed_1);
dxl_addparam_result = groupBulkWrite.addParam(1, ADDR_MX_PROFILE_ACCELERATION, 4, acceleration_time_bits); // Acceleration time in [ms] required to reach goal velocity
dxl_addparam_result = groupBulkWrite.addParam(2, ADDR_MX_GOAL_SPEED, 4, param_goal_speed_2);
dxl_addparam_result = groupBulkWrite.addParam(2, ADDR_MX_PROFILE_ACCELERATION, 4, acceleration_time_bits); // Acceleration time in [ms] required to reach goal velocity
dxl_addparam_result = groupBulkWrite.addParam(3, ADDR_MX_GOAL_SPEED, 4, param_goal_speed_3);
dxl_addparam_result = groupBulkWrite.addParam(3, ADDR_MX_PROFILE_ACCELERATION, 4, acceleration_time_bits); // Acceleration time in [ms] required to reach goal velocity
// Clean up
dxl_comm_result = groupBulkWrite.txPacket();
groupBulkWrite.clearParam();
}
The next function in this file is the read_dynamixel_position() function. The input to this function are pointers for the joint angles and speeds. There are also pointers to the previous joint positions - this is so that if there is no new packet to read, the previous value is held constant rather then returning a zero. Although intimidating, this function relies mostly on the Dynamixel API, and simply retrieves the encoder values and converts them into the correct units.
void read_dynamixel_position(double* JOINT1_POS_RAD, double* JOINT2_POS_RAD, double* JOINT3_POS_RAD, double* JOINT1_SPEED_RAD, double* JOINT2_SPEED_RAD, double* JOINT3_SPEED_RAD, double JOINT1_POS_PREV, double JOINT2_POS_PREV, double JOINT3_POS_PREV) {
// Define the transmission failure code
int dxl_comm_result = COMM_TX_FAIL;
bool dxl_addparam_result = false;
bool dxl_getdata_result = false;
bool dx2_getdata_result = false;
bool dx3_getdata_result = false;
// Syncread present position and velocity
dxl_comm_result = groupSyncRead.txRxPacket();
int32_t dxl1_present_position = 0;
int32_t dxl2_present_position = 0;
int32_t dxl3_present_position = 0;
int32_t dxl1_present_speed = 0;
int32_t dxl2_present_speed = 0;
int32_t dxl3_present_speed = 0;
// Add motors to the groupSyncRead (for read_dynamixel_position())
dxl_addparam_result = groupSyncRead.addParam(1);
dxl_addparam_result = groupSyncRead.addParam(2);
dxl_addparam_result = groupSyncRead.addParam(3);
// Check if groupsyncread data of Dynamixel#1 is available
dxl_getdata_result = groupSyncRead.isAvailable(1, ADDR_MX_PRESENT_POSITION, 4);
if (dxl_getdata_result == false)
{
*JOINT1_POS_RAD = JOINT1_POS_PREV;
} else {
dxl1_present_position = groupSyncRead.getData(1, ADDR_MX_PRESENT_POSITION, 4);
double joint1_wrapped = std::fmod(0.001534363 * dxl1_present_position, 6.283185307179586); // Converting bits to rads, making 0 rad be when the arm joint is extended, and wrapping to [-pi,pi)
if (joint1_wrapped < 0)
joint1_wrapped += 6.283185307179586;
*JOINT1_POS_RAD = joint1_wrapped - 3.14159;
}
// Check if groupsyncread data of Dynamixel#2 is available
dx2_getdata_result = groupSyncRead.isAvailable(2, ADDR_MX_PRESENT_POSITION, 4);
if (dx2_getdata_result == false)
{
*JOINT2_POS_RAD = JOINT2_POS_PREV;
} else {
dxl2_present_position = groupSyncRead.getData(2, ADDR_MX_PRESENT_POSITION, 4);
double joint2_wrapped = std::fmod(0.001534363 * dxl2_present_position, 6.283185307179586); // Converting bits to rads, making 0 rad be when the arm joint is extended, and wrapping to [-pi,pi)
if (joint2_wrapped < 0)
joint2_wrapped += 6.283185307179586;
*JOINT2_POS_RAD = joint2_wrapped - 3.14159;
}
// Check if groupsyncread data of Dynamixel#3 is available
dx3_getdata_result = groupSyncRead.isAvailable(3, ADDR_MX_PRESENT_POSITION, 4);
if (dx3_getdata_result == false)
{
*JOINT3_POS_RAD = JOINT3_POS_PREV;
} else {
dxl3_present_position = groupSyncRead.getData(3, ADDR_MX_PRESENT_POSITION, 4);
double joint3_wrapped = std::fmod(0.001534363 * dxl3_present_position, 6.283185307179586); // Converting bits to rads, making 0 rad be when the arm joint is extended, and wrapping to [-pi,pi)
if (joint3_wrapped < 0)
joint3_wrapped += 6.283185307179586;
*JOINT3_POS_RAD = joint3_wrapped - 3.14159;
}
// Get present velocity values
dxl1_present_speed = groupSyncRead.getData(1, ADDR_MX_PRESENT_SPEED, 4);
dxl2_present_speed = groupSyncRead.getData(2, ADDR_MX_PRESENT_SPEED, 4);
dxl3_present_speed = groupSyncRead.getData(3, ADDR_MX_PRESENT_SPEED, 4);
*JOINT1_SPEED_RAD = 0.023981131017500 * dxl1_present_speed; // Converting bits to rad/s
*JOINT2_SPEED_RAD = 0.023981131017500 * dxl2_present_speed; // Converting bits to rad/s
*JOINT3_SPEED_RAD = 0.023981131017500 * dxl3_present_speed; // Converting bits to rad/s
}
The termination function is called terminate_dynamixel() and it has not inputs. This function sends a command to disable torque on the actuators (i.e. disable the actuators), and then it closes the port:
void terminate_dynamixel()
{
// Define the transmission failure code
int dxl_comm_result = COMM_TX_FAIL;
// Initialize the dxl_error variable
uint8_t dxl_error = 0;
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 1, ADDR_MX_TORQUE_ENABLE, 0, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 2, ADDR_MX_TORQUE_ENABLE, 0, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, 3, ADDR_MX_TORQUE_ENABLE, 0, &dxl_error);
// Open COM port for serial communication with the actuators
portHandler->closePort();
}