Diff: main.cpp

Revision:

6:f10c0d9f228d

Parent:

5:0b555929c5b2

Child:

7:f139bb3b2401

--- a/main.cpp	Sat Dec 16 10:25:27 2017 +0000
+++ b/main.cpp	Sat Dec 16 11:10:56 2017 +0000
@@ -153,16 +153,22 @@
 
 void PTP_movement(){
     //mapping lokasi
-    float map_x[16]     = {150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0,  0};
-    float map_y[16]     = {  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150,  0};
-    float map_theta[16] = {  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270,360};
-    
+    float map_x[16]     = {  0,150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0};
+    float map_y[16]     = {  0,  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150};
+    //float map_theta[16] = {  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270,360};
+    float thet=0;
+    float thet_prev = 0;
     int i=0;
-    
+
     while(i < 16){
-        i += calculate_PID(&map_x[i],&map_y[i],&map_theta[i],false);
+        thet = thet_prev + 90*(i/4);
+        i += calculate_PID(&map_x[i],&map_y[i],&thet,false);
         
-        if (i == 16)    i = 0;
+        if (i == 16){
+            i = 0;
+            thet_prev = thet;
+        }
+
         Thread::wait(Ts);
     }
 }