Diff: main.cpp

Revision:

9:383084a01131

Parent:

8:069a607ef761

Child:

10:95e41bc4252c

--- a/main.cpp	Sun May 19 17:18:14 2019 +0000
+++ b/main.cpp	Sun May 19 18:54:54 2019 +0000
@@ -54,13 +54,13 @@
         galat = theta - total_pulse_in_degree;
         derivative = galat - last_galat;
         integral += galat;
-        pwm =  (0.007*galat)+(derivative*3) ;
+        pwm =  (0.01*galat)+(derivative) ;
 
         //limit the power of motor, the max power can be changed
-        if (pwm > 0.6) pwm = 0.6;
-        else if (pwm < -0.6) pwm = -0.6;
+        if (pwm > 0.4) pwm = 0.4;
+        else if (pwm < -0.4) pwm = -0.4;
         // set speed motor
-        if ((pwm > 0.1) || (pwm < -0.1)) main_motor.speed(pwm);
+        if (fabs(galat) > 0.6 || (fabs(pwm) > 0.1)) main_motor.speed(pwm);
         else {
             main_motor.speed(0);
             main_motor.brake(1);
@@ -69,7 +69,7 @@
         }
         
         last_galat = galat;
-        pc.printf("%f.2 \n",pwm);
+        pc.printf("%f.2 \n", total_pulse_in_degree);
         if (fabs(total_pulse_in_degree) > 120){
             main_motor.speed(0);
             main_motor.brake(1);            
@@ -90,7 +90,7 @@
     while (count_ball<6){
         pc.printf("%d \n",count_ball);
         if (state && !infrared) {
-            wait(1);
+            wait(1); 
             state = 0 ;
             pneu = 0;                   // menembakkan bola
             wait(1);
@@ -106,6 +106,7 @@
             state = 1;
             wait(1);
         }
+    if (count_ball==6) pneu = 1;
     
     }
 }