Diff: main.cpp

Revision:

6:e9ef196da46a

Parent:

5:dee879b9ae82

Child:

7:68af06a391f0

--- a/main.cpp	Tue May 14 09:16:38 2019 +0000
+++ b/main.cpp	Tue May 14 15:53:14 2019 +0000
@@ -7,10 +7,11 @@
 #include <mbed.h>
 #include <stdint.h>
 // declare
-Motor main_motor(PB_13, PB_14, PB_15 ); // input pin
-DigitalOut pneu(PC_5);// input pin
-DigitalIn infrared(PC_5); // input pin
-encoderKRAI encoder(PB_1,PB_2,538,encoderKRAI::X4_ENCODING);// input pin
+Motor main_motor(PB_13, PB_14, PC_4); // 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
+Serial pc(USBTX, USBRX,115200);
 
 int count, count_ball ;
 double pulse, total_pulse,total_pulse_in_degree,theta,pwm;
@@ -18,6 +19,12 @@
 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();
@@ -31,7 +38,7 @@
         galat = theta - total_pulse_in_degree;
         derivative = galat - last_galat;
 
-        pwm =  (0.06*galat) + (3*derivative);
+        pwm =  (0.006*galat) + (3*derivative);
 
         //limit the power of motor, the max power can be changed
         if (pwm > 0.9) pwm = 0.9;
@@ -42,29 +49,45 @@
     not_stop = 1;
     last_galat = theta;
     total_pulse =0;
+    integral = 0;
 
     // move motor
     last_galat = theta;
     while ((not_stop) && theta != 0){
         BacaEncoder();
-        PID(theta);
+        //PID(theta);
+        galat = theta - total_pulse_in_degree;
+        derivative = galat - last_galat;
+        integral += galat;
+        pwm =  (0.007*galat) + (3*derivative) + (0.000007*integral) ;
+
+        //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;
         // set speed motor
-        if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm);
+        if ((pwm > 0.000001) || (pwm < -0.000001)) main_motor.speed(pwm);
         else {
             main_motor.speed(0);
             main_motor.brake(1);
-            if (fabs(galat)<0.001) not_stop = 1;
+            if (fabs(pwm)<0.000001) not_stop = 1;
         }
         last_galat = galat;
+        pc.printf("%f.2 \n",total_pulse_in_degree);
     }
 }
 int t1;
 
 int main(){
-    count_ball = 0;
-    int state = 1;  // posisi awal
     while (1){
-        main_motor.speed(0.5);
+        BacaEncoder();
+        //pc.printf("%f.2 \n",total_pulse_in_degree);
+        MoveMotor(60);
+        pneu1=0;
+        pneu2=0;
+        pneu3=0;
+        pneu4=0;
+        pneu5=0;
+        pneu6=0;
     }