%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % A solution to CSE For Your Homework Project 2 % % Robot Arm Control: Like a Pendulum Swings % % Dianne P. O'Leary and Yalin E. Sadguyu % % 12/02 and 01/03 % % % % File name: problem5.m % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global mydata theta10 mydata.m = 1; % m is the mass mydata.l = 1; % l is the length of the pendulum mydata.g = 9.81; % g is the acceleration of gravity mydata.c = 0.5; % c is the damping constant mydata.forcedflag = 1; % indicates applied force mydata.linearflag = 1; % indicates the linear model mydata.forceangle = pi/8; % u(t) = m*g*sin(forceangle) % when forcedflag == 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Problem 5. % % Solutions to the boundary value problem by % % the shooting method and by finite differences. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% h=0.01; % h is the time step for which the solution y(i*h) % will be determined where i is a positive integer. t=[0:h:10]; % t is a row vector denoting the time instants % at which the solutions will be determined. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % Solve the original initial value problem. % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% theta0 = pi/32; [t,y]=ode45('pendulum',t,[theta0,0]); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % Solve the boundary value problem. % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % Shooting method % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % Apply the shooting method to find the solutions to % % the damped, driven pendulum equation on time interval % % t = [0,10]. The idea behind the shooting method is to % % guess at the missing initial values, integrate the % % equation using your favorite initial value problem % % method and then use the results to improve the guess. % % Hence, a shooting method involves using a nonlinear % % equation solver with function evaluation through an % % initial value problem ODE solver. We'll use the % % function fzero as the nonlinear equation solver and % % ode45 as the ODE solver for the shooting method. % % We start with the initial guess d theta /dt (0) = 0.5 % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % We solve the problem with theta(0) = theta0 and % % theta(10) = whatever value our previous solution % % predicted. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% theta10 = y(end,1); t = [0:h:10]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Use fzero with knowledge of theta(0) and theta(10) to % % find the missing initial condition d theta(t)(0)/dt. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% thetaprime0 = fzero('shooting',1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Use the initial condition to reconstruct the solution % % to compare with the previous one. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [t,ys]=ode45('pendulum',t,[theta0 thetaprime0 ]); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % Finite Difference Method % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % Now use the finite difference method to solve this % % boundary value problem. % % Replace all first derivatives % % y'(t) by (y(t+h)-y(t-h))/(2*h) % % and second derivatives % % y''(t) by (y(t+h) -2*y(t)+y(t-h))/(h^2). % % Let h = ts /n for a large number n % % and define y_j = y(jh), j = 0,...,n. % % Then, the boundary conditions can be % % stated as y_0 = 0, y_{n+1} = thetaB. % % Transform the second order differential equation % % m*l*theta"(t)+c*x'(t)+m*g*theta(t) = u(t) % % to a system of n-1 linear equations with n-1 unknowns. % % Use LU factorization to solve the resulting linear % % system of equations. % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% h=0.01; n=10/h; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % The linear system is A (yd) = b where % % yd is the vector of approximate values of y and % % all elements of A are zero except % % the subdiagonal elements % % c1 = (m*l/h^2) - c/(2h) % % the main diagonal elements % % c2 = (-2*m*l/h^2) + m*g, % % and the superdiagonal elements % % c3 = (m*l/h^2) + c/(2h) % % and % % b(i) = u % % except that the first and last entries have the % % extra terms % % -c1 y(0) and % % -c3 y(n) respectively. % % y(i) is the approximation to theta(ih). % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% c1 = mydata.m*mydata.l/(h*h) - mydata.c/(2*h); c2 = -2*mydata.m*mydata.l/(h*h) + mydata.m*mydata.g; c3 = mydata.m*mydata.l/(h*h) + mydata.c/(2*h); A = toeplitz([c2,c1,zeros(1,n-3)], [c2,c3,zeros(1,n-3)] ); u = mydata.m * mydata.g * sin(mydata.forceangle); b = u * ones(n-1,1); b(1) = b(1) -c1*theta0; b(n-1) = b(n-1) -c3*theta10; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Use LU factorization to solve the % % linear system of equations. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% yd = A\b; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Insert boundary values theta(0) and theta(10) % % to form the solution vector. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% yd=[theta0; yd; theta10]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Plot the three solutions for comparison. % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(3) subplot(2,1,1) plot(t,y(:,1),t,ys(:,1),t,yd) xlabel('time (t)') ylabel('\theta (t)') legend('Original solution','Shooting solution', ... 'Finite difference solution') title('Solutions to the Boundary Value Problem') subplot(2,1,2) plot(t,y(:,1)-ys(:,1),t,y(:,1)-yd) xlabel('time (t)') ylabel('\theta (t)') legend('Original - Shooting', ... 'Original - Finite Difference')