KRTMI 2019 / Mbed 2 deprecated Inverse_Kine_Lengan3New

Peningkatan Ketelitian

Dependencies:   mbed Servo

main.cpp@15:9ee2f71056f7, 2019-05-17 (annotated)

Committer:
andremanurung
Date:
Fri May 17 02:55:41 2019 +0000
Revision:
15:9ee2f71056f7
Parent:
14:ac60009841bd 
baru

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 12:6db54df00e14 29 //rumus teta2+teta1 versi 1
andremanurung 12:6db54df00e14 30 double rum1_tet(float sdt, float x, float y, float z, 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 12:6db54df00e14 36 //rumus teta2+teta1 versi 2
andremanurung 12:6db54df00e14 37 double rum2_tet(float sdt, float x, float y, float z, float r1, float r2){
andremanurung 12:6db54df00e14 38 double tet_dou = acos((z-r1*cos(sdt))/r2);
andremanurung 12:6db54df00e14 39 float tet = (float) tet_dou;
andremanurung 12:6db54df00e14 40 return tet;
andremanurung 12:6db54df00e14 41 }
andremanurung 12:6db54df00e14 42
andremanurung 12:6db54df00e14 43 //Penggunaan rumus 1 dan 2
andremanurung 12:6db54df00e14 44 // Apabila Nef(tujuan) berada di kuadran I
andremanurung 12:6db54df00e14 45 // atau x dan z positif maka rum1 dan rum2 berlaku
andremanurung 12:6db54df00e14 46 //
andremanurung 12:6db54df00e14 47 // rum1(sdt)=rum2(sdt)=O1+O2
andremanurung 12:6db54df00e14 48 // (dengan O1 : teta1 dan O2 : teta2)
andremanurung 12:6db54df00e14 49
andremanurung 12:6db54df00e14 50 // Apabila berada di kuadran IV
andremanurung 12:6db54df00e14 51 // atau x positif dan z negatif maka rum1 dan rum2 berlaku
andremanurung 12:6db54df00e14 52 // rum1(sdt)=rum2(sdt)=O1-O2
andremanurung 12:6db54df00e14 53
andremanurung 7:32fab84c1877 54 //rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0)
andremanurung 7:32fab84c1877 55 //didapat dari substitusi dua rumus diatas
andremanurung 12:6db54df00e14 56 double diffeI(float sdt,float x, float y, float z, float r1, float r2){
andremanurung 12:6db54df00e14 57 double dif_dou = (-1)*x_phi(x,y)+r1*sin(sdt)+r2*sin(rum2_tet(sdt,x,y,z,r1,r2));
andremanurung 12:6db54df00e14 58 float dif = (float) dif_dou;
andremanurung 12:6db54df00e14 59 return dif;
andremanurung 12:6db54df00e14 60 }
andremanurung 12:6db54df00e14 61
andremanurung 12:6db54df00e14 62 double diffeIV(float sdt,float x, float y, float z, float r1, float r2){
andremanurung 12:6db54df00e14 63 double dif_dou = (-1)*z+r1*cos(sdt)+r2*cos(2*sdt-rum1_tet(sdt,x,y,z,r1,r2));
andremanurung 7:32fab84c1877 64 float dif = (float) dif_dou;
andremanurung 7:32fab84c1877 65 return dif;
andremanurung 7:32fab84c1877 66 }
andremanurung 7:32fab84c1877 67
andremanurung 7:32fab84c1877 68 //--------------------------------------------------------------------
andremanurung 7:32fab84c1877 69 //--------------------------------------------------------------------
andremanurung 7:32fab84c1877 70
harrymunli 0:9dfae2e18414 71 int main() {
andremanurung 7:32fab84c1877 72 float xt=0.00; float yt=0.00; float zt=0.00; //x, y, dan z tujuan
andremanurung 7:32fab84c1877 73 float r1=10.00; float r2=18.00; //panjang potongan lengan dalam cm
andremanurung 12:6db54df00e14 74 //---------------batas servo------
andremanurung 12:6db54df00e14 75 float R1=-50.00; float T1=150; //R : sudut terendah
andremanurung 7:32fab84c1877 76 float R2=-40.00; float T2=T1; //T : sudut tertinggi
andremanurung 7:32fab84c1877 77 float R3=R1; float T3=140.00;
andremanurung 7:32fab84c1877 78 float deg=0.00; float rad=0.00;
andremanurung 12:6db54df00e14 79 //---------------batas servo------
andremanurung 7:32fab84c1877 80 float pi = 3.14159265359;
andremanurung 14:ac60009841bd 81 float kons = 0.002; //konstanta efektif inverse (dari simulasi python)
andremanurung 7:32fab84c1877 82 float awal = 0.00; //sudut awal servo (0)
andremanurung 8:cd8edcd62c54 83 float teta1=0.00; //sudut servo 1 dan 2 (dlm derajat)
andremanurung 8:cd8edcd62c54 84 float teta2=0.00; //sudut servo 3 (dlm derajat)
andremanurung 7:32fab84c1877 85 float phis=0.00; //sudut servo 0 (dlm derajat)
harrymunli 0:9dfae2e18414 86
andremanurung 7:32fab84c1877 87 pc.baud(9600);
andremanurung 7:32fab84c1877 88 SL3_1.calibrate(0.001,120); //harus kalibrasi (0.001,120)
andremanurung 7:32fab84c1877 89 SL3_2.calibrate(0.001,120);
andremanurung 7:32fab84c1877 90 SL3_3.calibrate(0.001,120);
andremanurung 7:32fab84c1877 91 SL3_base.calibrate(0.001,120);
andremanurung 7:32fab84c1877 92
andremanurung 12:6db54df00e14 93 SL3_1.position(((-1)*awal+51)*1.10); //semua ke sudut 0
andremanurung 12:6db54df00e14 94 SL3_2.position((awal-75)*1.05);
andremanurung 12:6db54df00e14 95 SL3_3.position((awal-51)*1.14);
andremanurung 12:6db54df00e14 96 SL3_base.position(((-1)*awal+70)*1.15);
andremanurung 12:6db54df00e14 97
andremanurung 12:6db54df00e14 98 while(1){
andremanurung 9:cc9c6244cb76 99 pc.printf("Masukkan koordinat x,y dan z berurutan : ");
andremanurung 7:32fab84c1877 100 pc.scanf("%f",&xt); //input komponen x
andremanurung 7:32fab84c1877 101 pc.scanf("%f",&yt); //input komponen y (rotasi base)
andremanurung 7:32fab84c1877 102 pc.scanf("%f",&zt); //input komponen z (tinggi)
andremanurung 7:32fab84c1877 103
andremanurung 7:32fab84c1877 104 //Command dibawah dilakukan untuk mencegah pembagian 0 dan 0
andremanurung 7:32fab84c1877 105 //karena bisa math.error
andremanurung 12:6db54df00e14 106 if(xt==0.00){
andremanurung 14:ac60009841bd 107 xt+=0.000001;
andremanurung 12:6db54df00e14 108 }else if(xt==r1+r2){
andremanurung 14:ac60009841bd 109 xt-=0.000001;
andremanurung 7:32fab84c1877 110 }
andremanurung 12:6db54df00e14 111 if(yt==0.00){
andremanurung 14:ac60009841bd 112 yt+=0.000001;
andremanurung 12:6db54df00e14 113 }else if(yt==r1+r2){
andremanurung 15:9ee2f71056f7 114 yt-=0.000001;
andremanurung 7:32fab84c1877 115 }
andremanurung 12:6db54df00e14 116 if(zt==0.00){
andremanurung 14:ac60009841bd 117 zt+=0.000001;
andremanurung 12:6db54df00e14 118 }else if(zt==r1+r2){
andremanurung 14:ac60009841bd 119 zt-=0.000001;
andremanurung 7:32fab84c1877 120 }
D4n1elR 6:38fb058f5827 121
andremanurung 7:32fab84c1877 122 //-------mulai proses teta1--------
andremanurung 12:6db54df00e14 123 if ((xt>0)&&(zt>0)){
andremanurung 12:6db54df00e14 124 deg = R1;
andremanurung 12:6db54df00e14 125 }else if ((xt>0)&&(zt<0)){
andremanurung 12:6db54df00e14 126 deg = awal;
andremanurung 12:6db54df00e14 127 }
andremanurung 12:6db54df00e14 128 //mulai tes looping dari sudut terendah (R1)
andremanurung 7:32fab84c1877 129 while (deg<T1) {
andremanurung 14:ac60009841bd 130 deg+=0.03;
andremanurung 7:32fab84c1877 131 rad=deg*pi/180;
andremanurung 12:6db54df00e14 132
andremanurung 15:9ee2f71056f7 133 if ((xt>0)&&(zt>0)){
andremanurung 12:6db54df00e14 134 if(kons>abs(diffeI(rad,xt,yt,zt,r1,r2))){ //jika diffeI mendekati 0 (<0.01) dan bukan nan,
andremanurung 12:6db54df00e14 135 break; //maka sudut tersebut memenuhi
andremanurung 12:6db54df00e14 136 }
andremanurung 12:6db54df00e14 137 }else if ((xt>0)&&(zt<0)){
andremanurung 12:6db54df00e14 138 if(kons>abs(diffeIV(rad,xt,yt,zt,r1,r2))){ //jika diffeIV mendekati 0 (<0.01) dan bukan nan,
andremanurung 12:6db54df00e14 139 break; //maka sudut tersebut memenuhi
andremanurung 12:6db54df00e14 140 }
andremanurung 15:9ee2f71056f7 141 }else{
andremanurung 15:9ee2f71056f7 142 pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi (bukan kuadran I atau IV");
andremanurung 7:32fab84c1877 143 }
andremanurung 7:32fab84c1877 144 }
andremanurung 11:b84501ec1c12 145
andremanurung 14:ac60009841bd 146 if(deg>=T1){
andremanurung 12:6db54df00e14 147 pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi");
andremanurung 11:b84501ec1c12 148 }
andremanurung 11:b84501ec1c12 149
andremanurung 7:32fab84c1877 150 float teta1_rad = rad; //dalam radian
andremanurung 7:32fab84c1877 151 teta1 = teta1_rad*180/pi; //dalam derajat
andremanurung 12:6db54df00e14 152
andremanurung 7:32fab84c1877 153 //--------mulai proses teta2--------
andremanurung 13:e465ee0ccafb 154 if((xt>0)&&(zt>0)){
andremanurung 12:6db54df00e14 155 float teta2_rad = rum2_tet(teta1_rad,xt,yt,zt,r1,r2)-teta1_rad;
andremanurung 12:6db54df00e14 156 teta2 = teta2_rad*180/pi;
andremanurung 13:e465ee0ccafb 157 }else if((xt>0)&&(zt<0)){
andremanurung 12:6db54df00e14 158 float teta2_rad = teta1_rad-rum1_tet(teta1_rad,xt,yt,zt,r1,r2);
andremanurung 12:6db54df00e14 159 teta2 = teta2_rad*180/pi;
andremanurung 12:6db54df00e14 160 }
andremanurung 12:6db54df00e14 161
andremanurung 7:32fab84c1877 162 //--------mulai proses phi----------
andremanurung 7:32fab84c1877 163 phis = phi(xt,yt)*180/pi;
D4n1elR 4:3db9524b9b63 164
andremanurung 7:32fab84c1877 165 //----Eksekusi gerakan servo-----
andremanurung 12:6db54df00e14 166 SL3_base.position(((-1)*phis+70)*1.15);
andremanurung 7:32fab84c1877 167 SL3_1.position(((-1)*teta1+51)*1.10);
andremanurung 7:32fab84c1877 168 SL3_2.position((teta1-75)*1.05);
andremanurung 7:32fab84c1877 169 SL3_3.position((teta2-51)*1.14);
andremanurung 12:6db54df00e14 170
andremanurung 12:6db54df00e14 171 wait_ms(2); //perlu kah??
harrymunli 1:9284c57e84da 172 }
andremanurung 12:6db54df00e14 173 }