API tools faq
paste
Advertisement
Guest User

FinalCode

a guest
Nov 24th, 2014
206
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 24.70 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. % Initalize circle values
  151. Xs = 140;
  152. Ys = 0; %does not change
  153. Zs = 240;
  154. Rs = 60;
  155. %omega represents the current angle of the circle
  156. for omega = 0:360
  157. %%Calculate our X/Y/Z corridnates in mm
  158. X = Xs + Rs*sind(omega);
  159. Y = Ys + Rs*cosd(omega);
  160. Z = Zs; %Z remains constant
  161. %Calculate our theta values in degrees
  162. theta1=atand(Y/X);
  163. %dist is used to find thetas 2 and 3
  164. Dist=sqrt((X.^2)+2500);
  165. theta2=acosd(250/Dist)+acosd(((-22800-Dist.^2)/(-320*Dist)));
  166. theta3=acosd((Dist.^2-22800)/-70400);
  167. %Robot forms a kind of quadralateral, so last theta can be
  168. %determined using algebra
  169. theta4=360-90-theta2-theta3;
  170.  
  171. %Draw the robot
  172. %It Goes Here
  173. end
  174.  
  175.  
  176. else
  177. HomeTest = samp{:};
  178. samp = str2num(samp{:});
  179. Size = size(samp);
  180. LENGTH = Size(1);
  181. %loop fro every line you inputed into the box with ; as the end line
  182. %setup for this run through the loop. set A-E to be the inputed
  183. %variables
  184. s1 = 'home';%matlab is fucking dumb
  185. tf = strcmp(HomeTest,s1)
  186. if tf == 1
  187. while ~isequal(curAn,Home)
  188. if curAn(1) < Home(1)
  189. curAn(1) = curAn(1) + 40;
  190. elseif curAn(1) > Home(1)
  191. curAn(1) = curAn(1) - 40;
  192. end
  193. if curAn(2) < Home(2)
  194. curAn(2) = curAn(2) + 40;
  195. elseif curAn(1) > Home(2)
  196. curAn(2) = curAn(2) - 40;
  197. end
  198. if curAn(3) < Home(3)
  199. curAn(3) = curAn(3) + 40;
  200. elseif curAn(3) > Home(3)
  201. curAn(3) = curAn(3) - 40;
  202. end
  203. if curAn(4) < Home(4) + 13.4 || curAn(4) > Home(4) - 13.4
  204. curAn(4) = Home(4);%We can't iterate anysmaller than this. So next move is to Home(4)
  205. elseif curAn(4) < Home(4)
  206. curAn(4) = curAn(4) + 13.33333;
  207. elseif curAn(4) > Home(4)
  208. curAn(4) = curAn(4) - 13.33333;
  209. end
  210. if curAn(5) < Home(5) + 13.4 || curAn(5) > Home(5) - 13.4
  211. curAn(5) = Home(5);%We can't iterate anysmaller than this. So next move is to Home(4)
  212. elseif curAn(5) < Home(5)
  213. curAn(5) = curAn(5) + 13.33333;
  214. elseif curAn(5) > Home(5)
  215. curAn(5) = curAn(5) - 13.33333;
  216. end
  217. %Setting up figure
  218. Fig = gca;
  219. Fig.FontSize = 12;
  220. Fig.TickDir = 'out';
  221. Fig.TickLength = [.02, .02];
  222. Fig.XLimMode = 'manual';
  223. Fig.YLimMode = 'manual';
  224. Fig.YLim = [-100, 800];
  225. Fig.XLim = [-100, 800];
  226. %Calculating angles for sine/cosine inputs
  227. T1 = 90 - (curAn(1) * .025);
  228. T2 = (curAn(2)) * .025;
  229. T3 = (curAn(3)) * .025;
  230. T4 = (curAn(4)) * .075;
  231. T5 = (curAn(5)) * .075;
  232. %Distance between clamps from side view
  233. OP = sind(T5)*L;
  234. %Marking points
  235. p3c1 = [-1*(OP/2); 0; 50; 1;];
  236. p3c2 = [(OP/2); 0; 50; 1;];
  237. p3c1L = [-1*(OP/2); 0; 50+75; 1;];
  238. p3c2L = [(OP/2); 0; 50+75; 1;];
  239. %Transfer functions
  240. tran01 = [[cosd(T2) 0 sind(T2) 0];
  241. [0 1 0 0];
  242. [(-1*sind(T2)) 0 cosd(T2) 250];
  243. [0 0 0 1];];
  244. tran12 = [[cosd(T3) 0 sind(T3) 0];
  245. [0 1 0 0];
  246. [(-1*sind(T3)) 0 cosd(T3) 220];
  247. [0 0 0 1];];
  248. tran23 = [[cosd(T4) 0 sind(T4) 0];
  249. [0 1 0 0];
  250. [(-1*sind(T4)) 0 cosd(T4) 160];
  251. [0 0 0 1];];
  252. tran0A = [[cosd(T1) sind(T1) 0 0];
  253. [(-1*sind(T1)) cosd(T1) 0 0];
  254. [0 0 1 0];
  255. [0 0 0 1];];
  256. %setting up Tt_ in order to calculate the top view better. Tt_ is an angle
  257. if abs(T2) > 90
  258. Tt2 = 90 - mod(T2,90);
  259. else
  260. Tt2 = T2;
  261. end
  262. if abs(T2+T3) > 90
  263. Tt3 = 90 - mod(T2+T3,90);
  264. else
  265. Tt3 = T2+T3;
  266. end
  267. if abs(T2+T3+T4) > 90
  268. Tt4 = 90 - mod(T2+T3+T4,90);
  269. else
  270. Tt4 = T2+T3+T4;
  271. end
  272. Tt5 = T5;
  273. %Lengths of each segment from top view
  274. Bt = sind(Tt2) * Bi;
  275. Ft = sind(Tt3) * F;
  276. Wt = sind(Tt4) * W;
  277. Ct = sind(Tt4) * Cl;
  278. AP = cosd(Tt5) * L;
  279. %points in top view
  280. PAL = [0;Bt+Ft+Wt; 0;1;];
  281. PA1L=[0; Bt; 0; 1;];
  282. PA2L=[0; Ft; 0; 1;];
  283. PA3L=[0; Wt; 0; 1;];
  284. %Claw points in side view
  285. PAc1 = [-1*AP/2; Bt+Ft+Wt; 0; 1;];
  286. PAc2 = [AP/2; Bt+Ft+Wt; 0; 1;];
  287. PAc1L = [-1*AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  288. PAc2L = [AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  289. %Points in side view of all the arms in the 0 frame
  290. P0a = tran01*p1a;
  291. P0b = tran01*tran12*p2b;
  292. P0c = tran01*tran12*tran23*p3c;
  293. P0c1 = tran01*tran12*tran23*p3c1;
  294. P0c2 = tran01*tran12*tran23*p3c2;
  295. P0c1L = tran01*tran12*tran23*p3c1L;
  296. P0c2L = tran01*tran12*tran23*p3c2L;
  297. %top view points in 0 frame
  298. P0AL = tran0A*PAL;
  299. P0A1L = tran0A*PA1L;
  300. P0A2L = tran0A*PA2L;
  301. P0A3L = tran0A*PA3L;
  302. P0Ac1 = tran0A*PAc1;
  303. P0Ac2 = tran0A*PAc2;
  304. P0Ac1L = tran0A*PAc1L;
  305. P0Ac2L = tran0A*PAc2L;
  306. hold on
  307. %get rid of all previously drawn lines
  308. lines = get(gca, 'Children');
  309. delete(lines);
  310. %draw axis
  311. line([0 0],[0 75],'LineWidth',2.75);
  312. line([0 75],[0 0],'LineWidth',2.75);
  313. line([400 400],[0 75],'LineWidth',2.75);
  314. line([400 475],[0 0],'LineWidth',2.75);
  315. %draw side view
  316. line([0 0],[10 250]);
  317. line([0 P0a(1)],[250 P0a(3)]);
  318. line([P0a(1) P0b(1)],[P0a(3) P0b(3)]);
  319. line([P0b(1) P0c(1)],[P0b(3) P0c(3)]);
  320. line([P0c1(1) P0c2(1)],[P0c1(3) P0c2(3)]);
  321. line([P0c1(1) P0c1L(1)],[P0c1(3) P0c1L(3)]);
  322. line([P0c2(1) P0c2L(1)],[P0c2(3) P0c2L(3)]);
  323. %draw top view
  324. line([400 P0A1L(1)+400],[0 P0A1L(2)]);
  325. line([400+P0A1L(1) P0A2L(1)+P0A1L(1)+400],[P0A1L(2) P0A2L(2)+P0A1L(2)]);
  326. line([P0A2L(1)+P0A1L(1)+400 P0A3L(1)+P0A2L(1)+P0A1L(1)+400],[P0A2L(2)+P0A1L(2) P0A3L(2)+P0A2L(2)+P0A1L(2)]);
  327. line([P0Ac1(1)+400 P0Ac2(1)+400],[P0Ac1(2) P0Ac2(2)]);
  328. line([P0Ac1(1)+400 P0Ac1L(1)+400],[P0Ac1(2) P0Ac1L(2)]);
  329. line([P0Ac2(1)+400 P0Ac2L(1)+400],[P0Ac2(2) P0Ac2L(2)]);
  330. %name axis
  331. text(-30,70,'Z');
  332. text(70,-30,'X');
  333. text(370,70,'Y');
  334. text(470,-30,'X');
  335. hold off
  336. %pause so everyone can see what changed on the frame
  337. pause(.01);
  338. end
  339. else
  340. for i = 1:LENGTH
  341. A = samp(i,1);%waist
  342. B = samp(i,2);%shoulder
  343. C = samp(i,3);%elbow
  344. D = samp(i,4);%wrist1
  345. E = samp(i,5);%wrist2
  346. while A ~= 0 || B ~= 0 || C ~= 0 || D ~= 0 || E ~= 0
  347. %Find if angle change A
  348. % is within limits
  349. if A > 0
  350. if curAn(1) + 40 < max(1)
  351. curAn(1) = curAn(1) + 40;
  352. if A < 40
  353. A = 0;
  354. else
  355. A = A - 40;
  356. end
  357. else
  358. curAn(1) = max(1);
  359. A = 0;
  360. end
  361. elseif A < 0
  362. if curAn(1) - 40 > min(1)
  363. curAn(1) = curAn(1) - 40;
  364. if A > -40
  365. A = 0;
  366. else
  367. A = A + 40;
  368. end
  369. else
  370. curAn(1) = min(1)
  371. A = 0;
  372. end
  373. end
  374. if B > 0
  375. if curAn(2) + 40 < max(2)
  376. curAn(2) = curAn(2) + 40;
  377. if B < 40
  378. B = 0;
  379. else
  380. B = B - 40;
  381. end
  382. else
  383. curAn(2) = max(2);
  384. B = 0;
  385. end
  386. elseif B < 0
  387. if curAn(2) - 40 > min(2)
  388. curAn(2) = curAn(2) - 40;
  389. if B > -40
  390. B = 0;
  391. else
  392. B = B + 40;
  393. end
  394. else
  395. curAn(2) = min(2);
  396. B = 0;
  397. end
  398. end
  399. if C > 0
  400. if curAn(3) + 40 < max(3)
  401. curAn(3) = curAn(3) + 40;
  402. if C < 40
  403. C = 0;
  404. else
  405. C = C - 40;
  406. end
  407. else
  408. curAn(3) = max(3);
  409. C = 0;
  410. end
  411. elseif C < 0
  412. if curAn(3) - 40 > min(3)
  413. curAn(3) = curAn(3) - 40;
  414. if C > -40
  415. C = 0;
  416. else
  417. C = C + 40;
  418. end
  419. else
  420. curAn(3) = min(3);
  421. C = 0;
  422. end
  423. end
  424. %Find out what kind of angle change D
  425. %and E are. Both the same sign means
  426. %wrist up/down and different signs
  427. %means wrist spin left/right
  428. %then check limits of angle 4/5
  429. if (D < 0 & E < 0) || (D*E > 0)
  430. if D > 0
  431. if curAn(5) + 13.33333 < max(5)
  432. curAn(5) = curAn(5) + 13.33333;
  433. if D < 13.4
  434. D = 0;
  435. E = 0;
  436. else
  437. D = D - 13.33333;
  438. E = E - 13.33333;
  439. end
  440. else
  441. curAn(5) = max(5);
  442. D = 0;
  443. E = 0;
  444. end
  445. elseif D < 0
  446. if curAn(5) - 13.33333 > min(5)
  447. curAn(5) = curAn(5) - 13.33333;
  448. if D > -13.4
  449. D = 0;
  450. E = 0;
  451. else
  452. D = D + 13.33333;
  453. E = E + 13.33333;
  454. end
  455. else
  456. curAn(5) = min(5);
  457. D = 0;
  458. E = 0;
  459. end
  460. end
  461. else
  462. if D-E > 0
  463. if curAn(4) + 13.33333 < max(4)
  464. curAn(4) = curAn(4) + 13.33333;
  465. if D-E < 13.4
  466. D = 0;
  467. E = 0;
  468. else
  469. D = D - 13.33333;
  470. E = E + 13.33333;
  471. end
  472. else
  473. curAn(4) = max(4);
  474. D = 0;
  475. E = 0;
  476. end
  477. elseif D-E < 0
  478. if curAn(4) - 13.33333 > min(4)
  479. curAn(4) = curAn(4) - 13.33333;
  480. if D-E > -13.4
  481. D = 0;
  482. E = 0;
  483. else
  484. D = D - 13.33333;
  485. E = E + 13.33333;
  486. end
  487. else
  488. curAn(4) = min(4);
  489. D = 0;
  490. E = 0;
  491. end
  492. end
  493. end
  494. %Setting up figure
  495. Fig = gca;
  496. Fig.FontSize = 12;
  497. Fig.TickDir = 'out';
  498. Fig.TickLength = [.02, .02];
  499. Fig.XLimMode = 'manual';
  500. Fig.YLimMode = 'manual';
  501. Fig.YLim = [-100, 800];
  502. Fig.XLim = [-100, 800];
  503. %Calculating angles for sine/cosine inputs
  504. T1 = 90 - (curAn(1) * .025);
  505. T2 = (curAn(2)) * .025;
  506. T3 = (curAn(3)) * .025;
  507. T4 = (curAn(4)) * .075;
  508. T5 = (curAn(5)) * .075;
  509. %Distance between clamps from side view
  510. OP = sind(T5)*L;
  511. %Marking points
  512. p3c1 = [-1*(OP/2); 0; 50; 1;];
  513. p3c2 = [(OP/2); 0; 50; 1;];
  514. p3c1L = [-1*(OP/2); 0; 50+75; 1;];
  515. p3c2L = [(OP/2); 0; 50+75; 1;];
  516. %Transfer functions
  517. tran01 = [[cosd(T2) 0 sind(T2) 0];
  518. [0 1 0 0];
  519. [(-1*sind(T2)) 0 cosd(T2) 250];
  520. [0 0 0 1];];
  521. tran12 = [[cosd(T3) 0 sind(T3) 0];
  522. [0 1 0 0];
  523. [(-1*sind(T3)) 0 cosd(T3) 220];
  524. [0 0 0 1];];
  525. tran23 = [[cosd(T4) 0 sind(T4) 0];
  526. [0 1 0 0];
  527. [(-1*sind(T4)) 0 cosd(T4) 160];
  528. [0 0 0 1];];
  529. tran0A = [[cosd(T1) sind(T1) 0 0];
  530. [(-1*sind(T1)) cosd(T1) 0 0];
  531. [0 0 1 0];
  532. [0 0 0 1];];
  533. %setting up Tt_ in order to calculate the top view better. Tt_ is an angle
  534. if abs(T2) > 90
  535. Tt2 = 90 - mod(T2,90);
  536. else
  537. Tt2 = T2;
  538. end
  539. if abs(T2+T3) > 90
  540. Tt3 = 90 - mod(T2+T3,90);
  541. else
  542. Tt3 = T2+T3;
  543. end
  544. if abs(T2+T3+T4) > 90
  545. Tt4 = 90 - mod(T2+T3+T4,90);
  546. else
  547. Tt4 = T2+T3+T4;
  548. end
  549. Tt5 = T5;
  550. %Lengths of each segment from top view
  551. Bt = sind(Tt2) * Bi;
  552. Ft = sind(Tt3) * F;
  553. Wt = sind(Tt4) * W;
  554. Ct = sind(Tt4) * Cl;
  555. AP = cosd(Tt5) * L;
  556. %points in top view
  557. PAL = [0;Bt+Ft+Wt; 0;1;];
  558. PA1L=[0; Bt; 0; 1;];
  559. PA2L=[0; Ft; 0; 1;];
  560. PA3L=[0; Wt; 0; 1;];
  561. %Claw points in side view
  562. PAc1 = [-1*AP/2; Bt+Ft+Wt; 0; 1;];
  563. PAc2 = [AP/2; Bt+Ft+Wt; 0; 1;];
  564. PAc1L = [-1*AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  565. PAc2L = [AP/2; Bt+Ft+Wt+Ct; 0; 1;];
  566. %Points in side view of all the arms in the 0 frame
  567. P0a = tran01*p1a;
  568. P0b = tran01*tran12*p2b;
  569. P0c = tran01*tran12*tran23*p3c;
  570. P0c1 = tran01*tran12*tran23*p3c1;
  571. P0c2 = tran01*tran12*tran23*p3c2;
  572. P0c1L = tran01*tran12*tran23*p3c1L;
  573. P0c2L = tran01*tran12*tran23*p3c2L;
  574. %top view points in 0 frame
  575. P0AL = tran0A*PAL;
  576. P0A1L = tran0A*PA1L;
  577. P0A2L = tran0A*PA2L;
  578. P0A3L = tran0A*PA3L;
  579. P0Ac1 = tran0A*PAc1;
  580. P0Ac2 = tran0A*PAc2;
  581. P0Ac1L = tran0A*PAc1L;
  582. P0Ac2L = tran0A*PAc2L;
  583. hold on
  584. %get rid of all previously drawn lines
  585. lines = get(gca, 'Children');
  586. delete(lines);
  587. %draw axis
  588. line([0 0],[0 75],'LineWidth',2.75);
  589. line([0 75],[0 0],'LineWidth',2.75);
  590. line([400 400],[0 75],'LineWidth',2.75);
  591. line([400 475],[0 0],'LineWidth',2.75);
  592. %draw side view
  593. line([0 0],[10 250]);
  594. line([0 P0a(1)],[250 P0a(3)]);
  595. line([P0a(1) P0b(1)],[P0a(3) P0b(3)]);
  596. line([P0b(1) P0c(1)],[P0b(3) P0c(3)]);
  597. line([P0c1(1) P0c2(1)],[P0c1(3) P0c2(3)]);
  598. line([P0c1(1) P0c1L(1)],[P0c1(3) P0c1L(3)]);
  599. line([P0c2(1) P0c2L(1)],[P0c2(3) P0c2L(3)]);
  600. %draw top view
  601. line([400 P0A1L(1)+400],[0 P0A1L(2)]);
  602. line([400+P0A1L(1) P0A2L(1)+P0A1L(1)+400],[P0A1L(2) P0A2L(2)+P0A1L(2)]);
  603. line([P0A2L(1)+P0A1L(1)+400 P0A3L(1)+P0A2L(1)+P0A1L(1)+400],[P0A2L(2)+P0A1L(2) P0A3L(2)+P0A2L(2)+P0A1L(2)]);
  604. line([P0Ac1(1)+400 P0Ac2(1)+400],[P0Ac1(2) P0Ac2(2)]);
  605. line([P0Ac1(1)+400 P0Ac1L(1)+400],[P0Ac1(2) P0Ac1L(2)]);
  606. line([P0Ac2(1)+400 P0Ac2L(1)+400],[P0Ac2(2) P0Ac2L(2)]);
  607. %name axis
  608. text(-30,70,'Z');
  609. text(70,-30,'X');
  610. text(370,70,'Y');
  611. text(470,-30,'X');
  612. hold off
  613. %pause so everyone can see what changed on the frame
  614. pause(.1);
  615. end
  616. end
  617. end
  618. end
  619. %ask if they want to end the loop
  620. exit = input('1 to quit and 0 to continue: ');
  621.  
  622.  
  623. %Continue? Make exit = 1 for ending
  624. 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!