![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Boleh bisa langsung dicoba, tap servo base belum dikalibrasi
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 +}