0001 function [ xo, t ] = ContinuousDynamics( xi, phi )
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016 global mH m a b l hx;
0017 global AMP PER;
0018 global qx0 env;
0019 global SWSTFlag;
0020 global qHist2;
0021
0022 g=9.8;
0023 dt=0.001;
0024 gamma=0.0;
0025
0026 q=xi(1:2)';
0027 q_d=xi(3:4)';
0028
0029
0030 qHist=[];
0031
0032
0033 for t=1:1000
0034
0035
0036 ha=Controller(t,q,q_d,phi);
0037
0038
0039
0040
0041
0042 H=[m*b*b, -m*l*b*cos(q(2)-q(1)); -m*l*b*cos(q(2)-q(1)), (mH+m)*l*l+m*a*a];
0043 B=[0, m*l*b*sin(q(2)-q(1))*q_d(2); -m*l*b*sin(q(2)-q(1))*q_d(1), 0];
0044 G=[m*b*g*sin(q(1)); -(mH*l+m*a+m*l)*g*sin(q(2))];
0045 S=[0 -1; 1 1];
0046 U=[0; ha];
0047
0048
0049 if(inv(H)==Inf)
0050 xo=[NaN NaN NaN NaN];
0051 break;
0052 end
0053 q_dd=inv(H)*(S*U-B*q_d-G);
0054
0055 q_new=q+q_d*dt;
0056 q_d_new=q_d+q_dd*dt;
0057
0058 q=q_new;
0059 q_d=q_d_new;
0060
0061 if SWSTFlag==0
0062 qHist2=[qHist2; q(1) q_d(1)];
0063 else
0064 qHist2=[qHist2; q(2) q_d(2)];
0065 end
0066
0067 qHist=[qHist; q' q_d' ha t];
0068
0069
0070
0071 qx1=qx0+[l*sin(-q(2)), l*cos(-q(2))]';
0072 qx2=qx1-[-l*sin(q(1)), l*cos(q(1))]';
0073
0074 if q(2)-q(1)<-0.001
0075 for xpos=1:env(1)
0076 if qx2(1)<env(2*xpos)
0077 xpos=xpos-1;
0078 break;
0079 end
0080 end
0081
0082 gamma=-atan((env(2*xpos+1)-qx0(2))/(env(2*xpos)-qx0(1)));
0083
0084 if q(1)>0 & (q(1)+q(2))<=-gamma*2
0085 qx0=[qx2(1) env(2*xpos+1)]';
0086 SWSTFlag=~SWSTFlag;
0087 t
0088 break;
0089 end
0090 end
0091
0092
0093 if mod(t,50)==0
0094
0095 figure(hx);
0096 subplot(3,1,1);
0097 cla;
0098 hold on;
0099
0100
0101 for i=1:500
0102 plot([env((i-1)*2+2) env(i*2+2)], [env((i-1)*2+3) env((i-1)*2+3)],'-k');
0103 end
0104
0105
0106 if SWSTFlag==0
0107 plot([qx0(1), qx1(1)],[qx0(2), qx1(2)],'-b');
0108 plot([qx1(1), qx2(1)],[qx1(2), qx2(2)],'-r');
0109 else
0110 plot([qx0(1), qx1(1)],[qx0(2), qx1(2)],'-r');
0111 plot([qx1(1), qx2(1)],[qx1(2), qx2(2)],'-b');
0112 end
0113 xlim([qx1(1)-1 qx1(1)+1.5]);
0114 ylim([qx1(2)-1.1 qx1(2)+0.2]);
0115
0116 x = qx1(1) + 1.25*[-1,1];
0117 x = 0:0.1:2*pi;
0118 fill(qx1(1) + 0.008*sin(x), qx1(2) + 0.008*cos(x), [0 1 0]);
0119
0120 drawnow();
0121 end
0122 end
0123
0124
0125 if t==1000
0126 xo=[NaN NaN NaN NaN];
0127 else
0128 xo=[q' q_d'];
0129 end
0130
0131
0132 figure(hx);
0133 subplot(3,1,2);
0134 hold on;
0135 plot([0 800],[0 0],'-k');
0136 plot([PER/2 PER/2],[-0.5 0.5],'-k');
0137 plot([qHist(end,6) qHist(end,6)],[-0.3 0.3],'-r');
0138 plot(qHist(:,6),qHist(:,1),'-k');
0139 plot(qHist(:,6),qHist(:,2),'-r');
0140 plot(qHist(:,6),qHist(:,5)/AMP,'-g');
0141
0142 drawnow;
0143
0144 figure(hx);
0145 subplot(3,1,3);
0146 hold on
0147
0148 plot(qHist2(:,1),qHist2(:,2));
0149
0150 drawnow;
0151
0152
0153
0154
0155
0156
0157
0158
0159
0160
0161
0162
0163