parallel1.m

% function [j_vars] = parallel1()
clear all
close all
clc
%% global variables
global l lengths radiusx radiusy ell_angle
global l1a l2a L angles phidot
%% trajectory information
d2r=pi/180;
radiusx=1; radiusy=3; ell_angle=30; % ellipse data
x=radiusx*cos(ell_angle*d2r); % initial starting x for platform center
y=radiusx*sin(ell_angle*d2r); % initial starting y for platform center
phi=30*d2r; phidot=0*d2r; % initial angle of platform and its change rate
%% system parameters
l=3; % length of side of center platform
angles=[pi/3 2*pi/3 0]; % angles of the base triangle in absolute frame
L(1)=5; % base triangle side1
L(2)=5; % base triangle side2
l1a=5; % link length for first rotating link
basepoint=[-L(1)/2 -L(1)/2*tan(pi/6)]; % the base point for the first prismatic joint
Sim_Angle=2*pi; % angle along ellipse
%% system calculation
bases(1,:)=basepoint;
bases(2,:)=bases(1,:)+[L(1)*cos(angles(1)) L(1)*sin(angles(1))];
bases(3,:)=bases(2,:)+[L(2)*cos(angles(2)) L(2)*sin(angles(2))];
angles(3)=atan2(bases(1,2)-bases(3,2),bases(1,1)-bases(3,1));
L(3)=sqrt((bases(1,1)-bases(3,1))^2+(bases(1,2)-bases(3,2))^2);
bases(4,:)=bases(1,:); % calculate the bases of the prismatic joints
l2a=l/2/cos(pi/6); % link lengths for second rotating link
lengths=[l1a l2a;l1a l2a;l1a l2a]; % only two link lengths for 2R
%% initialization for startup
j_vars=zeros(1,9); % 3*3=9 joints
options=odeset; % for use in ode45/ode4
feasible=1; % set feasibility crieteria default value
ends=myfunc(x,y,phi); % calculate the ends of the platform link
for i=1:3
    j_vars(1,i*3-2:i*3-1)=myrevkin1(bases(i:i+1,:),ends(1:2,i),lengths(i,1)); % inverse kinematics routine
    if(j_vars(1,i*3-1)==10) % if found infeasible
        j_vars(1,i*3)=10;
        feasible=0;
    else
        j_vars(1,i*3)=(i-1)*2*pi/3+pi/6+phi*d2r;
    end
end
%% Final ODE routines and simulation
if(feasible~=1)
    disp('no feasible soln in parallel');
    pause
else
%     [T Y]=ode45(@Mydiff,0:0.05:Sim_Angle,j_vars(1,:),options); % variable time step
    Y=ode4(@Mydiff,0:0.05:Sim_Angle,j_vars(1,:)); T=0:0.05:Sim_Angle; % fixed time step
    span=length(T);
    x=zeros(span,3);
    y=zeros(span,3);
%     aviobj = avifile('trial.avi','compression','Cinepak'); % Step 1: Declare an avi object
    for i=1:length(T)
        for i2=1:3
            x(i,i2)=bases(i2,1)+L(i2)*Y(i,i2*3-2)*cos(angles(i2))+lengths(i2,1)*cos(Y(i,i2*3-1))+lengths(i2,2)*cos(Y(i,i2*3));
            y(i,i2)=bases(i2,2)+L(i2)*Y(i,i2*3-2)*sin(angles(i2))+lengths(i2,1)*sin(Y(i,i2*3-1))+lengths(i2,2)*sin(Y(i,i2*3));
        end
        plot(x(1:i,2),y(1:i,2),'color','k','linewidth',2);
        hold on;
        plot(bases(:,1),bases(:,2),'o--k');
        parallelplot1(Y(i,:),bases,lengths);
%         frame= getframe(gcf);   %Step 2: Grab the frame
%         aviobj = addframe(aviobj,frame); % Step 3: Add frame to avi object
        hold off;
    end
%     aviobj = close(aviobj);  % Step 4: Close the avi object
end