Diff: main.cpp

Revision:

10:f0f0dc3904e0

Parent:

9:5a50782510fb

Child:

12:e07c59c28c29

--- a/main.cpp	Tue Nov 29 11:54:22 2016 +0000
+++ b/main.cpp	Tue Nov 29 15:15:28 2016 +0000
@@ -54,7 +54,7 @@
 float speed2=0.6;
 float speed3=0.6;
 float speed4=0.6;
-float speedL=0.2;
+float speedL=0.6;
 
 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
 
@@ -71,18 +71,18 @@
 encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
 
 // Deklarasi variabel motor
-Motor motor1(PB_8, PB_1 , PA_13); // pwm, fwd, rev
-Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
-Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev
-Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev
+Motor motor1(PA_8, PB_0 , PC_15); // pwm, fwd, rev
+Motor motor2(PA_11, PA_6 ,PA_5); // pwm, fwd, rev
+Motor motor3(PA_9, PC_2 , PC_3); // pwm, fwd, rev
+Motor motor4(PA_10, PB_5 ,PB_4); // pwm, fwd, rev
 
 //Motor Atas
-Motor motorld(PA_8, PB_0 ,PC_15); // pwm, fwd, rev
-Motor motorlb(PA_11, PA_5 ,PA_6); // pwm, fwd, rev
+Motor motorld(PB_8, PB_1 , PA_13); // pwm, fwd, rev
+Motor motorlb(PB_9, PA_12, PC_5 ); // pwm, fwd, rev
 
 //Servo Atas
-Servo servoS(pinservo1);
-Servo servoB(pinservo2);
+Servo servoS(PC_9);
+Servo servoB(PC_7);
 
 // Deklarasi variabel posisi robot
 //robotPos posisi();
@@ -233,7 +233,9 @@
 {
     //Servo
     if (ServoGo){
-        servoS.position(-90);
+        servoS.position(10);
+        wait_ms(500);
+        servoS.position(-70);
         wait_ms(500);
         servoS.position(10);
         wait_ms(500);
@@ -288,7 +290,7 @@
            }
     case (3):
         {   
-           YT = YT + 0.01;
+           YT = YT + 0.1;
             
             maju=true;             
             mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
@@ -299,7 +301,7 @@
         }
     case (4):
         { 
-            YT = YT - 0.01;
+            YT = YT - 0.1;
 
             mundur=true; 
             maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
@@ -311,8 +313,8 @@
         }
     case (5) :
         {   
-            XT = XT + 0.01;
-            YT = YT + 0.01;
+            XT = XT + 0.1;
+            YT = YT + 0.1;
             
             saka=true; 
             maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
@@ -324,8 +326,8 @@
         }
     case (6) :
         {   
-             XT = XT + 0.01;
-            YT = YT - 0.01;
+             XT = XT + 0.1;
+            YT = YT - 0.1;
 
             
             sbka=true; 
@@ -338,8 +340,8 @@
         }
     case (7) :
         {   
-         XT = XT - 0.01;
-         YT = YT + 0.01;
+         XT = XT - 0.1;
+         YT = YT + 0.1;
 
             
             saki=true; 
@@ -352,8 +354,8 @@
         }
     case (8) :
         {   
-           XT = XT - 0.01;
-           YT = YT - 0.01;
+           XT = XT - 0.1;
+           YT = YT - 0.1;
          
             pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
             
@@ -362,7 +364,7 @@
         }
     case (9) :
         {   
-            XT = XT + 0.01;
+            XT = XT + 0.1;
             kanan=true; 
             maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
@@ -372,7 +374,7 @@
         }
     case (10) :
         {   
-          XT = XT - 0.01;
+          XT = XT - 0.1;
             
             kiri=true; 
             maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
@@ -385,8 +387,8 @@
     case (11): 
         {          
                      
-         XT = XT + 0.01*x;
-         YT = YT + 0.01*y;
+         XT = XT + 0.1*x;
+         YT = YT + 0.1*y;
                        
             analog=true;
             maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;