API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Oct 18th, 2019
141
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 6.47 KB | None | 0 0
raw download clone embed print report
  1. clear all;
  2. close all;
  3.  
  4. %% Record the video
  5. VIDEO = 1;
  6.  
  7. %% Initial conditions
  8. theta = deg2rad(20); % ramp angle (rad)
  9. R = 0.1; % disk radius (m)
  10. g = -9.81; % acceleration from gravity (m/s^2)
  11. m = 1; % disk mass (kg)
  12.  
  13. thickness = 0.01;
  14.  
  15. x_init = 0;
  16. y_init = 0;
  17. %z_init = R;
  18. a_init = 0;
  19. b_init = deg2rad(90);
  20. p_init = 0;
  21.  
  22. xd_init = 0;
  23. yd_init = 0;
  24. zd_init = 0;
  25. ad_init = 0;
  26. bd_init = 0;
  27. pd_init = 0;
  28.  
  29. z_init = y_init * sin(theta) + R;
  30.  
  31. X_init = [x_init; y_init; z_init; a_init; b_init; p_init; xd_init; yd_init; zd_init; ad_init; bd_init; pd_init];
  32.  
  33. % Start and finish times
  34. tspan = [0 10];
  35. options = odeset('RelTol',1e-7,'AbsTol',1e-7');
  36.  
  37. % Solve the EOMs
  38. sol = ode45(@diskEOM, tspan, X_init, R, theta, g, m, options);
  39.  
  40. % Evaluate the solution
  41. dt = 0.03;
  42. t = tspan(1):dt:tspan(2);
  43. X = deval(sol, t);
  44.  
  45. % %% SETUP VIDEO IF REQUIRED
  46. % if VIDEO
  47. % fps = 1/dt;
  48. % MyVideo = VideoWriter('gyro_animation','MPEG-4');
  49. % MyVideo.FrameRate = fps;
  50. % open(MyVideo);
  51. % end
  52. %
  53. %
  54. % %% PLOT THE STATES
  55. % plot(t,X)
  56. % xlabel('time')
  57. % ylabel('states')
  58. % h = legend('$x$', '$y$', '$z$', '$\alpha$', '$\beta$', '$\phi$',...
  59. % '$\dot{x}$', '$\dot{y}$', '$\dot{z}$',...
  60. % '$\dot{\alpha}$', '$\dot{\beta}$', '$\dot{\phi}$');
  61. % set(h,'Interpreter','latex')
  62. %
  63. %
  64. %
  65. % %% Draw Disk
  66. %
  67. % [xcyl, ycyl, zcyl] = cylinder(R);
  68. % zcyl = thickness*zcyl - thickness/2;
  69. %
  70. % r_OC = [xcyl; ycyl; zcyl];
  71. % r_OC = r_OC + [1; 1; 0; 0; 0; 0];
  72. %
  73. %
  74. % %% Create Animation
  75. % handle = figure;
  76. % hold on;
  77. %
  78. % for i = 1:length(t)
  79. % cla
  80. %
  81. % x = X(1, i);
  82. % y = X(2, i);
  83. % z = X(3, i);
  84. % a = X(4, i);
  85. % b = X(5, i);
  86. % p = X(6, i);
  87. %
  88. % R10 = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)]; % Pos x rotation (slope)
  89. % R21 = [cos(a) -sin(a) 0; sin(a) cos(a) 0; 0 0 1]; % Pos z rotation (yaw)
  90. % R32 = [cos(b) 0 sin(b); 0 1 0; -sin(b) 0 cos(b)]; % Pos y rotation
  91. % R43 = [cos(p) -sin(p) 0; sin(p) cos(p) 0; 0 0 1]; % Pos z rotation
  92. %
  93. % % Rotate disk
  94. % [xcyl_r, ycyl_r, zcyl_r] = rotation(xcyl, ycyl, zcyl, R10*R21*R32*R43);
  95. %
  96. % % Disk movement
  97. % xcyl_r = xcyl_r + x;
  98. % ycyl_r = ycyl_r + y;
  99. % zcyl_r = zcyl_r + z;
  100. %
  101. % % For surf method (unused)
  102. % % xcyl_top = [xcyl(2,:); zeros(1, length(xcyl))];
  103. % % ycyl_top = [ycyl(2,:); zeros(1, length(ycyl))];
  104. % % zcyl_top = [zcyl(2,:); zcyl(2,:)];
  105. % %
  106. % % xcyl_bot = [xcyl(1,:); zeros(1, length(xcyl))];
  107. % % ycyl_bot = [ycyl(1,:); zeros(1, length(ycyl))];
  108. % % zcyl_bot = [zcyl(1,:); zcyl(1,:)];
  109. %
  110. % surf(xcyl_r, ycyl_r, zcyl_r, 'edgeColor', 'none', 'faceColor', 'black')
  111. % hold on
  112. % patch(xcyl_r(1,:), ycyl_r(1,:), zcyl_r(1,:), 4*zcyl_r(1,:));
  113. % patch(xcyl_r(2,:), ycyl_r(2,:), zcyl_r(2,:), 4*zcyl_r(2,:));
  114. %
  115. % % Surf method (unused)
  116. % %surf(xcyl_top, ycyl_top, zcyl_top, 'edgeColor', 'none')
  117. % %surf(xcyl_bot, ycyl_bot, zcyl_bot, 'edgeColor', 'none')
  118. %
  119. % view(3)
  120. % axis(1*[-1 1 -1 1 -1 1])
  121. %
  122. % % Plot plane
  123. % [xgrid, ygrid] = meshgrid(-1:0.05:1, -1:0.05:1);
  124. % zgrid = ygrid*sin(-theta);
  125. % surf(xgrid, ygrid, zgrid, 'edgeColor', 'none', 'faceColor', 'blue');
  126. %
  127. % xlabel('X (Meters)')
  128. % ylabel('Y (Meters)')
  129. % zlabel('Z (Meters)')
  130. % if VIDEO
  131. % writeVideo(MyVideo,getframe(handle));
  132. % else
  133. % pause(dt);
  134. % end
  135. % end
  136. %
  137. %
  138. % %% Video setup
  139. % if VIDEO
  140. % fps = 1/dt;
  141. % MyVideo = VideoWriter('disk_animation','MPEG-4');
  142. % MyVideo.FrameRate = fps;
  143. % open(MyVideo);
  144. % end
  145. %
  146. %
  147. % %%
  148. % function [Xf,Yf,Zf] = rotation(Xi,Yi,Zi,R)
  149. %
  150. % I=size(Xi,1);
  151. % J=size(Xi,2);
  152. %
  153. % Xf=zeros(I,J);
  154. % Yf=zeros(I,J);
  155. % Zf=zeros(I,J);
  156. %
  157. % for ii=1:I
  158. % for jj=1:J
  159. % vector=[Xi(ii,jj);Yi(ii,jj);Zi(ii,jj)];
  160. % vector=R*vector;
  161. % Xf(ii,jj)=vector(1);
  162. % Yf(ii,jj)=vector(2);
  163. % Zf(ii,jj)=vector(3);
  164. % end
  165. % end
  166. %
  167. % end
  168. %
  169.  
  170. %% Solve the EOMs
  171. function X_d = diskEOM(t, X, R, theta, g, m)
  172.  
  173. % x, y, z, alpha, beta and phi and their derivatives are parsed from the
  174. % state vector X
  175.  
  176. x1 = X(1); x2 = X(2); x3 = X(3); x4 = X(4); x5 = X(5); x6 = X(6);
  177. x7 = X(7); x8 = X(8); x9 = X(9); x10 = X(10); x11 = X(11); x12 = X(12);
  178.  
  179. % Assign derivatives in the state vector
  180. x1_d = x7; x2_d = x8; x3_d = x9; x4_d = x10; x5_d = x11; x6_d = x12;
  181.  
  182. % Add the equations from Y for the double derivative terms
  183. x7_d = R*x11^2*cos(x4)*cos(x5) - (2*g*cos(x4)*sin(theta)*sin(x4))/15 - (R*x10*x12*cos(x4))/5 + R*x10^2*cos(x4)*cos(x5)^3 - (4*g*cos(theta)*cos(x4)*cos(x5)*sin(x5))/5 - (R*x10*x11*sin(x4)*sin(x5))/3 + (4*g*cos(x4)*cos(x5)^2*sin(theta)*sin(x4))/5 + (6*R*x10*x12*cos(x4)*cos(x5)^2)/5;
  184. x8_d = R*x11^2*sin(theta)*sin(x5) - (2*g*sin(2*theta))/5 + (4*g*cos(x5)*sin(x4)*sin(x5))/5 + (2*g*cos(theta)*cos(x4)^2*sin(theta))/15 + (8*g*cos(theta)*cos(x5)^2*sin(theta))/5 - R*x10^2*sin(theta)*sin(x5)*(sin(x5)^2 - 1) + R*x11^2*cos(theta)*cos(x5)*sin(x4) - (R*x10*x12*cos(theta)*sin(x4))/5 + R*x10^2*cos(theta)*cos(x5)^3*sin(x4) - (8*g*cos(theta)^2*cos(x5)*sin(x4)*sin(x5))/5 - (4*g*cos(theta)*cos(x4)^2*cos(x5)^2*sin(theta))/5 + (R*x10*x11*cos(theta)*cos(x4)*sin(x5))/3 + (6*R*x10*x12*cos(x5)*sin(theta)*sin(x5))/5 + (6*R*x10*x12*cos(theta)*cos(x5)^2*sin(x4))/5;
  185. x9_d = (2*g*cos(x4)^2)/15 - (4*g)/5 + (4*g*cos(x5)^2)/5 + (4*g*cos(theta)^2)/5 - (2*g*cos(theta)^2*cos(x4)^2)/15 - (8*g*cos(theta)^2*cos(x5)^2)/5 - (4*g*cos(x4)^2*cos(x5)^2)/5 - R*x11^2*cos(theta)*sin(x5) + (4*g*cos(theta)^2*cos(x4)^2*cos(x5)^2)/5 + R*x11^2*cos(x5)*sin(theta)*sin(x4) - (R*x10*x12*sin(theta)*sin(x4))/5 - R*x10^2*cos(theta)*cos(x5)^2*sin(x5) + R*x10^2*cos(x5)^3*sin(theta)*sin(x4) - (8*g*cos(theta)*cos(x5)*sin(theta)*sin(x4)*sin(x5))/5 - (6*R*x10*x12*cos(theta)*cos(x5)*sin(x5))/5 + (R*x10*x11*cos(x4)*sin(theta)*sin(x5))/3 + (6*R*x10*x12*cos(x5)^2*sin(theta)*sin(x4))/5;
  186. x10_d = (2*x11*x12)/sin(x5);
  187. x11_d = -(5*R*cos(x5)*sin(x5)*x10^2 + 6*R*x12*sin(x5)*x10 + 4*g*cos(theta)*cos(x5) + 4*g*sin(theta)*sin(x4)*sin(x5))/(5*R);
  188. x12_d = (2*g*cos(x4)*cos(x5)^2*sin(theta) - 5*R*x10*x11*sin(x5) - 2*g*cos(x4)*sin(theta) + 6*R*x11*x12*cos(x5)*sin(x5) + 5*R*x10*x11*cos(x5)^2*sin(x5))/(3*R*(cos(x5)^2 - 1));
  189.  
  190. X_d = [x1_d; x2_d; x3_d; x4_d; x5_d; x6_d; x7_d; x8_d; x9_d; x10_d; x11_d; x12_d];
  191.  
  192.  
  193. end
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!