library lengan 3
Lengan3.h
- Committer:
- harrymunli
- Date:
- 2019-05-19
- Revision:
- 0:42f8a5003e37
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