انجمن سایت کلیدستان



لطفا راهنمایی کنید.عجله اییزمان کنونی: ۱۳۹۸/۰۹/۲۵، ۰۶:۵۹ ق.ظ
کاربران در حال بازدید این موضوع: 1 مهمان
نویسنده: babak313araz@gmail.com
آخرین ارسال: babak313araz@gmail.com
پاسخ 2
بازدید 1384

رتبه موضوع:
  • 0 رای - 0 میانگین
  • 1
  • 2
  • 3
  • 4
  • 5
لطفا راهنمایی کنید.عجله ایی

لطفا راهنمایی کنید.عجله ایی

#1
Tr =

         0   37.1869

In an assignment  A(Smile = B, the number of elements in A and B must be the same.

Error in Predictive_base_controller (line 99)
    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)))))
 
>> 
پاسخ

لطفا راهنمایی کنید.عجله ایی

#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)')
پاسخ


پرش به انجمن:


کاربران در حال بازدید این موضوع: 1 مهمان