library lengan 3
Revision 0:42f8a5003e37, committed 2019-05-19
- Comitter:
- harrymunli
- Date:
- Sun May 19 15:58:20 2019 +0000
- Commit message:
- +library lengan3
Changed in this revision
Lengan3.cpp | Show annotated file Show diff for this revision Revisions of this file |
Lengan3.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Lengan3.cpp Sun May 19 15:58:20 2019 +0000 @@ -0,0 +1,174 @@ +//Default lengan3(PC_8, PC_6, PA_11, PB_2) + +//Programmed and formulated by Andre +//Convert to library by Harry +/* Nama servo + 4 + 3 + 1(L3) 2 + 0 +*/ + +#include "mbed.h" +#include "Servo.h" +#include "Lengan3.h" + +//SL3_X => lengan3-servoX, contoh SL3_1 => lengan3-servo1 +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) { + 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); + + SL3_1.position((51)*1.10); //semua ke sudut 0 + SL3_2.position((-75)*1.05); + SL3_3.position((-51)*1.14); + SL3_base.position(((-1)+70)*1.15); +} + +//rumus mencari sudut phi +double Lengan3::phi(float x, float y){ + return atan(y/x); +} + +//rumus mencari x yg dipengaruhi phi +double Lengan3::x_phi(float x, float y){ + return x/cos(phi(x,y)); +} + +//rumus teta2+teta1 versi 1 +double Lengan3::rum1_tet(float sdt, float x, float y, float z, float r1, float r2){ + double tet_dou = asin((x_phi(x,y)-r1*sin(sdt))/r2); + float tet = (float) tet_dou; + return tet; +} + +//rumus teta2+teta1 versi 2 +double Lengan3::rum2_tet(float sdt, float x, float y, float z, float r1, float r2){ + double tet_dou = acos((z-r1*cos(sdt))/r2); + float tet = (float) tet_dou; + return tet; +} + +//Penggunaan rumus 1 dan 2 +// Apabila Nef(tujuan) berada di kuadran I +// atau x dan z positif maka rum1 dan rum2 berlaku +// +// rum1(sdt)=rum2(sdt)=O1+O2 +// (dengan O1 : teta1 dan O2 : teta2) + +// Apabila berada di kuadran IV +// atau x positif dan z negatif maka rum1 dan rum2 berlaku +// rum1(sdt)=rum2(sdt)=O1-O2 + +//rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0) +//didapat dari substitusi dua rumus diatas +double Lengan3::diffeI(float sdt,float x, float y, float z, float r1, float r2){ + double dif_dou = (-1)*x_phi(x,y)+r1*sin(sdt)+r2*sin(rum2_tet(sdt,x,y,z,r1,r2)); + float dif = (float) dif_dou; + return dif; +} + +double Lengan3::diffeIV(float sdt,float x, float y, float z, float r1, float r2){ + double dif_dou = (-1)*z+r1*cos(sdt)+r2*cos(2*sdt-rum1_tet(sdt,x,y,z,r1,r2)); + float dif = (float) dif_dou; + return dif; +} + +//input : x,y,z +//output : lengan 3 bergerak ke koordinat input +void Lengan3::invKinMain(float xt=0.00, float yt=0.00, float zt=0.00 ){ + if (xt<=0 or zt==0){ + //error penanganan kondisi awal + //pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi (bukan kuadran I atau IV"); + }else{ + float r1=10.00; float r2=18.00; //panjang potongan lengan dalam cm + //---------------batas servo------ + float R1=-50.00; float T1=150; //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.002; //konstanta efektif inverse (dari simulasi python) + float teta1=0.00; //sudut servo 1 dan 2 (dlm derajat) + float teta2=0.00; //sudut servo 3 (dlm derajat) + float phis=0.00; //sudut servo 0 (dlm derajat) + + while(1){ + /*pc.printf("Masukkan koordinat x,y dan z berurutan : "); + 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.000001; + }else if(xt==r1+r2){ + xt-=0.000001; + } + if(yt==0.00){ + yt+=0.000001; + }else if(yt==r1+r2){ + yt-=0.000001; + } + if(zt==0.00){ + zt+=0.000001; + }else if(zt==r1+r2){ + zt-=0.000001; + } + + //-------mulai proses teta1-------- + if ((xt>0)&&(zt>0)){ + deg = R1; + }else if ((xt>0)&&(zt<0)){ + deg = 0; + } + //mulai tes looping dari sudut terendah (R1) + while (deg<T1) { + deg+=0.03; + rad=deg*pi/180; + + if ((xt>0)&&(zt>0)){ + if(kons>abs(diffeI(rad,xt,yt,zt,r1,r2))){ //jika diffeI mendekati 0 (<0.01) dan bukan nan, + break; //maka sudut tersebut memenuhi + } + }else if ((xt>0)&&(zt<0)){ + if(kons>abs(diffeIV(rad,xt,yt,zt,r1,r2))){ //jika diffeIV mendekati 0 (<0.01) dan bukan nan, + break; //maka sudut tersebut memenuhi + } + } + } + + if(deg>=T1){ + //error + //pc.printf("Ada kemungkinan koordinat tidak dapat dijangkau ini tidak dapat terjadi"); + }else{ + + float teta1_rad = rad; //dalam radian + teta1 = teta1_rad*180/pi; //dalam derajat + + //--------mulai proses teta2-------- + if((xt>0)&&(zt>0)){ + float teta2_rad = rum2_tet(teta1_rad,xt,yt,zt,r1,r2)-teta1_rad; + teta2 = teta2_rad*180/pi; + }else if((xt>0)&&(zt<0)){ + float teta2_rad = teta1_rad-rum1_tet(teta1_rad,xt,yt,zt,r1,r2); + teta2 = teta2_rad*180/pi; + } + + //--------mulai proses phi---------- + phis = phi(xt,yt)*180/pi; + + //----Eksekusi gerakan servo----- + SL3_base.position(((-1)*phis+70)*1.15); + SL3_1.position(((-1)*teta1+51)*1.10); + SL3_2.position((teta1-75)*1.05); + SL3_3.position((teta2-51)*1.14); + + wait_ms(2); //perlu kah?? + } + } + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Lengan3.h Sun May 19 15:58:20 2019 +0000 @@ -0,0 +1,61 @@ +#ifndef MBED_LENGAN3_H +#define MBED_LENGAN3_H + +#include "mbed.h" +#include "Servo.h" + +//Programmed and formulated by Andre +/* Nama servo + 4 + 3 + 1(L3) 2 + 0 +*/ + +class Lengan3 { +protected : + Servo SL3_1; //lengan3-ser1 + Servo SL3_2; //lengan3-ser2 + Servo SL3_3; //lengan3-ser3 + Servo SL3_base; //lengan3-servo base + +public : + + //SL3_1 => lengan3-ser1 + Lengan3(PinName pin_SL3_1, PinName pin_SL3_2, PinName pin_SL3_3, PinName pin_SL3_base); + + //rumus mencari sudut phi + double phi(float x, float y); + + //rumus mencari x yg dipengaruhi phi + double x_phi(float x, float y); + + //rumus teta2+teta1 versi 1 + double rum1_tet(float sdt, float x, float y, float z, float r1, float r2); + + //rumus teta2+teta1 versi 2 + double rum2_tet(float sdt, float x, float y, float z, float r1, float r2); + + //Penggunaan rumus 1 dan 2 + // Apabila Nef(tujuan) berada di kuadran I + // atau x dan z positif maka rum1 dan rum2 berlaku + // + // rum1(sdt)=rum2(sdt)=O1+O2 + // (dengan O1 : teta1 dan O2 : teta2) + + // Apabila berada di kuadran IV + // atau x positif dan z negatif maka rum1 dan rum2 berlaku + // rum1(sdt)=rum2(sdt)=O1-O2 + + //rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0) + //didapat dari substitusi dua rumus diatas + double diffeI(float sdt,float x, float y, float z, float r1, float r2); + + double diffeIV(float sdt,float x, float y, float z, float r1, float r2); + + //mainnya program andre + void invKinMain(float xt, float yt, float zt); + +}; + +#endif \ No newline at end of file