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