KRTMI 2019
/
Inverse_Kine_Lengan3New
Peningkatan Ketelitian
Diff: main.cpp
- Revision:
- 7:32fab84c1877
- Parent:
- 6:38fb058f5827
- Child:
- 8:cd8edcd62c54
--- a/main.cpp Sun May 12 16:00:19 2019 +0000 +++ b/main.cpp Tue May 14 13:24:25 2019 +0000 @@ -1,37 +1,123 @@ #include "mbed.h" #include "Servo.h" -#include "Motor.h" +//Programmed and formulated by Andre /* Nama servo - 4 - 3 - 21 22 - 1 */ + 4 + 3 + 1(L3) 2 + 0 +*/ -//PwmOut servo1(PB_7); -Servo servo1(PB_7); +Servo SL3_1(PC_8); //lengan3-ser1 +Servo SL3_2(PC_6); //lengan3-ser2 +Servo SL3_3(PA_11); //lengan3-ser3 +Servo SL3_base(PB_2); //lengan3-servo base + Serial pc(USBTX,USBRX); +//rumus mencari sudut phi +double phi(float x, float y){ + return atan(y/x); +} + +//rumus mencari x yg dipengaruhi phi +double x_phi(float x, float y){ + return x/cos(phi(x,y)); +} + +//rumus teta2+teta1 +double rum_tet(float sdt, float x, float y, float r1, float r2){ + double tet_dou = asin((x_phi(x,y)-r1*sin(sdt))/r2); + float tet = (float) tet_dou; + return tet; +} + +//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)); + float dif = (float) dif_dou; + return dif; +} + +//-------------------------------------------------------------------- +//-------------------------------------------------------------------- + int main() { - pc.baud(9600); - pc.printf("test begin\n"); + 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 + 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------ + float pi = 3.14159265359; + float kons = 0.01; //konstanta efektif inverse (dari percobaan) + float awal = 0.00; //sudut awal servo (0) + float teta1=0.00; //sudut servo 1 (dlm derajat) + float teta2=0.00; //sudut servo 2 (dlm derajat) + float phis=0.00; //sudut servo 0 (dlm derajat) - //tes - servo1.calibrate(0.001,120); - //servo2.calibrate(0.001,150); - float f=0; - //servo2.write(1.0); + pc.baud(9600); + pc.printf("Masukkan koordinat x,y dan z berurutan : "); + SL3_1.calibrate(0.001,120); //harus kalibrasi (0.001,120) + SL3_2.calibrate(0.001,120); + SL3_3.calibrate(0.001,120); + SL3_base.calibrate(0.001,120); + while(1){ - servo1.position((f-51)*1.14); - //servo1.pulsewidth_ms(f); //pc.printf("input (1..10) bakal dijadiin persen : "); - pc.printf("input (1..360) bakal dijadiin derajat : "); - pc.scanf("%f",&f); + 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 + + pc.scanf("%f",&xt); //input komponen x + pc.scanf("%f",&yt); //input komponen y (rotasi base) + pc.scanf("%f",&zt); //input komponen z (tinggi) + + //Command dibawah dilakukan untuk mencegah pembagian 0 dan 0 + //karena bisa math.error + if(xt=0.00){ + xt+=0.1; + }else if(xt=r1+r2){ + xt-=0.1; + } + if(yt=0.00){ + yt+=0.1; + }else if(yt=r1+r2){ + yt-=0.1; + } + if(zt=0.00){ + zt+=0.1; + }else if(zt=r1+r2){ + zt-=0.1; + } - //pc.printf("\n"); -// float f = (float)n/(float)10; - wait_ms(20); - pc.printf("%f\n",servo1.read()); - //pc.printf(" \n"); + //-------mulai proses teta1-------- + deg = R1; //mulai tes looping dari sudut terendah (R1) + while (deg<T1) { + deg+=0.1; + rad=deg*pi/180; + if(kons>diffe(rad,xt,yt,zt,r1,r2)){ //jika diffe mendekati 0 (<0.01) dan bukan nan, + break; //maka sudut tersebut memenuhi + } + } + 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; + //--------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_1.position(((-1)*teta1+51)*1.10); + SL3_2.position((teta1-75)*1.05); + SL3_3.position((teta2-51)*1.14); } -} +} \ No newline at end of file