AD

Author: Kareem

Course-project.pdf

@@ -0,0 +1,30 @@
+function [retval] = GetMaxTorque (l1,l2,l3, MinQ1,MaxQ1,MinQ2,MaxQ2,MinQ3,MaxQ3,load)
+r=eye(3,3);
+r(3,1)=1;r(3,2)=1;r(3,3)=1;
+m=0;
+m2=0;
+m3=0;
+for i=MinQ1:10:MaxQ1
+  for j=MinQ2:10:MaxQ2
+    for k=MinQ3:10:MaxQ3
+      r(1,1)=-l1*sind(i)-l2*sind(i+j)-l3*sind(i+j+k);
+      r(1,2)=-l2*sind(i+j)-l3*sind(i+j+k);
+      r(1,3)=-l3*sind(i+j+k);
+      r(2,1)=l1*cosd(i)+l2*cosd(i+j)+l3*cosd(i+j+k);
+      r(2,2)=l2*cosd(i+j)+l3*cosd(i+j+k);
+      r(2,3)=l3*cosd(i+j+k);
+      aa=-r'*load;
+      if(abs(aa(1))>abs(m))
+        m = aa(1);
+      end
+      if(abs(aa(2))>abs(m2))
+        m2 = aa(2);
+      end
+      if(abs(aa(3))>abs(m3))
+        m3 = aa(3);
+      end
+     end
+   end
+end
+retval = [m m2 m3];
+endfunction

GetMaxTorque.m

@@ -52,6 +52,9 @@
   while(q33<0)
   q33=q33+360;
   end
+  else
+    printf("Cannot reach ,irony :V \n");
+    return;
  end
  if(q1!=q11&&q2!=q22&&q3!=q33)
  [q1 q2 q3 q11 q22 q33]

InverseKinematic.m

@@ -1,7 +1,7 @@
-function [retval] = PlotWorkingSpace (l1, l2,l3,maxQ1,maxQ2,maxQ3)
-k=0:maxQ3;
-for i=0:maxQ1
-    for j=0:maxQ2
+function [retval] = PlotWorkingSpace (l1, l2,l3,MinQ1,maxQ1,MinQ2,maxQ2,MinQ3,maxQ3)
+k=MinQ3:maxQ3;
+for i=MinQ1:maxQ1
+    for j=MinQ2:maxQ2
             plot(l1*cosd(i)+l2*cosd(i+j)+l3*cosd(i+j+k),l1*sind(i)+l2*sind(i+j)+l3*sind(i+j+k),'.y','markersize',10);
             hold on;
     end

PlotWorkingSpace.m

@@ -1,8 +1,21 @@
 fileID = fopen('input.txt','r');
 formatSpec = '%f';
 input = fscanf(fileID,formatSpec);
-InverseKinematic(input(7),input(8),input(9),input(10),input(11),input(12));
 printf("\n");
-#printf("program passed , press anykey to continue to Plotting");
-#pause;
-#PlotWorkingSpace(input(1),input(2),input(3),input(4),input(5),input(6));
+printf("program passed , press anykey to continue to Calculating InverseKinematics");
+pause;
+printf("\n");
+
+InverseKinematic(input(10),input(11),input(12),input(13),input(14),input(15));
+printf("\n");
+printf("program passed , press anykey to continue to Calculating Max Torque");
+pause;
+printf("\n");
+
+GetMaxTorque(input(16),input(17),input(18),input(19),input(20),input(21),input(22),input(23),input(24),[input(25) input(26) input(27)]')
+printf("\n");
+printf("program passed , press anykey to continue to Plotting");
+pause;
+printf("\n");
+
+PlotWorkingSpace(input(1),input(2),input(3),input(4),input(5),input(6),input(7),input(8),input(9));

Project_Main.m

@@ -0,0 +1,11 @@
+1 1 1
+0 45
+0 45
+0 45
+100 100 100
+50 50 20
+1 1 1
+0 45
+0 45
+0 45
+1 1 1