File content as of revision 0:42f8a5003e37:

//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??
            }
        }
    }   
}