Diff: main.cpp

Revision:

5:dee879b9ae82

Parent:

4:8a50d972066d

Child:

6:e9ef196da46a

--- a/main.cpp	Tue May 14 08:37:02 2019 +0000
+++ b/main.cpp	Tue May 14 09:16:38 2019 +0000
@@ -22,7 +22,7 @@
 void BacaEncoder(){// read encoder
     pulse = (double)encoder.getPulses();
     encoder.reset();
-    total_pulse +=pulse;
+    total_pulse += pulse;
     total_pulse_in_degree = total_pulse*360/538;
 }
 
@@ -34,11 +34,10 @@
         pwm =  (0.06*galat) + (3*derivative);
 
         //limit the power of motor, the max power can be changed
-        if (pwm > 1) pwm = 1;
-        else if (pwm < -1) pwm = -1;
+        if (pwm > 0.9) pwm = 0.9;
+        else if (pwm < -0.9) pwm = -0.9;
+}
 
-
-}
 void MoveMotor(double theta){
     not_stop = 1;
     last_galat = theta;
@@ -46,17 +45,15 @@
 
     // move motor
     last_galat = theta;
-    BacaEncoder();
-    PID(theta);
     while ((not_stop) && theta != 0){
         BacaEncoder();
         PID(theta);
         // set speed motor
-        if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm); 
+        if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm);
         else {
             main_motor.speed(0);
             main_motor.brake(1);
-            not_stop = 0;
+            if (fabs(galat)<0.001) not_stop = 1;
         }
         last_galat = galat;
     }
@@ -66,10 +63,11 @@
 int main(){
     count_ball = 0;
     int state = 1;  // posisi awal
-    while (count_ball != 2){
-        move_motor(60);
-        count_ball++;   
+    while (1){
+        main_motor.speed(0.5);
     }
+    
+    
     /*while (count_ball<6){
         if (state && !infrared) {
             state = 0 ;