Dynamixel Velocity Control in Matlab

I'v created a ROS package to control the velocity of the dynamixel motors on Cayton Gamma 1500 robot arm.

The code and Simulink model  can be found in my repository, here with detailed description. 

I modified the official dynamixel_driver pkg so I can send velocity commands to multiple joints at the same time.

Matlab

Further more, I have an example of a PID controller implemented in Simulink, which controls the last two joints of the robot arm in real time.

Below is the block representation:

The subscriber outputs are the motor position, velocity, time stamp of the message, if it is moving and a step signal activated when a new message arrives, IsNew. 

This signal is used to synchronize the PID and the command publisher to the message arrival period. 

The controller has all the 3 gains present, so anyone can play with it, but notice that the I gain is 0,because we already have an integrator due to using position as input and outputting velocity command.

The publisher takes a double as input and converts it so it can be assigned to the message bus:

I ran in some problems to generate a MultiArray message by using the existing blocks, but there is a workaround.

Use a Matlab function block and paste the following code:

function y = assigneArray(u)

% generate a ros message whth flaot64 multi array type

arrayMSG = rosmessage(rostype.std_msgs_Float64MultiArray);

% assign input u to data field of the message

arrayMSG.Data = u;

y =arrayMSG.Data;