Diff: main.cpp

Revision:

1:7533e025de71

Parent:

0:ac8956bbab28

Child:

2:710b092cfcc0

--- a/main.cpp	Thu May 09 10:27:51 2019 +0000
+++ b/main.cpp	Sun May 12 23:45:42 2019 +0000
@@ -14,6 +14,9 @@
 int count, count_ball ;
 double pulse, total_pulse,total_pulse_in_degree,theta,pwm;
 double kp,ki,kd;
+bool not_stop ;
+double error, integral, derivative;
+double last_error ;
 
 void BacaEncoder(){// read encoder
     pulse = (double)encoder.getPulses();
@@ -22,58 +25,58 @@
     total_pulse_in_degree = total_pulse*360/538;
 }
 
-void MoveMotor(int count_ball){
-    bool not_stop = 1;
-    double error, integral, derivative;
-    double last_error = 0;
+void PID(theta){
+        //pid
+        error = theta - total_pulse_in_degree;
+        derivative = error - last_error;
+
+        pwm =  (kp*error) + (kd*derivative);
+
+        //limit the power of motor, the max power can be changed
+        if (pwm > 1) pwm = 1;
+        else if (pwm < -1) pwm = -1;
+
+
+}
+void MoveMotor(theta){
+    not_stop = 1;
+    last_error = 0;
     total_pulse =0;
 
-    // take max theta
-    if (count_ball % 2 == 0){
-        if (count_ball == 2) theta = 60;
-        else if (count_ball == 4) theta = 90;
-        else theta = 0;
-    }
-
     // move motor
+    last_error = theta;
     while ((not_stop) && theta != 0){
         BacaEncoder();
-
-        //pid
-        error = theta - total_pulse_in_degree;
-        integral = integral + error;
-        derivative = error - last_error;
-
-        pwm =  (kp*error) + (ki*integral) + (kd*derivative);
-
-        //limit the power of motor, the max power can be changed
-        if (pwm > 255) pwm = 255;
-        else if (pwm < -255) pwm = -255;
-
+        PID(theta);
         // set speed motor
-        if (pwm > 0) main_motor._fwd(pwm);
-        else if (pwm < 0) main_motor._rev(pwm);
+        if (pwm != 0) main_motor.speed(pwm); 
         else {
             main_motor.speed(0);
             main_motor.brake(1);
             not_stop = 0;
         }
-        last_error = 0;
+        last_error = error;
     }
-
 }
 
 int main(){
-    count_ball = 0 ;
+    count_ball = 0;
+    int state = 1;  // posisi awal
 
-    while (count_ball < 6 ) { // eop if all ball has been throwen
-        if (infrared ){ 
-            pneu = 1;
-            milis(500);
+    while (count_ball<6){
+        if (state && !infrared) {
+            state = 0 ;
+            pneu = 1;                   // menembakkan bola
+            count_ball++;
+            t1 = milis();
+
+            // menggerakkan motor
+            if (count_ball = 2) moveMotor(60);
+            else if (count_ball = 4) moveMotor(90);
+
+        } else {
             pneu = 0;
-            count_ball +=1; // increment of ball
+            if (milis()-t1 >1000 ) state = 1;   // tunggu pneumatik kembali
         }
-        moveMotor(count_ball);
-        milis(500); // adjust with try
     }
-}
\ No newline at end of file
+}