KRTMI 2019 / Mbed 2 deprecated Inverse_Kine_Lengan3New

Peningkatan Ketelitian

Dependencies:   mbed Servo

main.cpp@7:32fab84c1877, 2019-05-14 (annotated)

Committer:
andremanurung
Date:
Tue May 14 13:24:25 2019 +0000
Revision:
7:32fab84c1877
Parent:
6:38fb058f5827
Child:
8:cd8edcd62c54
Udah bisa langsung dicoba, tapi yaaa servo base belum di kalibrasi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harrymunli 0:9dfae2e18414 1 #include "mbed.h"
harrymunli 0:9dfae2e18414 2 #include "Servo.h"
harrymunli 0:9dfae2e18414 3
andremanurung 7:32fab84c1877 4 //Programmed and formulated by Andre
harrymunli 0:9dfae2e18414 5 /* Nama servo
andremanurung 7:32fab84c1877 6 4
andremanurung 7:32fab84c1877 7 3
andremanurung 7:32fab84c1877 8 1(L3) 2
andremanurung 7:32fab84c1877 9 0
andremanurung 7:32fab84c1877 10 */
harrymunli 0:9dfae2e18414 11
andremanurung 7:32fab84c1877 12 Servo SL3_1(PC_8); //lengan3-ser1
andremanurung 7:32fab84c1877 13 Servo SL3_2(PC_6); //lengan3-ser2
andremanurung 7:32fab84c1877 14 Servo SL3_3(PA_11); //lengan3-ser3
andremanurung 7:32fab84c1877 15 Servo SL3_base(PB_2); //lengan3-servo base
andremanurung 7:32fab84c1877 16
D4n1elR 4:3db9524b9b63 17 Serial pc(USBTX,USBRX);
harrymunli 0:9dfae2e18414 18
andremanurung 7:32fab84c1877 19 //rumus mencari sudut phi
andremanurung 7:32fab84c1877 20 double phi(float x, float y){
andremanurung 7:32fab84c1877 21 return atan(y/x);
andremanurung 7:32fab84c1877 22 }
andremanurung 7:32fab84c1877 23
andremanurung 7:32fab84c1877 24 //rumus mencari x yg dipengaruhi phi
andremanurung 7:32fab84c1877 25 double x_phi(float x, float y){
andremanurung 7:32fab84c1877 26 return x/cos(phi(x,y));
andremanurung 7:32fab84c1877 27 }
andremanurung 7:32fab84c1877 28
andremanurung 7:32fab84c1877 29 //rumus teta2+teta1
andremanurung 7:32fab84c1877 30 double rum_tet(float sdt, float x, float y, float r1, float r2){
andremanurung 7:32fab84c1877 31 double tet_dou = asin((x_phi(x,y)-r1*sin(sdt))/r2);
andremanurung 7:32fab84c1877 32 float tet = (float) tet_dou;
andremanurung 7:32fab84c1877 33 return tet;
andremanurung 7:32fab84c1877 34 }
andremanurung 7:32fab84c1877 35
andremanurung 7:32fab84c1877 36 //rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0)
andremanurung 7:32fab84c1877 37 //didapat dari substitusi dua rumus diatas
andremanurung 7:32fab84c1877 38 double diffe(float sdt,float x, float y, float z, float r1, float r2){
andremanurung 7:32fab84c1877 39 double dif_dou = -z+r1*cos(sdt)+r2*cos(rum_tet(sdt,x,y,r1,r2));
andremanurung 7:32fab84c1877 40 float dif = (float) dif_dou;
andremanurung 7:32fab84c1877 41 return dif;
andremanurung 7:32fab84c1877 42 }
andremanurung 7:32fab84c1877 43
andremanurung 7:32fab84c1877 44 //--------------------------------------------------------------------
andremanurung 7:32fab84c1877 45 //--------------------------------------------------------------------
andremanurung 7:32fab84c1877 46
harrymunli 0:9dfae2e18414 47 int main() {
andremanurung 7:32fab84c1877 48 float xt=0.00; float yt=0.00; float zt=0.00; //x, y, dan z tujuan
andremanurung 7:32fab84c1877 49 float r1=10.00; float r2=18.00; //panjang potongan lengan dalam cm
andremanurung 7:32fab84c1877 50 //------batas servo------
andremanurung 7:32fab84c1877 51 float R1=-50.00; float T1=110; //R : sudut terendah
andremanurung 7:32fab84c1877 52 float R2=-40.00; float T2=T1; //T : sudut tertinggi
andremanurung 7:32fab84c1877 53 float R3=R1; float T3=140.00;
andremanurung 7:32fab84c1877 54 float deg=0.00; float rad=0.00;
andremanurung 7:32fab84c1877 55 //------batas servo------
andremanurung 7:32fab84c1877 56 float pi = 3.14159265359;
andremanurung 7:32fab84c1877 57 float kons = 0.01; //konstanta efektif inverse (dari percobaan)
andremanurung 7:32fab84c1877 58 float awal = 0.00; //sudut awal servo (0)
andremanurung 7:32fab84c1877 59 float teta1=0.00; //sudut servo 1 (dlm derajat)
andremanurung 7:32fab84c1877 60 float teta2=0.00; //sudut servo 2 (dlm derajat)
andremanurung 7:32fab84c1877 61 float phis=0.00; //sudut servo 0 (dlm derajat)
harrymunli 0:9dfae2e18414 62
andremanurung 7:32fab84c1877 63 pc.baud(9600);
andremanurung 7:32fab84c1877 64 pc.printf("Masukkan koordinat x,y dan z berurutan : ");
andremanurung 7:32fab84c1877 65 SL3_1.calibrate(0.001,120); //harus kalibrasi (0.001,120)
andremanurung 7:32fab84c1877 66 SL3_2.calibrate(0.001,120);
andremanurung 7:32fab84c1877 67 SL3_3.calibrate(0.001,120);
andremanurung 7:32fab84c1877 68 SL3_base.calibrate(0.001,120);
andremanurung 7:32fab84c1877 69
harrymunli 1:9284c57e84da 70 while(1){
andremanurung 7:32fab84c1877 71 SL3_1.position(((-1)*awal+51)*1.10); //semua ke sudut 0
andremanurung 7:32fab84c1877 72 SL3_2.position((awal-75)*1.05);
andremanurung 7:32fab84c1877 73 SL3_3.position((awal-51)*1.14);
andremanurung 7:32fab84c1877 74 SL3_base.position(awal); // B E L U M D I K A L I B R A S I
andremanurung 7:32fab84c1877 75
andremanurung 7:32fab84c1877 76 pc.scanf("%f",&xt); //input komponen x
andremanurung 7:32fab84c1877 77 pc.scanf("%f",&yt); //input komponen y (rotasi base)
andremanurung 7:32fab84c1877 78 pc.scanf("%f",&zt); //input komponen z (tinggi)
andremanurung 7:32fab84c1877 79
andremanurung 7:32fab84c1877 80 //Command dibawah dilakukan untuk mencegah pembagian 0 dan 0
andremanurung 7:32fab84c1877 81 //karena bisa math.error
andremanurung 7:32fab84c1877 82 if(xt=0.00){
andremanurung 7:32fab84c1877 83 xt+=0.1;
andremanurung 7:32fab84c1877 84 }else if(xt=r1+r2){
andremanurung 7:32fab84c1877 85 xt-=0.1;
andremanurung 7:32fab84c1877 86 }
andremanurung 7:32fab84c1877 87 if(yt=0.00){
andremanurung 7:32fab84c1877 88 yt+=0.1;
andremanurung 7:32fab84c1877 89 }else if(yt=r1+r2){
andremanurung 7:32fab84c1877 90 yt-=0.1;
andremanurung 7:32fab84c1877 91 }
andremanurung 7:32fab84c1877 92 if(zt=0.00){
andremanurung 7:32fab84c1877 93 zt+=0.1;
andremanurung 7:32fab84c1877 94 }else if(zt=r1+r2){
andremanurung 7:32fab84c1877 95 zt-=0.1;
andremanurung 7:32fab84c1877 96 }
D4n1elR 6:38fb058f5827 97
andremanurung 7:32fab84c1877 98 //-------mulai proses teta1--------
andremanurung 7:32fab84c1877 99 deg = R1; //mulai tes looping dari sudut terendah (R1)
andremanurung 7:32fab84c1877 100 while (deg<T1) {
andremanurung 7:32fab84c1877 101 deg+=0.1;
andremanurung 7:32fab84c1877 102 rad=deg*pi/180;
andremanurung 7:32fab84c1877 103 if(kons>diffe(rad,xt,yt,zt,r1,r2)){ //jika diffe mendekati 0 (<0.01) dan bukan nan,
andremanurung 7:32fab84c1877 104 break; //maka sudut tersebut memenuhi
andremanurung 7:32fab84c1877 105 }
andremanurung 7:32fab84c1877 106 }
andremanurung 7:32fab84c1877 107 float teta1_rad = rad; //dalam radian
andremanurung 7:32fab84c1877 108 teta1 = teta1_rad*180/pi; //dalam derajat
andremanurung 7:32fab84c1877 109 //--------mulai proses teta2--------
andremanurung 7:32fab84c1877 110 float teta2_rad = rum_tet(teta1_rad,xt,yt,r1,r2)-teta1_rad;
andremanurung 7:32fab84c1877 111 teta2 = teta2_rad*180/pi;
andremanurung 7:32fab84c1877 112 //--------mulai proses phi----------
andremanurung 7:32fab84c1877 113 phis = phi(xt,yt)*180/pi;
andremanurung 7:32fab84c1877 114
andremanurung 7:32fab84c1877 115 wait_ms(20); //perlu kah??
D4n1elR 4:3db9524b9b63 116
andremanurung 7:32fab84c1877 117 //----Eksekusi gerakan servo-----
andremanurung 7:32fab84c1877 118 SL3_base.position(phis); // B E L U M D I K A L I B R A S I
andremanurung 7:32fab84c1877 119 SL3_1.position(((-1)*teta1+51)*1.10);
andremanurung 7:32fab84c1877 120 SL3_2.position((teta1-75)*1.05);
andremanurung 7:32fab84c1877 121 SL3_3.position((teta2-51)*1.14);
harrymunli 1:9284c57e84da 122 }
andremanurung 7:32fab84c1877 123 }