KRTMI 2019 / Lengan3

library lengan 3

Dependents:   KRTMI_v1

Lengan3.cpp@0:42f8a5003e37, 2019-05-19 (annotated)

Committer:
harrymunli
Date:
Sun May 19 15:58:20 2019 +0000
Revision:
0:42f8a5003e37
+library lengan3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harrymunli 0:42f8a5003e37 1 //Default lengan3(PC_8, PC_6, PA_11, PB_2)
harrymunli 0:42f8a5003e37 2
harrymunli 0:42f8a5003e37 3 //Programmed and formulated by Andre
harrymunli 0:42f8a5003e37 4 //Convert to library by Harry
harrymunli 0:42f8a5003e37 5 /* Nama servo
harrymunli 0:42f8a5003e37 6 4
harrymunli 0:42f8a5003e37 7 3
harrymunli 0:42f8a5003e37 8 1(L3) 2
harrymunli 0:42f8a5003e37 9 0
harrymunli 0:42f8a5003e37 10 */
harrymunli 0:42f8a5003e37 11
harrymunli 0:42f8a5003e37 12 #include "mbed.h"
harrymunli 0:42f8a5003e37 13 #include "Servo.h"
harrymunli 0:42f8a5003e37 14 #include "Lengan3.h"
harrymunli 0:42f8a5003e37 15
harrymunli 0:42f8a5003e37 16 //SL3_X => lengan3-servoX, contoh SL3_1 => lengan3-servo1
harrymunli 0:42f8a5003e37 17 Lengan3::Lengan3(PinName pin_SL3_1, PinName pin_SL3_2, PinName pin_SL3_3, PinName pin_SL3_base): SL3_1(pin_SL3_1), SL3_2(pin_SL3_2), SL3_3(pin_SL3_3), SL3_base(pin_SL3_base) {
harrymunli 0:42f8a5003e37 18 SL3_1.calibrate(0.001,120); //harus kalibrasi (0.001,120)
harrymunli 0:42f8a5003e37 19 SL3_2.calibrate(0.001,120);
harrymunli 0:42f8a5003e37 20 SL3_3.calibrate(0.001,120);
harrymunli 0:42f8a5003e37 21 SL3_base.calibrate(0.001,120);
harrymunli 0:42f8a5003e37 22
harrymunli 0:42f8a5003e37 23 SL3_1.position((51)*1.10); //semua ke sudut 0
harrymunli 0:42f8a5003e37 24 SL3_2.position((-75)*1.05);
harrymunli 0:42f8a5003e37 25 SL3_3.position((-51)*1.14);
harrymunli 0:42f8a5003e37 26 SL3_base.position(((-1)+70)*1.15);
harrymunli 0:42f8a5003e37 27 }
harrymunli 0:42f8a5003e37 28
harrymunli 0:42f8a5003e37 29 //rumus mencari sudut phi
harrymunli 0:42f8a5003e37 30 double Lengan3::phi(float x, float y){
harrymunli 0:42f8a5003e37 31 return atan(y/x);
harrymunli 0:42f8a5003e37 32 }
harrymunli 0:42f8a5003e37 33
harrymunli 0:42f8a5003e37 34 //rumus mencari x yg dipengaruhi phi
harrymunli 0:42f8a5003e37 35 double Lengan3::x_phi(float x, float y){
harrymunli 0:42f8a5003e37 36 return x/cos(phi(x,y));
harrymunli 0:42f8a5003e37 37 }
harrymunli 0:42f8a5003e37 38
harrymunli 0:42f8a5003e37 39 //rumus teta2+teta1 versi 1
harrymunli 0:42f8a5003e37 40 double Lengan3::rum1_tet(float sdt, float x, float y, float z, float r1, float r2){
harrymunli 0:42f8a5003e37 41 double tet_dou = asin((x_phi(x,y)-r1*sin(sdt))/r2);
harrymunli 0:42f8a5003e37 42 float tet = (float) tet_dou;
harrymunli 0:42f8a5003e37 43 return tet;
harrymunli 0:42f8a5003e37 44 }
harrymunli 0:42f8a5003e37 45
harrymunli 0:42f8a5003e37 46 //rumus teta2+teta1 versi 2
harrymunli 0:42f8a5003e37 47 double Lengan3::rum2_tet(float sdt, float x, float y, float z, float r1, float r2){
harrymunli 0:42f8a5003e37 48 double tet_dou = acos((z-r1*cos(sdt))/r2);
harrymunli 0:42f8a5003e37 49 float tet = (float) tet_dou;
harrymunli 0:42f8a5003e37 50 return tet;
harrymunli 0:42f8a5003e37 51 }
harrymunli 0:42f8a5003e37 52
harrymunli 0:42f8a5003e37 53 //Penggunaan rumus 1 dan 2
harrymunli 0:42f8a5003e37 54 // Apabila Nef(tujuan) berada di kuadran I
harrymunli 0:42f8a5003e37 55 // atau x dan z positif maka rum1 dan rum2 berlaku
harrymunli 0:42f8a5003e37 56 //
harrymunli 0:42f8a5003e37 57 // rum1(sdt)=rum2(sdt)=O1+O2
harrymunli 0:42f8a5003e37 58 // (dengan O1 : teta1 dan O2 : teta2)
harrymunli 0:42f8a5003e37 59
harrymunli 0:42f8a5003e37 60 // Apabila berada di kuadran IV
harrymunli 0:42f8a5003e37 61 // atau x positif dan z negatif maka rum1 dan rum2 berlaku
harrymunli 0:42f8a5003e37 62 // rum1(sdt)=rum2(sdt)=O1-O2
harrymunli 0:42f8a5003e37 63
harrymunli 0:42f8a5003e37 64 //rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0)
harrymunli 0:42f8a5003e37 65 //didapat dari substitusi dua rumus diatas
harrymunli 0:42f8a5003e37 66 double Lengan3::diffeI(float sdt,float x, float y, float z, float r1, float r2){
harrymunli 0:42f8a5003e37 67 double dif_dou = (-1)*x_phi(x,y)+r1*sin(sdt)+r2*sin(rum2_tet(sdt,x,y,z,r1,r2));
harrymunli 0:42f8a5003e37 68 float dif = (float) dif_dou;
harrymunli 0:42f8a5003e37 69 return dif;
harrymunli 0:42f8a5003e37 70 }
harrymunli 0:42f8a5003e37 71
harrymunli 0:42f8a5003e37 72 double Lengan3::diffeIV(float sdt,float x, float y, float z, float r1, float r2){
harrymunli 0:42f8a5003e37 73 double dif_dou = (-1)*z+r1*cos(sdt)+r2*cos(2*sdt-rum1_tet(sdt,x,y,z,r1,r2));
harrymunli 0:42f8a5003e37 74 float dif = (float) dif_dou;
harrymunli 0:42f8a5003e37 75 return dif;
harrymunli 0:42f8a5003e37 76 }
harrymunli 0:42f8a5003e37 77
harrymunli 0:42f8a5003e37 78 //input : x,y,z
harrymunli 0:42f8a5003e37 79 //output : lengan 3 bergerak ke koordinat input
harrymunli 0:42f8a5003e37 80 void Lengan3::invKinMain(float xt=0.00, float yt=0.00, float zt=0.00 ){
harrymunli 0:42f8a5003e37 81 if (xt<=0 or zt==0){
harrymunli 0:42f8a5003e37 82 //error penanganan kondisi awal
harrymunli 0:42f8a5003e37 83 //pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi (bukan kuadran I atau IV");
harrymunli 0:42f8a5003e37 84 }else{
harrymunli 0:42f8a5003e37 85 float r1=10.00; float r2=18.00; //panjang potongan lengan dalam cm
harrymunli 0:42f8a5003e37 86 //---------------batas servo------
harrymunli 0:42f8a5003e37 87 float R1=-50.00; float T1=150; //R : sudut terendah
harrymunli 0:42f8a5003e37 88 float R2=-40.00; float T2=T1; //T : sudut tertinggi
harrymunli 0:42f8a5003e37 89 float R3=R1; float T3=140.00;
harrymunli 0:42f8a5003e37 90 float deg=0.00; float rad=0.00;
harrymunli 0:42f8a5003e37 91 //---------------batas servo------
harrymunli 0:42f8a5003e37 92 float pi = 3.14159265359;
harrymunli 0:42f8a5003e37 93 float kons = 0.002; //konstanta efektif inverse (dari simulasi python)
harrymunli 0:42f8a5003e37 94 float teta1=0.00; //sudut servo 1 dan 2 (dlm derajat)
harrymunli 0:42f8a5003e37 95 float teta2=0.00; //sudut servo 3 (dlm derajat)
harrymunli 0:42f8a5003e37 96 float phis=0.00; //sudut servo 0 (dlm derajat)
harrymunli 0:42f8a5003e37 97
harrymunli 0:42f8a5003e37 98 while(1){
harrymunli 0:42f8a5003e37 99 /*pc.printf("Masukkan koordinat x,y dan z berurutan : ");
harrymunli 0:42f8a5003e37 100 pc.scanf("%f",&xt); //input komponen x
harrymunli 0:42f8a5003e37 101 pc.scanf("%f",&yt); //input komponen y (rotasi base)
harrymunli 0:42f8a5003e37 102 pc.scanf("%f",&zt); //input komponen z (tinggi)*/
harrymunli 0:42f8a5003e37 103
harrymunli 0:42f8a5003e37 104 //Command dibawah dilakukan untuk mencegah pembagian 0 dan 0
harrymunli 0:42f8a5003e37 105 //karena bisa math.error
harrymunli 0:42f8a5003e37 106 if(xt==0.00){
harrymunli 0:42f8a5003e37 107 xt+=0.000001;
harrymunli 0:42f8a5003e37 108 }else if(xt==r1+r2){
harrymunli 0:42f8a5003e37 109 xt-=0.000001;
harrymunli 0:42f8a5003e37 110 }
harrymunli 0:42f8a5003e37 111 if(yt==0.00){
harrymunli 0:42f8a5003e37 112 yt+=0.000001;
harrymunli 0:42f8a5003e37 113 }else if(yt==r1+r2){
harrymunli 0:42f8a5003e37 114 yt-=0.000001;
harrymunli 0:42f8a5003e37 115 }
harrymunli 0:42f8a5003e37 116 if(zt==0.00){
harrymunli 0:42f8a5003e37 117 zt+=0.000001;
harrymunli 0:42f8a5003e37 118 }else if(zt==r1+r2){
harrymunli 0:42f8a5003e37 119 zt-=0.000001;
harrymunli 0:42f8a5003e37 120 }
harrymunli 0:42f8a5003e37 121
harrymunli 0:42f8a5003e37 122 //-------mulai proses teta1--------
harrymunli 0:42f8a5003e37 123 if ((xt>0)&&(zt>0)){
harrymunli 0:42f8a5003e37 124 deg = R1;
harrymunli 0:42f8a5003e37 125 }else if ((xt>0)&&(zt<0)){
harrymunli 0:42f8a5003e37 126 deg = 0;
harrymunli 0:42f8a5003e37 127 }
harrymunli 0:42f8a5003e37 128 //mulai tes looping dari sudut terendah (R1)
harrymunli 0:42f8a5003e37 129 while (deg<T1) {
harrymunli 0:42f8a5003e37 130 deg+=0.03;
harrymunli 0:42f8a5003e37 131 rad=deg*pi/180;
harrymunli 0:42f8a5003e37 132
harrymunli 0:42f8a5003e37 133 if ((xt>0)&&(zt>0)){
harrymunli 0:42f8a5003e37 134 if(kons>abs(diffeI(rad,xt,yt,zt,r1,r2))){ //jika diffeI mendekati 0 (<0.01) dan bukan nan,
harrymunli 0:42f8a5003e37 135 break; //maka sudut tersebut memenuhi
harrymunli 0:42f8a5003e37 136 }
harrymunli 0:42f8a5003e37 137 }else if ((xt>0)&&(zt<0)){
harrymunli 0:42f8a5003e37 138 if(kons>abs(diffeIV(rad,xt,yt,zt,r1,r2))){ //jika diffeIV mendekati 0 (<0.01) dan bukan nan,
harrymunli 0:42f8a5003e37 139 break; //maka sudut tersebut memenuhi
harrymunli 0:42f8a5003e37 140 }
harrymunli 0:42f8a5003e37 141 }
harrymunli 0:42f8a5003e37 142 }
harrymunli 0:42f8a5003e37 143
harrymunli 0:42f8a5003e37 144 if(deg>=T1){
harrymunli 0:42f8a5003e37 145 //error
harrymunli 0:42f8a5003e37 146 //pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi");
harrymunli 0:42f8a5003e37 147 }else{
harrymunli 0:42f8a5003e37 148
harrymunli 0:42f8a5003e37 149 float teta1_rad = rad; //dalam radian
harrymunli 0:42f8a5003e37 150 teta1 = teta1_rad*180/pi; //dalam derajat
harrymunli 0:42f8a5003e37 151
harrymunli 0:42f8a5003e37 152 //--------mulai proses teta2--------
harrymunli 0:42f8a5003e37 153 if((xt>0)&&(zt>0)){
harrymunli 0:42f8a5003e37 154 float teta2_rad = rum2_tet(teta1_rad,xt,yt,zt,r1,r2)-teta1_rad;
harrymunli 0:42f8a5003e37 155 teta2 = teta2_rad*180/pi;
harrymunli 0:42f8a5003e37 156 }else if((xt>0)&&(zt<0)){
harrymunli 0:42f8a5003e37 157 float teta2_rad = teta1_rad-rum1_tet(teta1_rad,xt,yt,zt,r1,r2);
harrymunli 0:42f8a5003e37 158 teta2 = teta2_rad*180/pi;
harrymunli 0:42f8a5003e37 159 }
harrymunli 0:42f8a5003e37 160
harrymunli 0:42f8a5003e37 161 //--------mulai proses phi----------
harrymunli 0:42f8a5003e37 162 phis = phi(xt,yt)*180/pi;
harrymunli 0:42f8a5003e37 163
harrymunli 0:42f8a5003e37 164 //----Eksekusi gerakan servo-----
harrymunli 0:42f8a5003e37 165 SL3_base.position(((-1)*phis+70)*1.15);
harrymunli 0:42f8a5003e37 166 SL3_1.position(((-1)*teta1+51)*1.10);
harrymunli 0:42f8a5003e37 167 SL3_2.position((teta1-75)*1.05);
harrymunli 0:42f8a5003e37 168 SL3_3.position((teta2-51)*1.14);
harrymunli 0:42f8a5003e37 169
harrymunli 0:42f8a5003e37 170 wait_ms(2); //perlu kah??
harrymunli 0:42f8a5003e37 171 }
harrymunli 0:42f8a5003e37 172 }
harrymunli 0:42f8a5003e37 173 }
harrymunli 0:42f8a5003e37 174 }