KRTMI 2019
library lengan 3
Diff: Lengan3.cpp
- Revision:
- 0:42f8a5003e37
--- /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