Matlab and WidowX robot arm communication and control

For a class, we needed to control the WidowX arm from Trossen Robotics from Matlab and get as real-time of a feedback as possible.

So I came up with a code for the Arbotix Robot-controller which is this

#include <ax12.h>
#include <BioloidController.h>

//constant global variables
const int led = 0;
const int sense = A0;

//global variables
int inputChoice;    //choice of action input
int inputID;        //servo ID input
int angleHigh;      //the angle is recieved in 2 bytes since it can go up to 4095. So it is saved into 2 variables then added
int angleLow;
int angle;          //angle read from the servos
int lowbyte;        //read the servo angle and divide it into 2 bytes before sending
int highbyte;
int garbage;        //since I only send 0-100 for servo 6, the high byte is useless to me but I need to get rid of it somewhere
int sensorValue;    //read the photo resistor value then save it here to be sent later

//the starting position and speed of the robot
int theta1=2048;
int theta2=1500;
int theta3=1500;
int theta4=1000;
int theta5=512;
int gripper=512;
int speed1=75;
int speed2=75;
int speed3=75;
int speed4=75;
int speed5=75;
int gripperSpeed=75;

//function prototype

void setup(void){
delay(500);
Serial.begin(115200);
delay(500);
pinMode(led, OUTPUT);

//wake up slowly
ax12SetRegister2(1,0x20,75);
ax12SetRegister2(2,0x20,75);
ax12SetRegister2(3,0x20,75);
ax12SetRegister2(4,0x20,75);
ax12SetRegister2(5,0x20,75);
ax12SetRegister2(6,0x20,75);
SetPosition(1,2048);
SetPosition(2,1500);
SetPosition(3,1500);
SetPosition(4,1000);
SetPosition(5,512);
SetPosition(6,512);

//indicate that the robot is ready
digitalWrite(led,HIGH);
}

void loop(void){
if (Serial.available()){

//if the choice was to set the servo angle
if (inputChoice == 10){
if (inputID == 1){
angleHigh=angleHigh<<8;
theta1=angleHigh+angleLow;
theta1=map(theta1,0,360,0,4095);
speed1=map(speed1,0,100,1,1023);
}
else if (inputID == 2){
angleHigh=angleHigh<<8;
theta2=angleHigh+angleLow;
theta2=map(theta2,0,360,0,4095);
speed2=map(speed2,0,100,1,1023);
}
else if (inputID == 3){
angleHigh=angleHigh<<8;
theta3=angleHigh+angleLow;
theta3=map(theta3,0,360,0,4095);
speed3=map(speed3,0,100,1,1023);
}
else if (inputID == 4){
angleHigh=angleHigh<<8;
theta4=angleHigh+angleLow;
theta4=map(theta4,0,360,0,4095);
speed4=map(speed4,0,100,1,1023);
}
else if (inputID == 5){
angleHigh=angleHigh<<8;
theta5=angleHigh+angleLow;
theta5=map(theta5,30,330,0,1023);
speed5=map(speed5,0,100,1,1023);
}
else if (inputID == 6){
gripper=map(gripper,0,100,0,512);
gripperSpeed=map(gripperSpeed,0,100,1,1023);
}

//set the speed
ax12SetRegister2(1,0x20,speed1);
ax12SetRegister2(2,0x20,speed2);
ax12SetRegister2(3,0x20,speed3);
ax12SetRegister2(4,0x20,speed4);
ax12SetRegister2(5,0x20,speed5);
ax12SetRegister2(6,0x20,gripperSpeed);

//set the position
SetPosition(1,theta1);
SetPosition(2,theta2);
SetPosition(3,theta3);
SetPosition(4,theta4);
SetPosition(5,theta5);
SetPosition(6,gripper);

}
//read the position of the servo
else if (inputChoice == 20){
angle = GetPosition(inputID);
lowbyte = angle & 0x00FF;
highbyte = angle >> 8;

Serial.write(highbyte);
Serial.write(lowbyte);
}
//relax the servo
else if (inputChoice == 30){
Relax(inputID);
}
else if (inputChoice == 40){
Serial.write(sensorValue);
}
}//end of if (Serial.available())
if (Serial.available()>2)
Serial.flush();
}//end of loop()

while (!Serial.available());
}
//end of program


So as you might be able to see, I receive each input in blocking communication mode and then put that straight into a variable then use it. This approach gave me some problems because if one byte was missed by the Arbotix, then the whole sequence is basically crap. So I had some pauses from my Matlab side here and there but that doesn't solve the issue.

Here is the Matlab functions:

function [angle]=getPosition(Arbotix,id)
%Inputs to this function are getPosition(SeraialObject,servo id)
%This function takes the serial object created such as
%Arbotix=serial('COM3','BaudRate',115200) and sends the servo id specified
%to the Arbotix-M robot controller that is connected with Matlab then
%retrieve the angle of the servo.

%clear the serial buffer
flushinput(Arbotix);

%write data
fwrite(Arbotix,20);     %send the action (20 is getPosition)
fwrite(Arbotix,id);     %send the servo id

%read data and concatenate the angle data recieved from the Arbotix-M
angle=bitshift(high,8);
angle=angle+low;

if id<=4
angle=round(angle*(360/4096),2);
elseif id==5
angle=round(angle*(300/1024),2);
else
angle=round(angle*(100/512),2);
end

%angle corrections
if id==1
angle=angle-180;
elseif id==2
angle=250-angle;
elseif id==3
angle=angle-250;
elseif id==4
angle=angle-270;
elseif id==5
angle=angle-150;
end

end


Another function:

function [sensorValue]=getLED(Arbotix)
%This function returns the value of the light based on the reading of the
%photoresistor on the end of the Widow-X Arm. The higher the value, the
%brighter the light

%clear the serial buffer
flushinput(Arbotix);

%write data
fwrite(Arbotix,40);     %send the action (40 is sensor value)

%get the sensor data from the Arbotix board
end


Another:

function []=Relax(Arbotix,id)
%Inputs to this function are Relax(SeraialObject,servo id)
%This function takes the serial object created such as
%Arbotix=serial('COM3','BaudRate',115200) and sends the servo id specified
%to the Arbotix-M robot controller that is connected with Matlab then
%relaxes the servo.

%write data
fwrite(Arbotix,30);     %send the action (30 is Relax)
fwrite(Arbotix,id);     %send the servo id

end


Another:

function []=setPosition(Arbotix,id,angle,speed)
%Inputs to this function are setPosition(SeraialObject,servo id,angle,speed)
%This function takes the serial object created such as
%Arbotix=serial('COM3','BaudRate',115200) and sends the servo id and the
%angle then the speed specified to the Arbotix-M robot controller that is
%connected with Matlab.

%angle corrections and single motor self damage protection
if id==1
if angle>=-180 && angle<=180
angle=angle+180;
else
error('Physical constraint 1');
end
elseif id==2
if angle>=-25 && angle<=165
angle=250-angle;
else
error('Physical constraint 2');
end
elseif id==3
if angle>=-160 && angle<=90
angle=angle+250;
else
error('Physical constraint 3');
end
elseif  id==4
if angle>=-200 && angle<=15
angle=angle+270;
else
error('Physical constraint 4');
end
elseif  id==5
if angle>=-130 && angle<=130
angle=angle+180;
else
error('Physical constraint 5');
end
elseif  id==6
if angle>100 && angle<0
error('Physical constraint 6');
end
end

%convert the angle to a 16 bit integer so that it is possible to separate
%into 2 bytes
angle=uint16(angle);

%separate the angle value into 2 bytes
high=bitshift(angle,-8);
low=bitshift(swapbytes(angle),-8);

%write data
fwrite(Arbotix,10);     %send the action (10 is setPosition)
fwrite(Arbotix,id);     %send the servo id
fwrite(Arbotix,high);   %send the angle high byte
fwrite(Arbotix,low);    %send the angle low byte
fwrite(Arbotix,speed);  %send the desired speed
end


If you are wondering about the angle conversions, that is because the dynamixel servos use angles between 0-512 or 0-4095 as there 30-330 degrees and 0-360 degrees respectively so I had to do conversions back and forth to get to the operator in easily understood numbers.

With these basic commands, they can be used together to make some pretty complex codes.

Anyone is welcome to use it and I could care less if I don't get credit.

If someone could please make this code run faster, better, smoother, with no interruptions, checks for errors and ignores if message is wrong (checksum), etc... this would be awesome.