KRTMI 2019
/
Inverse_Kine_Lengan3New
Peningkatan Ketelitian
main.cpp
- Committer:
- andremanurung
- Date:
- 2019-05-14
- Revision:
- 7:32fab84c1877
- Parent:
- 6:38fb058f5827
- Child:
- 8:cd8edcd62c54
File content as of revision 7:32fab84c1877:
#include "mbed.h" #include "Servo.h" //Programmed and formulated by Andre /* Nama servo 4 3 1(L3) 2 0 */ 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() { 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) 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){ 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; } //-------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); } }