Diff: main.cpp

Revision:

12:6db54df00e14

Parent:

11:b84501ec1c12

Child:

13:e465ee0ccafb

--- a/main.cpp	Tue May 14 16:59:04 2019 +0000
+++ b/main.cpp	Wed May 15 10:50:56 2019 +0000
@@ -26,17 +26,41 @@
     return x/cos(phi(x,y));
 }
 
-//rumus teta2+teta1
-double rum_tet(float sdt, float x, float y, float r1, float r2){
+//rumus teta2+teta1 versi 1
+double rum1_tet(float sdt, float x, float y, float z, float r1, float r2){
     double tet_dou = asin((x_phi(x,y)-r1*sin(sdt))/r2);
     float tet = (float) tet_dou;
     return tet;
 }
 
+//rumus teta2+teta1 versi 2
+double rum2_tet(float sdt, float x, float y, float z, float r1, float r2){
+    double tet_dou = acos((z-r1*cos(sdt))/r2);
+    float tet = (float) tet_dou;
+    return tet;
+}
+
+//Penggunaan rumus 1 dan 2
+//  Apabila Nef(tujuan) berada di kuadran I
+//   atau x dan z positif maka rum1 dan rum2 berlaku
+//  
+//  rum1(sdt)=rum2(sdt)=O1+O2
+//  (dengan O1 : teta1 dan O2 : teta2)
+
+//  Apabila berada di kuadran IV
+//  atau x positif dan z negatif maka rum1 dan rum2 berlaku
+//  rum1(sdt)=rum2(sdt)=O1-O2
+
 //rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0)
 //didapat dari substitusi dua rumus diatas
-double diffe(float sdt,float x, float y, float z, float r1, float r2){
-    double dif_dou = -z+r1*cos(sdt)+r2*cos(rum_tet(sdt,x,y,r1,r2));
+double diffeI(float sdt,float x, float y, float z, float r1, float r2){
+    double dif_dou = (-1)*x_phi(x,y)+r1*sin(sdt)+r2*sin(rum2_tet(sdt,x,y,z,r1,r2));
+    float dif = (float) dif_dou;
+    return dif;
+}
+
+double diffeIV(float sdt,float x, float y, float z, float r1, float r2){
+    double dif_dou = (-1)*z+r1*cos(sdt)+r2*cos(2*sdt-rum1_tet(sdt,x,y,z,r1,r2));
     float dif = (float) dif_dou;
     return dif;
 }
@@ -47,12 +71,12 @@
 int main() {
     float xt=0.00; float yt=0.00; float zt=0.00;    //x, y, dan z tujuan
     float r1=10.00; float r2=18.00;     //panjang potongan lengan dalam cm
-    //------batas servo------
-    float R1=-50.00; float T1=110;  //R : sudut terendah
+    //---------------batas servo------
+    float R1=-50.00; float T1=150;  //R : sudut terendah
     float R2=-40.00; float T2=T1;   //T : sudut tertinggi
     float R3=R1; float T3=140.00;
     float deg=0.00; float rad=0.00;
-    //------batas servo------
+    //---------------batas servo------
     float pi = 3.14159265359;
     float kons = 0.01;  //konstanta efektif inverse (dari percobaan)
     float awal = 0.00;  //sudut awal servo (0)
@@ -66,12 +90,12 @@
     SL3_3.calibrate(0.001,120);
     SL3_base.calibrate(0.001,120);
     
-    while(1){
-        SL3_1.position(((-1)*awal+51)*1.10);    //semua ke sudut 0
-        SL3_2.position((awal-75)*1.05);
-        SL3_3.position((awal-51)*1.14);
-        SL3_base.position(awal);     // B E L U M   D I K A L I B R A S I
-        
+    SL3_1.position(((-1)*awal+51)*1.10);    //semua ke sudut 0
+    SL3_2.position((awal-75)*1.05);
+    SL3_3.position((awal-51)*1.14);
+    SL3_base.position(((-1)*awal+70)*1.15);
+    
+    while(1){        
         pc.printf("Masukkan koordinat x,y dan z berurutan : ");
         pc.scanf("%f",&xt);     //input komponen x
         pc.scanf("%f",&yt);     //input komponen y (rotasi base)
@@ -79,50 +103,69 @@
         
         //Command dibawah dilakukan untuk mencegah pembagian 0 dan 0
         //karena bisa math.error
-        if(xt=0.00){
+        if(xt==0.00){
             xt+=0.1;        
-        }else if(xt=r1+r2){
+        }else if(xt==r1+r2){
             xt-=0.1;
         }
-        if(yt=0.00){
+        if(yt==0.00){
             yt+=0.1;        
-        }else if(yt=r1+r2){
+        }else if(yt==r1+r2){
             yt-=0.1;
         }
-        if(zt=0.00){
+        if(zt==0.00){
             zt+=0.1;        
-        }else if(zt=r1+r2){
+        }else if(zt==r1+r2){
             zt-=0.1;
         }
         
         //-------mulai proses teta1--------
-        deg = R1;   //mulai tes looping dari sudut terendah (R1)
+        if ((xt>0)&&(zt>0)){
+            deg = R1;
+        }else if ((xt>0)&&(zt<0)){
+            deg = awal;
+        }
+           //mulai tes looping dari sudut terendah (R1)
         while (deg<T1) {
-            deg+=0.1;
+            deg+=0.05;
             rad=deg*pi/180;
-            if(kons>abs(diffe(rad,xt,yt,zt,r1,r2))){     //jika diffe mendekati 0 (<0.01) dan bukan nan, 
-                break;                              //maka sudut tersebut memenuhi
+            
+            if ((xt>0)&&(zt>00)){
+                if(kons>abs(diffeI(rad,xt,yt,zt,r1,r2))){     //jika diffeI mendekati 0 (<0.01) dan bukan nan, 
+                    break;                              //maka sudut tersebut memenuhi
+                }
+            }else if ((xt>0)&&(zt<0)){
+                if(kons>abs(diffeIV(rad,xt,yt,zt,r1,r2))){     //jika diffeIV mendekati 0 (<0.01) dan bukan nan, 
+                    break;                              //maka sudut tersebut memenuhi
+                }
             }
         }
         
-        if(deg==110.1){
-            pc.printf("Ada kemungkinan kejadian ini tidak dapat terjadi");   
+        if(deg==T1+0.05){
+            pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi");   
         }
         
         float teta1_rad = rad;          //dalam radian
         teta1 = teta1_rad*180/pi;       //dalam derajat
+        
         //--------mulai proses teta2--------
-        float teta2_rad = rum_tet(teta1_rad,xt,yt,r1,r2)-teta1_rad;
-        teta2 = teta2_rad*180/pi;
+        if((xt>0)&&(yt>0)){
+            float teta2_rad = rum2_tet(teta1_rad,xt,yt,zt,r1,r2)-teta1_rad;
+            teta2 = teta2_rad*180/pi;       
+        }else if((xt>0)&&(yt<0)){
+            float teta2_rad = teta1_rad-rum1_tet(teta1_rad,xt,yt,zt,r1,r2);
+            teta2 = teta2_rad*180/pi;
+        }
+        
         //--------mulai proses phi----------
         phis = phi(xt,yt)*180/pi;
-
-        wait_ms(20);        //perlu kah??
         
         //----Eksekusi gerakan servo-----
-        SL3_base.position(phis);       // B E L U M   D I K A L I B R A S I
+        SL3_base.position(((-1)*phis+70)*1.15);
         SL3_1.position(((-1)*teta1+51)*1.10);   
         SL3_2.position((teta1-75)*1.05);
         SL3_3.position((teta2-51)*1.14);
+        
+        wait_ms(2);        //perlu kah??
     }
-}
\ No newline at end of file
+}