Diff: main.cpp

Revision:

8:0711dea61312

Parent:

7:d138c56dab20

--- a/main.cpp	Mon Nov 28 10:31:15 2016 +0000
+++ b/main.cpp	Tue Nov 29 11:51:42 2016 +0000
@@ -43,9 +43,9 @@
 #define diaEncoder 0.058
 #define PPR 1000
 #define diaRobot 0.64
-#define pinservo1 PC_9
+#define pinservo1 PC_7
 //PC 9
-#define pinservo2 PC_8
+#define pinservo2 PA_0
 
 float K_enc = pi*diaEncoder;
 float K_robot = pi*diaRobot;
@@ -233,44 +233,20 @@
 {
     //Servo
     if (ServoGo){
-  //      servoS.position(90);
-    //    servoB.position(-90);
-      /*  if (waktu.read()<=3){
-            pc.printf("Servo samping...");
-            servoS.position(90);
-        }else if ( waktu.read()>8) {
-        // Delay
-        
+        servoS.position(-90);
+        wait_ms(500);
+        servoS.position(10);
+        wait_ms(500);
+        for (int i = 0; i<=90; i++){
+        servoB.position(i);
+        wait_ms(10);
+        }
+        wait_ms(500);
+        servoB.position(0);
         ServoGo = false;
-         } else {
-        // Delay
-        servoB.position(90);
-        pc.printf("Servo belakang...");
-       
-        }        
-        */
-        switch (waktu.read)
-    {
-    case (1):
-            {
-                pc.printf("Servo samping...");
-                servoS.position(90);
-            }    
-    case ()
         
-        
-        
-        
-        
-        
-        
-    }    
-    
-    
-    
-    
     }else{
-        servoS.position(90);
+        servoS.position(10);
         servoB.position(0);
     
     }
@@ -349,7 +325,7 @@
     case (6) :
         {   
              XT = XT + 0.01;
-         YT = YT - 0.01;
+            YT = YT - 0.01;
 
             
             sbka=true; 
@@ -377,7 +353,7 @@
     case (8) :
         {   
            XT = XT - 0.01;
-         YT = YT - 0.01;
+           YT = YT - 0.01;
          
             pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);