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