Diff: main.cpp

Revision:

7:32fab84c1877

Parent:

6:38fb058f5827

Child:

8:cd8edcd62c54

--- a/main.cpp	Sun May 12 16:00:19 2019 +0000
+++ b/main.cpp	Tue May 14 13:24:25 2019 +0000
@@ -1,37 +1,123 @@
 #include "mbed.h"
 #include "Servo.h"
-#include "Motor.h"
 
+//Programmed and formulated by Andre
 /* Nama servo
-    4
-    3
-  21 22
-    1 */
+       4
+       3
+  1(L3)   2
+       0
+*/
     
-//PwmOut servo1(PB_7);
-Servo servo1(PB_7);
+Servo SL3_1(PC_8);      //lengan3-ser1
+Servo SL3_2(PC_6);      //lengan3-ser2
+Servo SL3_3(PA_11);     //lengan3-ser3
+Servo SL3_base(PB_2);   //lengan3-servo base
+
 Serial pc(USBTX,USBRX);
 
+//rumus mencari sudut phi
+double phi(float x, float y){
+    return atan(y/x);
+}
+
+//rumus mencari x yg dipengaruhi phi
+double x_phi(float x, float y){
+    return x/cos(phi(x,y));
+}
+
+//rumus teta2+teta1
+double rum_tet(float sdt, float x, float y, float r1, float r2){
+    double tet_dou = asin((x_phi(x,y)-r1*sin(sdt))/r2);
+    float tet = (float) tet_dou;
+    return tet;
+}
+
+//rumus untuk menentukan sudut teta1 terbaik (harus mendekati 0)
+//didapat dari substitusi dua rumus diatas
+double diffe(float sdt,float x, float y, float z, float r1, float r2){
+    double dif_dou = -z+r1*cos(sdt)+r2*cos(rum_tet(sdt,x,y,r1,r2));
+    float dif = (float) dif_dou;
+    return dif;
+}
+
+//--------------------------------------------------------------------
+//--------------------------------------------------------------------
+
 int main() {
-    pc.baud(9600);
-    pc.printf("test begin\n");
+    float xt=0.00; float yt=0.00; float zt=0.00;    //x, y, dan z tujuan
+    float r1=10.00; float r2=18.00;     //panjang potongan lengan dalam cm
+    //------batas servo------
+    float R1=-50.00; float T1=110;  //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.01;  //konstanta efektif inverse (dari percobaan)
+    float awal = 0.00;  //sudut awal servo (0)
+    float teta1=0.00;   //sudut servo 1 (dlm derajat)
+    float teta2=0.00;   //sudut servo 2 (dlm derajat)
+    float phis=0.00;    //sudut servo 0 (dlm derajat)
     
-    //tes
-    servo1.calibrate(0.001,120);
-    //servo2.calibrate(0.001,150);
-    float f=0;
-    //servo2.write(1.0);
+    pc.baud(9600);
+    pc.printf("Masukkan koordinat x,y dan z berurutan : ");
+    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);
+    
     while(1){
-        servo1.position((f-51)*1.14);
-        //servo1.pulsewidth_ms(f);    //pc.printf("input (1..10) bakal dijadiin persen : ");
-        pc.printf("input (1..360) bakal dijadiin derajat : ");
-        pc.scanf("%f",&f);
+        SL3_1.position(((-1)*awal+51)*1.10);    //semua ke sudut 0
+        SL3_2.position((awal-75)*1.05);
+        SL3_3.position((awal-51)*1.14);
+        SL3_base.position(awal);     // B E L U M   D I K A L I B R A S I
+        
+        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.1;        
+        }else if(xt=r1+r2){
+            xt-=0.1;
+        }
+        if(yt=0.00){
+            yt+=0.1;        
+        }else if(yt=r1+r2){
+            yt-=0.1;
+        }
+        if(zt=0.00){
+            zt+=0.1;        
+        }else if(zt=r1+r2){
+            zt-=0.1;
+        }
         
-        //pc.printf("\n");
-//        float f = (float)n/(float)10;
-        wait_ms(20);
-        pc.printf("%f\n",servo1.read());
-        //pc.printf(" \n");
+        //-------mulai proses teta1--------
+        deg = R1;   //mulai tes looping dari sudut terendah (R1)
+        while (deg<T1) {
+            deg+=0.1;
+            rad=deg*pi/180;
+            if(kons>diffe(rad,xt,yt,zt,r1,r2)){     //jika diffe mendekati 0 (<0.01) dan bukan nan, 
+                break;                              //maka sudut tersebut memenuhi
+            }
+        }
+        float teta1_rad = rad;          //dalam radian
+        teta1 = teta1_rad*180/pi;       //dalam derajat
+        //--------mulai proses teta2--------
+        float teta2_rad = rum_tet(teta1_rad,xt,yt,r1,r2)-teta1_rad;
+        teta2 = teta2_rad*180/pi;
+        //--------mulai proses phi----------
+        phis = phi(xt,yt)*180/pi;
+
+        wait_ms(20);        //perlu kah??
         
+        //----Eksekusi gerakan servo-----
+        SL3_base.position(phis);       // B E L U M   D I K A L I B R A S I
+        SL3_1.position(((-1)*teta1+51)*1.10);   
+        SL3_2.position((teta1-75)*1.05);
+        SL3_3.position((teta2-51)*1.14);
     }
-}
+}
\ No newline at end of file