Tr =

         0   37.1869

Error in Predictive_base_controller (line 99)

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 +

    (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw +

    (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) +

    (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(x4d - x(4,i) + h*x9d +

    (h^2*x10d)/2 + (sin(x(3,i))*(x(6,i)^2*sin(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) -

    l^2*mb^2*x(4,i)^2*sin(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) -

    (h*sin(x(3,i))*(x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) + g*l*mb*(2*M + mb +

    (2*J)/r^2) - l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) -

    (h^2*cos(x(3,i))*(x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2+ g*l*mb*(2*M + mb +

    (2*J)/r^2) - l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))

    + (h*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3,i)))*(x2d - x(2,i) + h*x7d + (h^2*x8d)/2 - (h*sin(x(3,i))*(l*mb*x(2,i)*x(6,i)

    + x(4,i)*x(6,i)*cos(x(3,i))*(2*mb*l^2 + 2*It1 - 2*It3)))/(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw +

    (2*J)/r^2))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (d*h*w6*(x(6,i) - x6d + h*x11d -

    (h*sin(x(3,i))*(l*mb*x(2,i)*x(6,i) + x(4,i)*x(6,i)*cos(x(3,i))*(2*mb*l^2 + 2*It1 - 2*It3)))/(It3 + 2*K +

    sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))))/(r*(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) +

    d^2*(2*mw + (2*J)/r^2))) + (l^2*mb^2*cos(x(3,i))*sin(x(3,i))^2*(2*x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2

    + It1 - It3) - l^2*mb^2) + 2*g*l*mb*(2*M + mb + (2*J)/r^2) - 2*l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(It2*mb + (mb*l^2 +

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2) - (Tr*h*w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw +

    (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb +

    2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) +

    (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(mb + 2*mw + (2*J)/r^2

    + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (Tr*d^2*h^2*w6*(It3 +

    2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))^2)/r^2 +

    (Tr*h^2*l*mb*w4*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb

    + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 +

    (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) +

    (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 +

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) +

    (Tr*h^2*l^2*mb^2*w4*cos(x(3,i))*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 +

    (l*mb*cos(x(3,i)))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2

    + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) +

    (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(It2*mb + (mb*l^2 +

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2)/(w7 + (d^2*h^2*w6)/(r^2*((2*mw + (2*J)/r^2)*d^2 + (mb*l^2 + It1 -

    It3)*sin(x(3,i))^2 + It3 + 2*K)^2) + (h^2*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3,i)))*((mb*l^2 + It2)/r -

    l*mb*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 -

    (h*w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw

    + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 +

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) +

    l^2*mb^2*sin(x(3,i))^2))/r)*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2)

    + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*w4*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 +

    (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw +

    (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) +

    (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 +

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) +

    (h^2*l^2*mb^2*w4*cos(x(3,i))*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 +

    (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw +

    (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) +

    (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 +

    It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2)))))

 

>> 

clear all

clc

h=0.001;

t=0:h:10;

mb=45;

mw=2;

l=0.135;

r=0.2032;

d=0.6;

g=9.81;

M=mb+mw;

It1=1.9;

It2=2.1;

It3=1.6;

J=0.02;

K=0.04;

% x(1,1)=0;

% v=0.01;

% teta=0.001*pi/2;

% teta_dot=0.001*pi/8;

% sy=0.001*pi/5;

% sy_dot=0.001*pi/9;

x(1,1)=8;

v=1;

teta=pi/2;

teta_dot=0.001*pi/8;

sy=pi/5;

sy_dot=0.001*pi/9;

x=[x(1,1);v;teta;teta_dot;sy;sy_dot;];

y(:,1)=[x(1,1);x(2,1);x(3,1);];

Tr(1)=0;

Tl(1)=0;

eta1(1)=0.001;

eta2(1)=0.001;

%h2=0.01;

for i=1:length(t)

   

   %%%%% non_linear system with predictive-base controller %%%%%

   

   eta1(i)=(mb^2)*(l^2)*sin(x(3,i))^2+mb*It2+2*(mw+(J/(r^2)))*(It2+mb*(l^2));

   eta2(i)=It3+2*K+2*(mw+((J)/(r^2)))*(d^2)+(It1+mb*(l^2)-It3)*(sin(x(3,i))^2);

   

   a2(i)=(1/eta1(i))*(sin(x(3,i))/x(3,i))*[(-mb^2)*(l^2)*g*cos(x(3,i))+(It2+mb*(l^2))*mb*l*(x(4,i)^2)+mb*l*(It2+mb*(l^2)+(It3-It1-mb*(l^2))*cos(x(3,i)))*(x(6,i)^2)];

   a4(i)=(1/eta1(i))*(sin(x(3,i))/x(3,i))*[(mb+2*M+((2*J)/(r^2)))*(mb*l*g)-(mb^2)*(l^2)*(x(4,i)^2)*cos(x(3,i))-((mb^2)*(l^2)+(It3-It1-mb*(l^2))*(mb+2*mw+((2*J)/(r^2))))*(x(6,i)^2)*cos(x(3,i))];

   a6(i)=(1/eta2(i))*(sin(x(3,i))/x(3,i))*[2*(It3-It1-mb*(l^2))*x(4,i)*x(6,i)*cos(x(3,i))-mb*l*x(2,i)*x(6,i)];

   b2(i)=((It2+mb*(l^2))/r+mb*l*cos(x(3,i)))/eta1(i);

   b4(i)=-((mb*l*cos(x(3,i)))/r+mb+2*mw+((2*J)/(r^2)))/eta1(i);

   b6(i)=-(d/r)/eta2(i);

   

  z(:,i)=0;

    if 2<h<3

    z(:,i)=0.01;

   end

   

   x(:,i+1)=[h*x(2,i)+x(1,i);

       h*(a2(i)*x(3,i)+b2(i)*Tr(i)+b2(i)*Tl(i))+x(2,i)+z(:,i);

       h*x(4,i)+x(3,i);

       h*(a4(i)*x(3,i)+b4(i)*Tr(i)+b4(i)*Tl(i))+x(4,i)+z(:,i);

       h*x(6,i)+x(5,i);

       h*(a6(i)*x(3,i)-b6(i)*Tr(i)+b6(i)*Tl(i))+x(6,i)+z(:,i);];

   

   

   y(:,i+1)=[x(2,i+1);x(4,i+1);x(6,i+1)];

   

   

   w2=10000;

   w4=10000;

   w6=10000;

   w7=10000;

   w8=10000;

   

   x2d=0;

   x4d=0;

   x6d=0;

   x7d=0;

   x8d=0;

   x9d=0;

   x10d=0;

   x11d=0;

   x12d=0;

   

   Tr(i+1)=-((Tl*h^2*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3+i)))*((mb*l^2 + It2)/r - l*mb*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(x4d - x(4,i) + h*x9d + (h^2*x10d)/2 + (sin(x(3,i))*(x(6,i)^2*sin(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) - l^2*mb^2*x(4,i)^2*sin(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - (h*sin(x(3,i))*(x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) + g*l*mb*(2*M + mb + (2*J)/r^2) - l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - (h^2*cos(x(3,i))*(x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2+ g*l*mb*(2*M + mb + (2*J)/r^2) - l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) + (h*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3,i)))*(x2d - x(2,i) + h*x7d + (h^2*x8d)/2 - (h*sin(x(3,i))*(l*mb*x(2,i)*x(6,i) + x(4,i)*x(6,i)*cos(x(3,i))*(2*mb*l^2 + 2*It1 - 2*It3)))/(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (d*h*w6*(x(6,i) - x6d + h*x11d - (h*sin(x(3,i))*(l*mb*x(2,i)*x(6,i) + x(4,i)*x(6,i)*cos(x(3,i))*(2*mb*l^2 + 2*It1 - 2*It3)))/(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))))/(r*(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))) + (l^2*mb^2*cos(x(3,i))*sin(x(3,i))^2*(2*x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) + 2*g*l*mb*(2*M + mb + (2*J)/r^2) - 2*l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2) - (Tl*h*w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (Tl*d^2*h^2*w6*(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))^2)/r^2 + (Tl*h^2*l*mb*w4*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) + (Tl*h^2*l^2*mb^2*w4*cos(x(3,i))*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2)/(w7 + (d^2*h^2*w6)/(r^2*((2*mw + (2*J)/r^2)*d^2 + (mb*l^2 + It1 - It3)*sin(x(3,i))^2 + It3 + 2*K)^2) + (h^2*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3,i)))*((mb*l^2 + It2)/r - l*mb*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*w4*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) + (h^2*l^2*mb^2*w4*cos(x(3,i))*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2)))))

 

   Tl(i+1)=-((Tr*h^2*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3+i)))*((mb*l^2 + It2)/r - l*mb*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(x4d - x(4,i) + h*x9d + (h^2*x10d)/2 + (sin(x(3,i))*(x(6,i)^2*sin(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) - l^2*mb^2*x(4,i)^2*sin(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - (h*sin(x(3,i))*(x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) + g*l*mb*(2*M + mb + (2*J)/r^2) - l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) - (h^2*cos(x(3,i))*(x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2+ g*l*mb*(2*M + mb + (2*J)/r^2) - l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) + (h*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3,i)))*(x2d - x(2,i) + h*x7d + (h^2*x8d)/2 - (h*sin(x(3,i))*(l*mb*x(2,i)*x(6,i) + x(4,i)*x(6,i)*cos(x(3,i))*(2*mb*l^2 + 2*It1 - 2*It3)))/(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (d*h*w6*(x(6,i) - x6d + h*x11d - (h*sin(x(3,i))*(l*mb*x(2,i)*x(6,i) + x(4,i)*x(6,i)*cos(x(3,i))*(2*mb*l^2 + 2*It1 - 2*It3)))/(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))))/(r*(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))) + (l^2*mb^2*cos(x(3,i))*sin(x(3,i))^2*(2*x(6,i)^2*cos(x(3,i))*((mb + 2*mw + (2*J)/r^2)*(mb*l^2 + It1 - It3) - l^2*mb^2) + 2*g*l*mb*(2*M + mb + (2*J)/r^2) - 2*l^2*mb^2*x(4,i)^2*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2) - (Tr*h*w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (Tr*d^2*h^2*w6*(It3 + 2*K + sin(x(3,i))^2*(mb*l^2 + It1 - It3) + d^2*(2*mw + (2*J)/r^2))^2)/r^2 + (Tr*h^2*l*mb*w4*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) + (Tr*h^2*l^2*mb^2*w4*cos(x(3,i))*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2)/(w7 + (d^2*h^2*w6)/(r^2*((2*mw + (2*J)/r^2)*d^2 + (mb*l^2 + It1 - It3)*sin(x(3,i))^2 + It3 + 2*K)^2) + (h^2*w2*((mb*l^2 + It2)/r + l*mb*cos(x(3,i)))*((mb*l^2 + It2)/r - l*mb*cos(x(3,i))))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*w4*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r)*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*w4*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)) + (h^2*l^2*mb^2*w4*cos(x(3,i))*sin(x(3,i))*((l^2*mb^2*cos(x(3,i))*sin(x(3,i))*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2 - (h*(mb + 2*mw + (2*J)/r^2 + (l*mb*cos(x(3,i)))/r))/(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2) + (h^2*l*mb*sin(x(3,i))*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2))/r))/(2*(It2*mb + (mb*l^2 + It2)*(2*mw + (2*J)/r^2) + l^2*mb^2*sin(x(3,i))^2)^2)))))

   

       

     

     

     

     

end

figure(1)

plot(t,x(1,2:end))

xlabel('Time (sec)')

ylabel('Displacement (m)')

figure(2)

plot(t,x(2,2:end))

xlabel('Time (sec)')

ylabel('Speed (m/s)')

figure(3)

plot(t,x(3,2:end))

xlabel('Time (sec)')

ylabel('Teta (rad)')

figure(4)

plot(t,x(4,2:end))

xlabel('Time (sec)')

ylabel('Teta_dot (rad/s)')

figure(5)

plot(t,x(5,2:end))

xlabel('Time (sec)')

ylabel('Sy (rad)')

figure(6)

plot(t,x(6,2:end))

xlabel('Time (sec)')

ylabel('Sy_dot (rad/s)')

figure(10)

plot(t,Tr(2:end))

xlabel('Time (sec)')

ylabel('Right Wheel Torque (N.m)')

figure(11)

plot(t,Tl(2:end))

xlabel('Time (sec)')

ylabel('Left Wheel Torque (N.m)')