API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 24th, 2014
134
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 29.27 KB | None | 0 0
raw download clone embed print report
  1. %PROJECT 3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2. %ROBOTICS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  3. %BRANDON JACQUES.32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  4. %David Ritz.32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  5.  
  6. %basic settup garbage
  7. clear;
  8. clc;
  9. min = [-5200 -400 0 -1200 -2400];
  10. max = [5200 4800 3600 1200 2400];
  11. curAn = [0 -400 3600 500 100];%HOME -> curAn = [0 3600 0 0 -1100];
  12. Home = [0 3600 0 0 -1100];
  13. Bi = 220;
  14. F = 160;
  15. W = 50;
  16. Cl = 75;
  17. p1a = [0; 0; 220; 1;];
  18. p2b = [0; 0; 160; 1;];
  19. p3c = [0; 0; 50; 1;];
  20. exit = 0;
  21.  
  22. %Setting up figure
  23. Fig = gca;
  24. Fig.FontSize = 12;
  25. Fig.TickDir = 'out';
  26. Fig.TickLength = [.02, .02];
  27. Fig.XLimMode = 'manual';
  28. Fig.YLimMode = 'manual';
  29. Fig.YLim = [-100, 800];
  30. Fig.XLim = [-100, 800];
  31. %Calculating angles for sine/cosine inputs
  32. T1 = 90 - (curAn(1) * .025);
  33. T2 = (curAn(2)) * .025;
  34. T3 = (curAn(3)) * .025;
  35. T4 = (curAn(4)) * .075;
  36. T5 = (curAn(5)) * .075;
  37. %Distance between clamps from side view
  38. L=75;
  39. OP = sind(T5)*L;
  40. %Marking points
  41. p3c1 = [-1*(OP/2); 0; 50; 1;];
  42. p3c2 = [(OP/2); 0; 50; 1;];
  43. p3c1L = [-1*(OP/2); 0; 50+75; 1;];
  44. p3c2L = [(OP/2); 0; 50+75; 1;];
  45. %Transfer functions
  46. tran01 = [[cosd(T2) 0 sind(T2) 0];
  47. [0 1 0 0];
  48. [(-1*sind(T2)) 0 cosd(T2) 250];
  49. [0 0 0 1];];
  50. tran12 = [[cosd(T3) 0 sind(T3) 0];
  51. [0 1 0 0];
  52. [(-1*sind(T3)) 0 cosd(T3) 220];
  53. [0 0 0 1];];
  54. tran23 = [[cosd(T4) 0 sind(T4) 0];
  55. [0 1 0 0];
  56. [(-1*sind(T4)) 0 cosd(T4) 160];
  57. [0 0 0 1];];
  58. tran0A = [[cosd(T1) sind(T1) 0 0];
  59. [(-1*sind(T1)) cosd(T1) 0 0];
  60. [0 0 1 0];
  61. [0 0 0 1];];
  62. %setting up Tt_ in order to calculate the top view better. Tt_ is an angle
  63. if abs(T2) > 90
  64. Tt2 = 90 - mod(T2,90);
  65. else
  66. Tt2 = T2;
  67. end
  68. if abs(T2+T3) > 90
  69. Tt3 = 90 - mod(T2+T3,90);
  70. else
  71. Tt3 = T2+T3;
  72. end
  73. if abs(T2+T3+T4) > 90
  74. Tt4 = 90 - mod(T2+T3+T4,90);
  75. else
  76. Tt4 = T2+T3+T4;
  77. end
  78. Tt5 = T5;
  79. %Lengths of each segment from top view
  80. Bt = sind(Tt2) * Bi;
  81. Ft = sind(Tt3) * F;
  82. Wt = sind(Tt4) * W;
  83. Ct = sind(Tt4) * Cl;
  84. AP = cosd(Tt5) * L;
  85. %points in top view
  86. PAL = [0;Bt+Ft+Wt; 0;1;];
  87. PA1L=[0; Bt; 0; 1;];
  88. PA2L=[0; Ft; 0; 1;];
  89. PA3L=[0; Wt; 0; 1;];
  90. %Claw points in side view
  91. PAc1 = [-1*AP/2; Bt+Ft+Wt; 0; 1;];
  92. PAc2 = [AP/2; Bt+Ft+Wt; 0; 1;];
  93. PAc1L = [-1*AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  94. PAc2L = [AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  95. %Points in side view of all the arms in the 0 frame
  96. P0a = tran01*p1a;
  97. P0b = tran01*tran12*p2b;
  98. P0c = tran01*tran12*tran23*p3c;
  99. P0c1 = tran01*tran12*tran23*p3c1;
  100. P0c2 = tran01*tran12*tran23*p3c2;
  101. P0c1L = tran01*tran12*tran23*p3c1L;
  102. P0c2L = tran01*tran12*tran23*p3c2L;
  103. %top view points in 0 frame
  104. P0AL = tran0A*PAL;
  105. P0A1L = tran0A*PA1L;
  106. P0A2L = tran0A*PA2L;
  107. P0A3L = tran0A*PA3L;
  108. P0Ac1 = tran0A*PAc1;
  109. P0Ac2 = tran0A*PAc2;
  110. P0Ac1L = tran0A*PAc1L;
  111. P0Ac2L = tran0A*PAc2L;
  112. hold on
  113. %get rid of all previously drawn lines
  114. lines = get(gca, 'Children');
  115. delete(lines);
  116. %draw axis
  117. line([0 0],[0 75],'LineWidth',2.75);
  118. line([0 75],[0 0],'LineWidth',2.75);
  119. line([400 400],[0 75],'LineWidth',2.75);
  120. line([400 475],[0 0],'LineWidth',2.75);
  121. %draw side view
  122. line([0 0],[10 250]);
  123. line([0 P0a(1)],[250 P0a(3)]);
  124. line([P0a(1) P0b(1)],[P0a(3) P0b(3)]);
  125. line([P0b(1) P0c(1)],[P0b(3) P0c(3)]);
  126. line([P0c1(1) P0c2(1)],[P0c1(3) P0c2(3)]);
  127. line([P0c1(1) P0c1L(1)],[P0c1(3) P0c1L(3)]);
  128. line([P0c2(1) P0c2L(1)],[P0c2(3) P0c2L(3)]);
  129. %draw top view
  130. line([400 P0A1L(1)+400],[0 P0A1L(2)]);
  131. line([400+P0A1L(1) P0A2L(1)+P0A1L(1)+400],[P0A1L(2) P0A2L(2)+P0A1L(2)]);
  132. line([P0A2L(1)+P0A1L(1)+400 P0A3L(1)+P0A2L(1)+P0A1L(1)+400],[P0A2L(2)+P0A1L(2) P0A3L(2)+P0A2L(2)+P0A1L(2)]);
  133. line([P0Ac1(1)+400 P0Ac2(1)+400],[P0Ac1(2) P0Ac2(2)]);
  134. line([P0Ac1(1)+400 P0Ac1L(1)+400],[P0Ac1(2) P0Ac1L(2)]);
  135. line([P0Ac2(1)+400 P0Ac2L(1)+400],[P0Ac2(2) P0Ac2L(2)]);
  136. %name axis
  137. text(-30,70,'Z');
  138. text(70,-30,'X');
  139. text(370,70,'Y');
  140. text(470,-30,'X');
  141. hold off
  142. %main loop
  143. while exit == 0
  144. %grab all the input angles. it comes out as a vector if you input it
  145. %into the box correctly
  146. samp = inputdlg('Input spaceseperated numbers(waist shoulder elbow wrist1 wrist2) then ; at the end of every position. Type "home" to move to home position OR type spin for spin function','Sampled', [1 50]);
  147. s2='spin'; % might not need this
  148. tf2 = strncmpi(samp,'spin',4);
  149. if tf2 == 1
  150.  
  151. % Initalize circle values
  152. Xs = 140;
  153. Ys = 0; %does not change
  154. Zs = 240;
  155. Rs = 60;
  156. for n=1:12
  157.  
  158. %omega represents the current angle of the circle
  159. for omega = 0:360
  160. %%Calculate our X/Y/Z corridnates in mm
  161. X = Xs + Rs*cosd(omega);
  162. Y = Ys + Rs*sind(omega);
  163. Z = Zs; %Z remains constant
  164. %Calculate our theta values in degrees
  165. theta1=atand(X/Y);
  166. %dist is used to find thetas 2 and 3
  167. Dist=sqrt((X.^2)+ ((Z+50).^2));
  168. theta2=acosd((Z+50)/Dist)+acosd(((-22800)-(Dist.^2))/(-2*220*Dist));
  169. theta3=acosd((Dist.^2-160.^2-220.^2)/(-2*160*220));
  170. theta4=180-theta2-theta3;
  171. T1 = theta1;
  172. T2 = theta2;
  173. T3 = theta3;
  174. T4 = theta4;
  175. T5 = 90;
  176. %Draw the robot
  177. %It Goes Here
  178. Fig = gca;
  179. Fig.FontSize = 12;
  180. Fig.TickDir = 'out';
  181. Fig.TickLength = [.02, .02];
  182. Fig.XLimMode = 'manual';
  183. Fig.YLimMode = 'manual';
  184. Fig.YLim = [-100, 800];
  185. Fig.XLim = [-100, 800];
  186. OP = sind(T5)*L;
  187. %Marking points
  188. p3c1 = [-1*(OP/2); 0; 50; 1;];
  189. p3c2 = [(OP/2); 0; 50; 1;];
  190. p3c1L = [-1*(OP/2); 0; 50+75; 1;];
  191. p3c2L = [(OP/2); 0; 50+75; 1;];
  192. %Transfer functions
  193. tran01 = [[cosd(T2) 0 sind(T2) 0];
  194. [0 1 0 0];
  195. [(-1*sind(T2)) 0 cosd(T2) 250];
  196. [0 0 0 1];];
  197. tran12 = [[cosd(T3) 0 sind(T3) 0];
  198. [0 1 0 0];
  199. [(-1*sind(T3)) 0 cosd(T3) 220];
  200. [0 0 0 1];];
  201. tran23 = [[cosd(T4) 0 sind(T4) 0];
  202. [0 1 0 0];
  203. [(-1*sind(T4)) 0 cosd(T4) 160];
  204. [0 0 0 1];];
  205. tran0A = [[cosd(T1) sind(T1) 0 0];
  206. [(-1*sind(T1)) cosd(T1) 0 0];
  207. [0 0 1 0];
  208. [0 0 0 1];];
  209. %setting up Tt_ in order to calculate the top view better. Tt_ is an angle
  210. if abs(T2) > 90
  211. Tt2 = 90 - mod(T2,90);
  212. else
  213. Tt2 = T2;
  214. end
  215. if abs(T2+T3) > 90
  216. Tt3 = 90 - mod(T2+T3,90);
  217. else
  218. Tt3 = T2+T3;
  219. end
  220. if abs(T2+T3+T4) > 90
  221. Tt4 = 90 - mod(T2+T3+T4,90);
  222. else
  223. Tt4 = T2+T3+T4;
  224. end
  225. Tt5 = T5;
  226. %Lengths of each segment from top view
  227. Bt = sind(Tt2) * Bi;
  228. Ft = sind(Tt3) * F;
  229. Wt = sind(Tt4) * W;
  230. Ct = sind(Tt4) * Cl;
  231. AP = cosd(Tt5) * L;
  232. %points in top view
  233. PAL = [0;Bt+Ft+Wt; 0;1;];
  234. PA1L=[0; Bt; 0; 1;];
  235. PA2L=[0; Ft; 0; 1;];
  236. PA3L=[0; Wt; 0; 1;];
  237. %Claw points in side view
  238. PAc1 = [-1*AP/2; Bt+Ft+Wt; 0; 1;];
  239. PAc2 = [AP/2; Bt+Ft+Wt; 0; 1;];
  240. PAc1L = [-1*AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  241. PAc2L = [AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  242. %Points in side view of all the arms in the 0 frame
  243. P0a = tran01*p1a;
  244. P0b = tran01*tran12*p2b;
  245. P0c = tran01*tran12*tran23*p3c;
  246. P0c1 = tran01*tran12*tran23*p3c1;
  247. P0c2 = tran01*tran12*tran23*p3c2;
  248. P0c1L = tran01*tran12*tran23*p3c1L;
  249. P0c2L = tran01*tran12*tran23*p3c2L;
  250. %top view points in 0 frame
  251. P0AL = tran0A*PAL;
  252. P0A1L = tran0A*PA1L;
  253. P0A2L = tran0A*PA2L;
  254. P0A3L = tran0A*PA3L;
  255. P0Ac1 = tran0A*PAc1;
  256. P0Ac2 = tran0A*PAc2;
  257. P0Ac1L = tran0A*PAc1L;
  258. P0Ac2L = tran0A*PAc2L;
  259. hold on
  260. %get rid of all previously drawn lines
  261. lines = get(gca, 'Children');
  262. delete(lines);
  263. %draw axis
  264. line([0 0],[0 75],'LineWidth',2.75);
  265. line([0 75],[0 0],'LineWidth',2.75);
  266. line([400 400],[0 75],'LineWidth',2.75);
  267. line([400 475],[0 0],'LineWidth',2.75);
  268. %draw side view
  269. line([0 0],[10 250]);
  270. line([0 P0a(1)],[250 P0a(3)]);
  271. line([P0a(1) P0b(1)],[P0a(3) P0b(3)]);
  272. line([P0b(1) P0c(1)],[P0b(3) P0c(3)]);
  273. line([P0c1(1) P0c2(1)],[P0c1(3) P0c2(3)]);
  274. line([P0c1(1) P0c1L(1)],[P0c1(3) P0c1L(3)]);
  275. line([P0c2(1) P0c2L(1)],[P0c2(3) P0c2L(3)]);
  276. %draw top view
  277. line([400 P0A1L(1)+400],[0 P0A1L(2)]);
  278. line([400+P0A1L(1) P0A2L(1)+P0A1L(1)+400],[P0A1L(2) P0A2L(2)+P0A1L(2)]);
  279. line([P0A2L(1)+P0A1L(1)+400 P0A3L(1)+P0A2L(1)+P0A1L(1)+400],[P0A2L(2)+P0A1L(2) P0A3L(2)+P0A2L(2)+P0A1L(2)]);
  280. line([P0Ac1(1)+400 P0Ac2(1)+400],[P0Ac1(2) P0Ac2(2)]);
  281. line([P0Ac1(1)+400 P0Ac1L(1)+400],[P0Ac1(2) P0Ac1L(2)]);
  282. line([P0Ac2(1)+400 P0Ac2L(1)+400],[P0Ac2(2) P0Ac2L(2)]);
  283. %name axis
  284. text(-30,70,'Z');
  285. text(70,-30,'X');
  286. text(370,70,'Y');
  287. text(470,-30,'X');
  288. hold off
  289. %pause so everyone can see what changed on the frame
  290. pause(.01);
  291. end
  292.  
  293. end
  294.  
  295. else
  296. HomeTest = samp{:};
  297. samp = str2num(samp{:});
  298. Size = size(samp);
  299. LENGTH = Size(1);
  300. %loop fro every line you inputed into the box with ; as the end line
  301. %setup for this run through the loop. set A-E to be the inputed
  302. %variables
  303. s1 = 'home';%matlab is fucking dumb
  304. tf = strcmp(HomeTest,s1)
  305. if tf == 1
  306. while ~isequal(curAn,Home)
  307. if curAn(1) < Home(1)
  308. curAn(1) = curAn(1) + 40;
  309. elseif curAn(1) > Home(1)
  310. curAn(1) = curAn(1) - 40;
  311. end
  312. if curAn(2) < Home(2)
  313. curAn(2) = curAn(2) + 40;
  314. elseif curAn(1) > Home(2)
  315. curAn(2) = curAn(2) - 40;
  316. end
  317. if curAn(3) < Home(3)
  318. curAn(3) = curAn(3) + 40;
  319. elseif curAn(3) > Home(3)
  320. curAn(3) = curAn(3) - 40;
  321. end
  322. if curAn(4) < Home(4) + 13.4 || curAn(4) > Home(4) - 13.4
  323. curAn(4) = Home(4);%We can't iterate anysmaller than this. So next move is to Home(4)
  324. elseif curAn(4) < Home(4)
  325. curAn(4) = curAn(4) + 13.33333;
  326. elseif curAn(4) > Home(4)
  327. curAn(4) = curAn(4) - 13.33333;
  328. end
  329. if curAn(5) < Home(5) + 13.4 || curAn(5) > Home(5) - 13.4
  330. curAn(5) = Home(5);%We can't iterate anysmaller than this. So next move is to Home(4)
  331. elseif curAn(5) < Home(5)
  332. curAn(5) = curAn(5) + 13.33333;
  333. elseif curAn(5) > Home(5)
  334. curAn(5) = curAn(5) - 13.33333;
  335. end
  336. %Setting up figure
  337. Fig = gca;
  338. Fig.FontSize = 12;
  339. Fig.TickDir = 'out';
  340. Fig.TickLength = [.02, .02];
  341. Fig.XLimMode = 'manual';
  342. Fig.YLimMode = 'manual';
  343. Fig.YLim = [-100, 800];
  344. Fig.XLim = [-100, 800];
  345. %Calculating angles for sine/cosine inputs
  346. T1 = 90 - (curAn(1) * .025);
  347. T2 = (curAn(2)) * .025;
  348. T3 = (curAn(3)) * .025;
  349. T4 = (curAn(4)) * .075;
  350. T5 = (curAn(5)) * .075;
  351. %Distance between clamps from side view
  352. OP = sind(T5)*L;
  353. %Marking points
  354. p3c1 = [-1*(OP/2); 0; 50; 1;];
  355. p3c2 = [(OP/2); 0; 50; 1;];
  356. p3c1L = [-1*(OP/2); 0; 50+75; 1;];
  357. p3c2L = [(OP/2); 0; 50+75; 1;];
  358. %Transfer functions
  359. tran01 = [[cosd(T2) 0 sind(T2) 0];
  360. [0 1 0 0];
  361. [(-1*sind(T2)) 0 cosd(T2) 250];
  362. [0 0 0 1];];
  363. tran12 = [[cosd(T3) 0 sind(T3) 0];
  364. [0 1 0 0];
  365. [(-1*sind(T3)) 0 cosd(T3) 220];
  366. [0 0 0 1];];
  367. tran23 = [[cosd(T4) 0 sind(T4) 0];
  368. [0 1 0 0];
  369. [(-1*sind(T4)) 0 cosd(T4) 160];
  370. [0 0 0 1];];
  371. tran0A = [[cosd(T1) sind(T1) 0 0];
  372. [(-1*sind(T1)) cosd(T1) 0 0];
  373. [0 0 1 0];
  374. [0 0 0 1];];
  375. %setting up Tt_ in order to calculate the top view better. Tt_ is an angle
  376. if abs(T2) > 90
  377. Tt2 = 90 - mod(T2,90);
  378. else
  379. Tt2 = T2;
  380. end
  381. if abs(T2+T3) > 90
  382. Tt3 = 90 - mod(T2+T3,90);
  383. else
  384. Tt3 = T2+T3;
  385. end
  386. if abs(T2+T3+T4) > 90
  387. Tt4 = 90 - mod(T2+T3+T4,90);
  388. else
  389. Tt4 = T2+T3+T4;
  390. end
  391. Tt5 = T5;
  392. %Lengths of each segment from top view
  393. Bt = sind(Tt2) * Bi;
  394. Ft = sind(Tt3) * F;
  395. Wt = sind(Tt4) * W;
  396. Ct = sind(Tt4) * Cl;
  397. AP = cosd(Tt5) * L;
  398. %points in top view
  399. PAL = [0;Bt+Ft+Wt; 0;1;];
  400. PA1L=[0; Bt; 0; 1;];
  401. PA2L=[0; Ft; 0; 1;];
  402. PA3L=[0; Wt; 0; 1;];
  403. %Claw points in side view
  404. PAc1 = [-1*AP/2; Bt+Ft+Wt; 0; 1;];
  405. PAc2 = [AP/2; Bt+Ft+Wt; 0; 1;];
  406. PAc1L = [-1*AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  407. PAc2L = [AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  408. %Points in side view of all the arms in the 0 frame
  409. P0a = tran01*p1a;
  410. P0b = tran01*tran12*p2b;
  411. P0c = tran01*tran12*tran23*p3c;
  412. P0c1 = tran01*tran12*tran23*p3c1;
  413. P0c2 = tran01*tran12*tran23*p3c2;
  414. P0c1L = tran01*tran12*tran23*p3c1L;
  415. P0c2L = tran01*tran12*tran23*p3c2L;
  416. %top view points in 0 frame
  417. P0AL = tran0A*PAL;
  418. P0A1L = tran0A*PA1L;
  419. P0A2L = tran0A*PA2L;
  420. P0A3L = tran0A*PA3L;
  421. P0Ac1 = tran0A*PAc1;
  422. P0Ac2 = tran0A*PAc2;
  423. P0Ac1L = tran0A*PAc1L;
  424. P0Ac2L = tran0A*PAc2L;
  425. hold on
  426. %get rid of all previously drawn lines
  427. lines = get(gca, 'Children');
  428. delete(lines);
  429. %draw axis
  430. line([0 0],[0 75],'LineWidth',2.75);
  431. line([0 75],[0 0],'LineWidth',2.75);
  432. line([400 400],[0 75],'LineWidth',2.75);
  433. line([400 475],[0 0],'LineWidth',2.75);
  434. %draw side view
  435. line([0 0],[10 250]);
  436. line([0 P0a(1)],[250 P0a(3)]);
  437. line([P0a(1) P0b(1)],[P0a(3) P0b(3)]);
  438. line([P0b(1) P0c(1)],[P0b(3) P0c(3)]);
  439. line([P0c1(1) P0c2(1)],[P0c1(3) P0c2(3)]);
  440. line([P0c1(1) P0c1L(1)],[P0c1(3) P0c1L(3)]);
  441. line([P0c2(1) P0c2L(1)],[P0c2(3) P0c2L(3)]);
  442. %draw top view
  443. line([400 P0A1L(1)+400],[0 P0A1L(2)]);
  444. line([400+P0A1L(1) P0A2L(1)+P0A1L(1)+400],[P0A1L(2) P0A2L(2)+P0A1L(2)]);
  445. line([P0A2L(1)+P0A1L(1)+400 P0A3L(1)+P0A2L(1)+P0A1L(1)+400],[P0A2L(2)+P0A1L(2) P0A3L(2)+P0A2L(2)+P0A1L(2)]);
  446. line([P0Ac1(1)+400 P0Ac2(1)+400],[P0Ac1(2) P0Ac2(2)]);
  447. line([P0Ac1(1)+400 P0Ac1L(1)+400],[P0Ac1(2) P0Ac1L(2)]);
  448. line([P0Ac2(1)+400 P0Ac2L(1)+400],[P0Ac2(2) P0Ac2L(2)]);
  449. %name axis
  450. text(-30,70,'Z');
  451. text(70,-30,'X');
  452. text(370,70,'Y');
  453. text(470,-30,'X');
  454. hold off
  455. %pause so everyone can see what changed on the frame
  456. pause(.01);
  457. end
  458. else
  459. for i = 1:LENGTH
  460. A = samp(i,1);%waist
  461. B = samp(i,2);%shoulder
  462. C = samp(i,3);%elbow
  463. D = samp(i,4);%wrist1
  464. E = samp(i,5);%wrist2
  465. while A ~= 0 || B ~= 0 || C ~= 0 || D ~= 0 || E ~= 0
  466. %Find if angle change A
  467. % is within limits
  468. if A > 0
  469. if curAn(1) + 40 < max(1)
  470. curAn(1) = curAn(1) + 40;
  471. if A < 40
  472. A = 0;
  473. else
  474. A = A - 40;
  475. end
  476. else
  477. curAn(1) = max(1);
  478. A = 0;
  479. end
  480. elseif A < 0
  481. if curAn(1) - 40 > min(1)
  482. curAn(1) = curAn(1) - 40;
  483. if A > -40
  484. A = 0;
  485. else
  486. A = A + 40;
  487. end
  488. else
  489. curAn(1) = min(1)
  490. A = 0;
  491. end
  492. end
  493. if B > 0
  494. if curAn(2) + 40 < max(2)
  495. curAn(2) = curAn(2) + 40;
  496. if B < 40
  497. B = 0;
  498. else
  499. B = B - 40;
  500. end
  501. else
  502. curAn(2) = max(2);
  503. B = 0;
  504. end
  505. elseif B < 0
  506. if curAn(2) - 40 > min(2)
  507. curAn(2) = curAn(2) - 40;
  508. if B > -40
  509. B = 0;
  510. else
  511. B = B + 40;
  512. end
  513. else
  514. curAn(2) = min(2);
  515. B = 0;
  516. end
  517. end
  518. if C > 0
  519. if curAn(3) + 40 < max(3)
  520. curAn(3) = curAn(3) + 40;
  521. if C < 40
  522. C = 0;
  523. else
  524. C = C - 40;
  525. end
  526. else
  527. curAn(3) = max(3);
  528. C = 0;
  529. end
  530. elseif C < 0
  531. if curAn(3) - 40 > min(3)
  532. curAn(3) = curAn(3) - 40;
  533. if C > -40
  534. C = 0;
  535. else
  536. C = C + 40;
  537. end
  538. else
  539. curAn(3) = min(3);
  540. C = 0;
  541. end
  542. end
  543. %Find out what kind of angle change D
  544. %and E are. Both the same sign means
  545. %wrist up/down and different signs
  546. %means wrist spin left/right
  547. %then check limits of angle 4/5
  548. if (D < 0 & E < 0) || (D*E > 0)
  549. if D > 0
  550. if curAn(5) + 13.33333 < max(5)
  551. curAn(5) = curAn(5) + 13.33333;
  552. if D < 13.4
  553. D = 0;
  554. E = 0;
  555. else
  556. D = D - 13.33333;
  557. E = E - 13.33333;
  558. end
  559. else
  560. curAn(5) = max(5);
  561. D = 0;
  562. E = 0;
  563. end
  564. elseif D < 0
  565. if curAn(5) - 13.33333 > min(5)
  566. curAn(5) = curAn(5) - 13.33333;
  567. if D > -13.4
  568. D = 0;
  569. E = 0;
  570. else
  571. D = D + 13.33333;
  572. E = E + 13.33333;
  573. end
  574. else
  575. curAn(5) = min(5);
  576. D = 0;
  577. E = 0;
  578. end
  579. end
  580. else
  581. if D-E > 0
  582. if curAn(4) + 13.33333 < max(4)
  583. curAn(4) = curAn(4) + 13.33333;
  584. if D-E < 13.4
  585. D = 0;
  586. E = 0;
  587. else
  588. D = D - 13.33333;
  589. E = E + 13.33333;
  590. end
  591. else
  592. curAn(4) = max(4);
  593. D = 0;
  594. E = 0;
  595. end
  596. elseif D-E < 0
  597. if curAn(4) - 13.33333 > min(4)
  598. curAn(4) = curAn(4) - 13.33333;
  599. if D-E > -13.4
  600. D = 0;
  601. E = 0;
  602. else
  603. D = D - 13.33333;
  604. E = E + 13.33333;
  605. end
  606. else
  607. curAn(4) = min(4);
  608. D = 0;
  609. E = 0;
  610. end
  611. end
  612. end
  613. %Setting up figure
  614. Fig = gca;
  615. Fig.FontSize = 12;
  616. Fig.TickDir = 'out';
  617. Fig.TickLength = [.02, .02];
  618. Fig.XLimMode = 'manual';
  619. Fig.YLimMode = 'manual';
  620. Fig.YLim = [-100, 800];
  621. Fig.XLim = [-100, 800];
  622. %Calculating angles for sine/cosine inputs
  623. T1 = 90 - (curAn(1) * .025);
  624. T2 = (curAn(2)) * .025;
  625. T3 = (curAn(3)) * .025;
  626. T4 = (curAn(4)) * .075;
  627. T5 = (curAn(5)) * .075;
  628. %Distance between clamps from side view
  629. OP = sind(T5)*L;
  630. %Marking points
  631. p3c1 = [-1*(OP/2); 0; 50; 1;];
  632. p3c2 = [(OP/2); 0; 50; 1;];
  633. p3c1L = [-1*(OP/2); 0; 50+75; 1;];
  634. p3c2L = [(OP/2); 0; 50+75; 1;];
  635. %Transfer functions
  636. tran01 = [[cosd(T2) 0 sind(T2) 0];
  637. [0 1 0 0];
  638. [(-1*sind(T2)) 0 cosd(T2) 250];
  639. [0 0 0 1];];
  640. tran12 = [[cosd(T3) 0 sind(T3) 0];
  641. [0 1 0 0];
  642. [(-1*sind(T3)) 0 cosd(T3) 220];
  643. [0 0 0 1];];
  644. tran23 = [[cosd(T4) 0 sind(T4) 0];
  645. [0 1 0 0];
  646. [(-1*sind(T4)) 0 cosd(T4) 160];
  647. [0 0 0 1];];
  648. tran0A = [[cosd(T1) sind(T1) 0 0];
  649. [(-1*sind(T1)) cosd(T1) 0 0];
  650. [0 0 1 0];
  651. [0 0 0 1];];
  652. %setting up Tt_ in order to calculate the top view better. Tt_ is an angle
  653. if abs(T2) > 90
  654. Tt2 = 90 - mod(T2,90);
  655. else
  656. Tt2 = T2;
  657. end
  658. if abs(T2+T3) > 90
  659. Tt3 = 90 - mod(T2+T3,90);
  660. else
  661. Tt3 = T2+T3;
  662. end
  663. if abs(T2+T3+T4) > 90
  664. Tt4 = 90 - mod(T2+T3+T4,90);
  665. else
  666. Tt4 = T2+T3+T4;
  667. end
  668. Tt5 = T5;
  669. %Lengths of each segment from top view
  670. Bt = sind(Tt2) * Bi;
  671. Ft = sind(Tt3) * F;
  672. Wt = sind(Tt4) * W;
  673. Ct = sind(Tt4) * Cl;
  674. AP = cosd(Tt5) * L;
  675. %points in top view
  676. PAL = [0;Bt+Ft+Wt; 0;1;];
  677. PA1L=[0; Bt; 0; 1;];
  678. PA2L=[0; Ft; 0; 1;];
  679. PA3L=[0; Wt; 0; 1;];
  680. %Claw points in side view
  681. PAc1 = [-1*AP/2; Bt+Ft+Wt; 0; 1;];
  682. PAc2 = [AP/2; Bt+Ft+Wt; 0; 1;];
  683. PAc1L = [-1*AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  684. PAc2L = [AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  685. %Points in side view of all the arms in the 0 frame
  686. P0a = tran01*p1a;
  687. P0b = tran01*tran12*p2b;
  688. P0c = tran01*tran12*tran23*p3c;
  689. P0c1 = tran01*tran12*tran23*p3c1;
  690. P0c2 = tran01*tran12*tran23*p3c2;
  691. P0c1L = tran01*tran12*tran23*p3c1L;
  692. P0c2L = tran01*tran12*tran23*p3c2L;
  693. %top view points in 0 frame
  694. P0AL = tran0A*PAL;
  695. P0A1L = tran0A*PA1L;
  696. P0A2L = tran0A*PA2L;
  697. P0A3L = tran0A*PA3L;
  698. P0Ac1 = tran0A*PAc1;
  699. P0Ac2 = tran0A*PAc2;
  700. P0Ac1L = tran0A*PAc1L;
  701. P0Ac2L = tran0A*PAc2L;
  702. hold on
  703. %get rid of all previously drawn lines
  704. lines = get(gca, 'Children');
  705. delete(lines);
  706. %draw axis
  707. line([0 0],[0 75],'LineWidth',2.75);
  708. line([0 75],[0 0],'LineWidth',2.75);
  709. line([400 400],[0 75],'LineWidth',2.75);
  710. line([400 475],[0 0],'LineWidth',2.75);
  711. %draw side view
  712. line([0 0],[10 250]);
  713. line([0 P0a(1)],[250 P0a(3)]);
  714. line([P0a(1) P0b(1)],[P0a(3) P0b(3)]);
  715. line([P0b(1) P0c(1)],[P0b(3) P0c(3)]);
  716. line([P0c1(1) P0c2(1)],[P0c1(3) P0c2(3)]);
  717. line([P0c1(1) P0c1L(1)],[P0c1(3) P0c1L(3)]);
  718. line([P0c2(1) P0c2L(1)],[P0c2(3) P0c2L(3)]);
  719. %draw top view
  720. line([400 P0A1L(1)+400],[0 P0A1L(2)]);
  721. line([400+P0A1L(1) P0A2L(1)+P0A1L(1)+400],[P0A1L(2) P0A2L(2)+P0A1L(2)]);
  722. line([P0A2L(1)+P0A1L(1)+400 P0A3L(1)+P0A2L(1)+P0A1L(1)+400],[P0A2L(2)+P0A1L(2) P0A3L(2)+P0A2L(2)+P0A1L(2)]);
  723. line([P0Ac1(1)+400 P0Ac2(1)+400],[P0Ac1(2) P0Ac2(2)]);
  724. line([P0Ac1(1)+400 P0Ac1L(1)+400],[P0Ac1(2) P0Ac1L(2)]);
  725. line([P0Ac2(1)+400 P0Ac2L(1)+400],[P0Ac2(2) P0Ac2L(2)]);
  726. %name axis
  727. text(-30,70,'Z');
  728. text(70,-30,'X');
  729. text(370,70,'Y');
  730. text(470,-30,'X');
  731. hold off
  732. %pause so everyone can see what changed on the frame
  733. pause(.1);
  734. end
  735. end
  736. end
  737. end
  738. %ask if they want to end the loop
  739. exit = input('1 to quit and 0 to continue: ');
  740.  
  741.  
  742. %Continue? Make exit = 1 for ending
  743. 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!