Diff: main.cpp

Revision:

4:8a50d972066d

Parent:

3:3ee76478b110

Child:

5:dee879b9ae82

--- a/main.cpp	Tue May 14 06:37:33 2019 +0000
+++ b/main.cpp	Tue May 14 08:37:02 2019 +0000
@@ -31,7 +31,7 @@
         galat = theta - total_pulse_in_degree;
         derivative = galat - last_galat;
 
-        pwm =  (kp*galat) + (kd*derivative);
+        pwm =  (0.06*galat) + (3*derivative);
 
         //limit the power of motor, the max power can be changed
         if (pwm > 1) pwm = 1;
@@ -41,16 +41,18 @@
 }
 void MoveMotor(double theta){
     not_stop = 1;
-    last_galat = 0;
+    last_galat = theta;
     total_pulse =0;
 
     // move motor
     last_galat = theta;
+    BacaEncoder();
+    PID(theta);
     while ((not_stop) && theta != 0){
         BacaEncoder();
         PID(theta);
         // set speed motor
-        if (pwm != 0) main_motor.speed(pwm); 
+        if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm); 
         else {
             main_motor.speed(0);
             main_motor.brake(1);
@@ -64,8 +66,11 @@
 int main(){
     count_ball = 0;
     int state = 1;  // posisi awal
-
-    while (count_ball<6){
+    while (count_ball != 2){
+        move_motor(60);
+        count_ball++;   
+    }
+    /*while (count_ball<6){
         if (state && !infrared) {
             state = 0 ;
             pneu = 1;                   // menembakkan bola
@@ -80,5 +85,5 @@
             pneu = 0;
             if (us_ticker_read()-t1 >1000 ) state = 1;   // tunggu pneumatik kembali
         }
-    }
+    }*/
 }