MATLAB code used for my ENG-03 ARM Project

arm.m

% This code relies on the Arbotix class and the SerialLink class from the
% Robotics Toolbox, which can be found here:
% http://petercorke.com/Robotics_Toolbox.html

addpath(fullfile( fileparts(which('startup_rvc')), 'robot', 'interfaces'))

% This was using a direct USB to bioloid interface, so we run it at
% 1 Mbit/sec
arb = Arbotix('port', '/dev/ttyUSB0', 'baud', 1000000, 'nservos', 2);

a1 = 148;   % Length of link 1
a2 = 116;   % Length of link 2

p2 = SerialLink([
    Revolute('d', 0, 'a', a1, 'alpha', 0, 'standard')
    Revolute('d', 0, 'a', a2, 'alpha', 0, 'standard')
    ], ...
    'name', 'two link');
qz = [0 0];

p2.base = transl(200, 200, 0);
p2.plotopt = { 'workspace', [-100, 500, -100, 500, -100, 100], 'trail', '-'};

% Positions
%   200,  80
%   300,  20
%   380, 320
%    20, 380
%   380, 380

pt = [[200, 80]; [300, 20]; [380, 320]; [20, 380]; [380, 380]];

C = [];
for i=2:5
    x0 = pt(i-1, 1);
    y0 = pt(i-1, 2);
    x1 = pt(i, 1);
    y1 = pt(i, 2);
    dx = x1 - x0;
    dy = y1 - y0;
    
    dist = round(sqrt(dx^2 + dy^2));

    T0 = transl(x0, y0, 0);
    T1 = transl(x1, y1, 0);
    
    % ctraj does linear interpolation from T0 To T1 with a trapezoidal
    % velocity profile.

    C = cat(3, C, ctraj(T0, T1, dist));
end
Q = p2.ikine(C, [-2 2], [1 1 0 0 0 0]);

% To plot the path, use:
%
% XY = transl(C)            % Extract x, y, z coords
% plot(XY(:,1), XY(:,2))    % Shows the path (xy only)
% hold on
% scatter(pt(:,1), pt(:,2)) % Shows the points

Q = Q * -1  % Our servos are mounted inverted from the way arbotix expects it to be

disp('Moving to starting position. Press a key to continue')
arb.setpos(Q(1,:), [100 100])
pause

disp('And away we go')
for q=Q'
    arb.setpos(q)
    pause(0.01)
end