Home e-Manual

Help creating custom dynamixel addon for Matlab

We have a robot figure using 5 Dynamixel MX-106Ts. We are needing to control the motors using Matlab thru a Dynamixel Shield equipped Arduino Mega. To do this, I am (attempting) to build a custom Matlab addon using the Dynamixel shield cpp source.

The software workflow goes as such: Matlab 2021 -> matlab library file -> cpp library file -> Arduino (dynamixel2arduino library). The following link gives more details of the requirements.

The first step is creating the cpp library file. I have one created, but I do not believe I have the class member defined correctly, nor do I think the commands in the command handlers are set correctly. Any insight that can be given is appreciated.

Code is attached at the bottom:

/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.

*file dynamixel.h
*
*Class definitions for the Dynamixel shield that wraps the APIs of the Dynamixel Shield and Dynamixel2Arduino libraries
*
*******************************************************************************/

#ifndef DYNAMIXEL_SHIELD_H_
#define DYNAMIXEL_SHIELD_H_

#define MAX_NUMBER_MOTORS 5

#include "LibraryBase.h"
#include "Arduino.h"
#include "Dynamixel2Arduino.h"


#ifndef DYNAMIXEL_2_ARDUINO_H_
#error "\r\nWarning : To use DynamixelShield, you must install Dynamixel2Arduino library."
#error "\r\nWarning : Please search and install Dynamixel2Arduino in Arduino Library Manager. (For version dependencies, see http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/)"
#endif


// https://www.arduino.cc/reference/en/language/functions/communication/serial/
//#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAM_DUE)
//  #define DXL_SERIAL   Serial
//#else
#define DXL_SERIAL   Serial3
//#endif

//#if defined(USE_ARDUINO_MKR_PIN_LAYOUT) || defined(ARDUINO_PORTENTA_H7_M7) || defined(ARDUINO_PORTENTA_H7_M4)
//  #define DXL_DIR_PIN		A6
//#else
#define DXL_DIR_PIN	2
//#endif

// Command ID's
#define SET_BAUD				0x01
#define SET_POSITION			0x02
#define SET_VELOCITY			0x03
#define SET_CURRENT				0x04
#define GET_POSITION			0x05
#define GET_VELOCITY			0x06
#define GET_CURRENT				0x07

class DynamixelShield : public LibraryBase
{
//Most of the public functions of this class inherit the API of Dynamixel2Arduino.
//So, if you want to modify or view the code, please refer to the code in the Dynamixel2Arduino library. 

public:
	// Constructor
	DynamixelShield(MWArduinoClass& a)
	{
		//define library name
		libName = "DynamixelShield";
		a.registerLibrary(this);
	}
	
public:
  DynamixelShield(HardwareSerial& port = DXL_SERIAL, int dir_pin = DXL_DIR_PIN);
  ~DynamixelShield();
  
public:
	Dynamixel2Arduino* motors[MAX_NUMBER_MOTORS];
  
public:
   void commandHandler(byte cmdID, byte* dataIn, unsigned int payloadSize)
   {
		switch (cmdID)
		{
			case SET_BAUD: {
				// Get the motor ID
				byte motorID = dataIn[0];
				VALUE value;
				// Get the new baudrate
				byte newBaud = dataIn[1];
				//set the new baudrate to the desired motor
				motors[motorID]->setBaudrate(motorID, newBaud);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			case SET_POSITION: {
				// Get the motor ID
				byte motorID = dataIn[0];
				VALUE value;
				// Get the desired position
				byte newPosition = dataIn[1];
				//set the new position to the desired motor in encoder counts
				motors[motorID]->setGoalPosition(motorID, newPosition);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			case SET_VELOCITY: {
				// Get the motor ID
				byte motorID = dataIn[0];
				VALUE value;
				// Get the desired position
				byte newVelocity = dataIn[1];
				//set the new velocity to the desired motor in rads/sec
				motors[motorID]->setGoalVelocity(motorID, newVelocity);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			case SET_CURRENT: {
				// Get the motor ID
				byte motorID = dataIn[0];
				VALUE value;
				// Get the desired position
				byte newCurrent = dataIn[1];
				//set the new current to the desired motor in mA
				motors[motorID]->setGoalCurrent(motorID, newCurrent);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			case GET_POSITION: {
				// Get the motor ID
				byte motorID = dataIn[0];
				//read the current position of the desired motor in encoder counts
				value.number = motors[motorID]->getPresentPosition(motorID);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			case GET_VELOCITY: {
				// Get the motor ID
				byte motorID = dataIn[0];
				//read the current velocity of the desired motor in rads/sec
				value.number = motors[motorID]->getPresentVelocity(motorID);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			case GET_CURRENT: {
				// Get the motor ID
				byte motorID = dataIn[0];
				//read the current motor current of the desired motor in mA
				value.number = motors[motorID]->getPresentCurrent(motorID);
				// Send response
				sendResponseMsg(cmdID, value.bytes, 4);
				break;
			}
			
			default: {
                // Print debug string
                debugPrint(MSG_UNKNOWN_CMD);
                break;
            }
        }
    }
};

Hi @radugbhr, thanks for your post and welcome to our Community Page! It’s great to hear you are working with DYNAMIXEL for your MatLAB system- I have a couple questions and can provide some recommendations below:

  1. For my understanding, is there a specific reason the Dynamixel Shield + Arduino MEGA were chosen to interface to MatLAB? If it’s helpful to reference, ROBOTIS offers libraries compatible with MatLAB through our DYNAMIXEL SDK.

  2. We also have tutorial videos for our DYNAMIXEL SDK code using MatLAB here: https://www.youtube.com/watch?v=1VWksgPqyDs . These tutorials are using our U2D2 USB-serial converter to connect the DYNAMIXEL to the computer instead of the Arduino.

As my experience with MatLAB is limited, allow me to tag some of our members who may have more insight to help: @Yogurt_Man or @willson, would you be able to shed some light on this topic?

1 Like

Hi Andrew. Thank you for the reply. We are using the Matlab-Arduino combination for two reasons.

  1. The arduino is simultaneously controlling a motor driver shield connected to an electromagnet.

  2. The Matlab program is implementing and outputting a 5th degree polynomial motion profile for the Dynamixel motors to follow.

So both the arduino with Dynamixel shield and Matlab program are required for the project to function at intended.

The code I posted above is not a Matlab code, but a C++ header file. It’s purpose it to map each of the arduino commands to a c++ command that Matlab can interpret.

Hi. I didnt get your question. What is the point of question radugbhr? I could give you more help if you threw out qusetion in detail such as some
error outout.

Thank you for the reply. I am needing someone to check my c++ header code that I posted in the OP. The purpose of the c++ code is to map the dynamixel2arduino read and write functions (getPresentPosition, setGoalPosition) to c++ command handler functions that Matlab can read and interpret.

Hi @radugbhr,

One of the hassles with using Mega with DYNAMIXEL Shield is that Mega shares USB CDC with its Serial port.
If you are communicating with MatLAB through the USB cable, this serial port restriction will need to be resolved first.

DYNAMIXEL Shield specifically uses Serial port to communicate with DYNAMIXEL, therefore, setting Serial3 for DXL_SERIAL will fail to communicate with DYNAMIXEL.
https://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/#layout

Once you resolve the DYNAMIXEL Serial port issue, you will need another serial port to communicate with your PC(MatLAB).
You may use a SW UART port on DYNAMIXEL Shield with LN-101 interface, but since this is a software serial, it is recommended to use a lower baudrate such as 57,600 bps.

1 Like

Thank you for the reply. I have already solved the serial port conflict issue by wiring the Dynamixel shield serial pins to pins 14 and 15 (Serial3) on the Arduino Mega 2560. I tested by building/compiling the DynamixelShield and Dynamixel2Arduino libraries to the Arduino used a test script to check the connection while displaying data over serial monitor. This method appears to work.

I’ve created the Matlab library file to match the cpp header file. Still need assistance coding the command handler commands that write values to the motors.

classdef DynamixelShield < matlabshared.addon.LibraryBase $ matlab.mixin.CustomDisplay
% This creates the Dynamixel Shield device object, which is connected to the serial 3 port on the Arduino

properties(Access = private, Constant = true) %all commands and hex values must match the cpp header file
	SET_BAUD		= hex2dec('01')
	SET_POSITION	= hex2dec('02')
	SET_VELOCITY 	= hex2dec('03')
	SET_CURRENT		= hex2dec('04')
	GET_POSITION	= hex2dec('05')
	GET_VELOCITY	= hex2dec('06')
	GET_CURRENT		= hex2dec('07')
	
	MAX_NUMBER_MOTORS = 5
end

properties(Access = protected, Constant = true)
	LibraryName = 'DynamixelShield'
	DependentLibraries - {} %unsure if dependent Arduino libraries need to be declared here
	LibraryHeaderFiles = {'Dynamixel2Arduino.h'}  %syntax on full file path?
	CppHeaderFile = fullfile(arduinoio.FilePath(mfilename('fullpath')), 'src', 'DynamixelShield.h')
	CppClassName = 'DynamixelShield'
end

properties(Access = private) %declare all variables that Matlab will pass to Arduino
	motorID;
	newBaud;
	newPosition;
	newVelocity;
	newCurrent;
	ResourceOwner = 'DynamixelShield';
end

properties(SetAccess = private)
	Serial
end

%Constructor
methods(Hidden, Access = public)
	function obj = DynamixelShield(parentObj, Serial)
		%This function connects to the dynamixel shield
		%dshield = addon(a,'DynamixelShield', Serial3) should creates
		%a connection to the dynamixel shield on the 
		%Arduino Mega 2560 object, where a is the arduino object in Matlab,
		%'DynamixelShield' is the library, and Serial is the Serial
		%port the dynamixel is connected to, which is Serial3.
	
		obj.Parent = parentObj;
	
		try
			%error handling for invalid motor ID
			obj.motorID = getResourceCount(obj.parent, 'DynamixelShield');
			if obj.motorID >= obj.MAX_NUMBER_MOTORS
			error('DynamixelShield:ValueError', 'You have given an invalid motor ID', obj.MAX_NUMBER_MOTORS);
		end
		incrementResourceCount(obj.Parent, 'DynamixelShield'); %is the Resource count necessary?
		%Create DynamixelShield
		createDynamixelShield(obj);
		catch e
			throwAsCaller(e);
		end
	end
end

methods(Access - protected)
	function delete(obj)
		try
			parentObj = obj.Parent;
			decrementResourceCount(obj.Parent, 'DynamixelShield');
			deleteDynamixelShield(obj);
		catch
			%no error needed
		end
	end
end

methods(Access = public) %need to add in commands to send values (Baud, position, velocity, current)
	%this will read a Position value from the motor ID entered
	function val = getPosition(obj.motorID);
		cmdID = obj.GET_POSITION;
	
		try
			val = sendCommand(obj, obj.LibraryName, cmdID, obj.motorID);
			val = double(typecase(uint8(val), 'single'));
		catch e
			throwAsCaller(e);
		end
	end
	
	%this will read a velocity value from the motor ID entered		
	function val = getVelocity(obj.motorID); %same for velocity
		cmdID = obj.GET_VELOCITY;
	
		try
			val = sendCommand(obj, obj.LibraryName, cmdID, obj.motorID);
			val = double(typecase(uint8(val), 'single'));
		catch e
			throwAsCaller(e);
		end
	end

	%this will read a current value from the motor ID entered		
	function val = getCurrent(obj.motorID); %same for motor current
		cmdID = obj.GET_CURRENT;
	
		try
			val = sendCommand(obj, obj.LibraryName, cmdID, obj.motorID);
			val = double(typecase(uint8(val), 'single'));
		catch e
			throwAsCaller(e);
		end
	end
end

methods (Access = protected)
    function displayScalarObject(obj)
        % Format for printing returned values from Dynamixel motors.
        
        header = getHeader(obj);
        disp(header);
        
        % Display main options
        fprintf('          Motor: %s\n', obj.motorID   );
        fprintf('\n');
        
        % Allow for the possibility of a footer.
        footer = getFooter(obj);
        if ~isempty(footer)
            disp(footer);
        end
    end
end

end