Diff: main.cpp

Revision:

7:68af06a391f0

Parent:

6:e9ef196da46a

Child:

8:069a607ef761

--- a/main.cpp	Tue May 14 15:53:14 2019 +0000
+++ b/main.cpp	Fri May 17 10:03:49 2019 +0000
@@ -1,16 +1,16 @@
 //
 
 //
-#include <stdio.h>
 #include <Motor.h>
 #include <encoderKRAI.h>
 #include <mbed.h>
-#include <stdint.h>
 // declare
-Motor main_motor(PB_13, PB_14, PC_4); // input pin
+Motor main_motor(PB_15, PB_14, PB_13 ); // input pin
 DigitalOut pneu(PC_6);// input pin
-DigitalIn infrared(PC_1); // input pin
-encoderKRAI encoder(PC_10,PC_11,538,encoderKRAI::X4_ENCODING);// input pin
+DigitalOut pneu1(PC_5);// input pin
+DigitalOut pneu2(PC_8);// input pin
+DigitalIn infrared(PB_12, PullUp); // input pin
+encoderKRAI encoder(PB_2,PB_1,538,encoderKRAI::X4_ENCODING);// input pin
 Serial pc(USBTX, USBRX,115200);
 
 int count, count_ball ;
@@ -19,12 +19,7 @@
 bool not_stop ;
 double galat, integral, derivative;
 double last_galat ;
-DigitalOut pneu1(PC_7);
-DigitalOut pneu2(PC_8);
-DigitalOut pneu3(PC_9);
-DigitalOut pneu4(PC_6);
-DigitalOut pneu5(PC_5);
-DigitalOut pneu6(PB_6);
+
 
 void BacaEncoder(){// read encoder
     pulse = (double)encoder.getPulses();
@@ -38,11 +33,11 @@
         galat = theta - total_pulse_in_degree;
         derivative = galat - last_galat;
 
-        pwm =  (0.006*galat) + (3*derivative);
+        pwm =  (0.006*galat);
 
         //limit the power of motor, the max power can be changed
-        if (pwm > 0.9) pwm = 0.9;
-        else if (pwm < -0.9) pwm = -0.9;
+        if (pwm > 0.6) pwm = 0.6;
+        else if (pwm < -0.6) pwm = -0.6;
 }
 
 void MoveMotor(double theta){
@@ -53,58 +48,58 @@
 
     // move motor
     last_galat = theta;
-    while ((not_stop) && theta != 0){
+    while ((not_stop) && theta!=0){
         BacaEncoder();
         //PID(theta);
         galat = theta - total_pulse_in_degree;
         derivative = galat - last_galat;
         integral += galat;
-        pwm =  (0.007*galat) + (3*derivative) + (0.000007*integral) ;
+        pwm =  (0.007*galat)+(derivative*3) ;
 
         //limit the power of motor, the max power can be changed
-        if (pwm > 0.9) pwm = 0.9;
-        else if (pwm < -0.9) pwm = -0.9;
+        if (pwm > 0.6) pwm = 0.6;
+        else if (pwm < -0.6) pwm = -0.6;
         // set speed motor
-        if ((pwm > 0.000001) || (pwm < -0.000001)) main_motor.speed(pwm);
+        if ((pwm > 0.1) || (pwm < -0.1)) main_motor.speed(pwm);
         else {
             main_motor.speed(0);
             main_motor.brake(1);
-            if (fabs(pwm)<0.000001) not_stop = 1;
+            not_stop = 0;
+            break;
         }
+        
         last_galat = galat;
-        pc.printf("%f.2 \n",total_pulse_in_degree);
+        pc.printf("%f.2 \n",pwm);
+        if (fabs(galat) > 150){
+            break;
+        }
     }
 }
 int t1;
 
 int main(){
-    while (1){
-        BacaEncoder();
-        //pc.printf("%f.2 \n",total_pulse_in_degree);
-        MoveMotor(60);
-        pneu1=0;
-        pneu2=0;
-        pneu3=0;
-        pneu4=0;
-        pneu5=0;
-        pneu6=0;
-    }
-    
-    
-    /*while (count_ball<6){
+    encoder.reset();
+    int state = 1;
+    wait(1);
+    count_ball = 0;
+    while (count_ball<6){
+        pc.printf("%d \n",count_ball);
         if (state && !infrared) {
             state = 0 ;
-            pneu = 1;                   // menembakkan bola
+            pneu = 0;                   // menembakkan bola
+            wait(1);
             count_ball++;
-            t1 = us_ticker_read();
-
-            // menggerakkan motor
+                    // menggerakkan motor
             if (count_ball == 2) MoveMotor(60);
             else if (count_ball == 4) MoveMotor(90);
+            wait(1);
+            
 
         } else {
-            pneu = 0;
-            if (us_ticker_read()-t1 >1000 ) state = 1;   // tunggu pneumatik kembali
+            pneu = 1;
+            state = 1;
+            wait(1);
         }
-    }*/
+    
+    }
 }