File content as of revision 0:42f8a5003e37:

#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