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;
}
}
}
};