Diff: main.cpp

Revision:

6:0e159860e5c6

Parent:

5:34be90fa6d27

Child:

7:4d6a73d924ff

--- a/main.cpp	Fri Feb 19 12:49:34 2016 +0000
+++ b/main.cpp	Fri Feb 19 14:18:01 2016 +0000
@@ -36,7 +36,7 @@
 float max=0.50;
 
 // PID sensor garis 
-//PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
+PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
 
 // Motor(pwm, fwd, rev)
 //Motor motor2(PC_6, PC_8, PC_9);  //gripper
@@ -58,7 +58,7 @@
 Servo servo_gripper(PB_9);
 //Deklarasi Input Limit Switch
 
-/*
+
 // Sensor
 DigitalIn S1(PC_0);
 DigitalIn S2(PC_1);
@@ -74,7 +74,7 @@
 DigitalIn S12(PA_11);
 DigitalIn S13(PA_12);
 DigitalOut calibrate(PA_15);
-*/
+
 
 //DigitalIn button(USER_BUTTON);
 
@@ -112,7 +112,169 @@
 /*********************************************************************************************/
 /**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
 /*********************************************************************************************/     
+void init_sensor()
+{
+    if(ps2.read(PS_PAD::PAD_SQUARE)==1)
+    {
+        calibrate=0;
+    }
+    else
+    {
+            calibrate=1;
+    }
+     
+    sensor[0]=(S1.read()==1);
+    sensor[1]=(S2.read()==1);
+    sensor[2]=(S3.read()==1);
+    sensor[3]=(S4.read()==1);
+    sensor[4]=(S5.read()==1);
+    sensor[5]=(S6.read()==1);
+    sensor[6]=(S7.read()==1);
+    sensor[7]=(S8.read()==1);
+    sensor[8]=(S9.read()==1);
+    sensor[9]=(S10.read()==1);
+    sensor[10]=(S11.read()==1);
+    sensor[11]=(S12.read()==1);
+    sensor[12]=(S13.read()==1);
+}
 
+void PIDrunning()   //menjalankan perintah untuk line follower
+{  
+    int pv;
+    int over=0;
+    float speedR,speedL;
+    
+    //init_sensor();
+    //////////////////logic dari PV (present Value)/////////////////////////
+    if(sensor[0]){
+        pv = -12;
+        over=1;
+    }
+    else if(sensor[12]){
+        pv = 12;
+        over=2;
+    }
+    else if(sensor[0] && sensor[1]){
+        pv = -10;
+    }
+    else if(sensor[11] && sensor[12]){
+        pv = 10;
+    }
+    else if(sensor[1]){
+        pv = -9;
+    }
+    else if(sensor[11]){
+        pv = 9;
+    }
+    else if(sensor[1] && sensor[2]){
+        pv = -8;
+    }
+    else if(sensor[10] && sensor[11]){
+        pv = 8;
+    }
+    else if(sensor[2]){
+        pv = -7;
+    }
+    else if(sensor[10]){
+        pv = 7;
+    }
+    else if(sensor[2] && sensor[3]){
+        pv = -6;
+    }
+    else if(sensor[9] && sensor[10]){
+        pv = 6;
+    }
+    else if(sensor[3]){
+        pv = -5;
+    }
+    else if(sensor[9]){
+        pv = 5;
+    }
+    else if(sensor[3] && sensor[4]){
+        pv = -4;
+    }
+    else if(sensor[8] && sensor[9]){
+        pv = 4;  
+    }
+    else if(sensor[4]){
+        pv = -3;
+    }
+    else if(sensor[8]){
+        pv = 3;
+    }
+    else if(sensor[4] && sensor[5]){
+        pv = -2;
+    }
+    else if(sensor[7] && sensor[8]){
+        pv = 2;
+    }
+    else if(sensor[5]){
+        pv = -1;
+    }  
+     else if(sensor[7]){
+        pv = 1;
+    }
+    else if(sensor[5] && sensor[6]){
+        pv = -0.5;    
+    }
+    else if(sensor[6] && sensor[7]){
+        pv = 0.5;
+    } 
+    else if (sensor[6]){
+        pv = 0;
+    }
+    ///////////////// robot bergerak keluar dari sensor/////////////////////
+        if(over==1){
+            /*if(speed_ka > 0){
+                wait_ms(10);
+                motor2.speed(-speed_ka);
+                wait_ms(10);
+                }
+            else{
+                motor2.speed(speed_ka);
+                }
+            wait_ms(10);*/
+            
+            motor1.brake(1);
+            //wait_ms(100);
+            
+        }
+        else if(over==2){
+            /*if(speed_ki > 0){
+                wait_ms(10);
+                motor1.speed(-speed_ki);
+                wait_ms(10);
+                }
+            else{
+                wait_ms(10);
+                motor1.speed(speed_ki);
+                wait_ms(10);
+                }
+            wait_ms(10);*/
+            motor2.brake(1);
+            //wait_ms(100);
+        }
+    
+    PID.setProcessValue(pv);
+    PID.setSetPoint(0);
+    
+    // memulai perhitungan PID
+
+    speedR = gMax_speed - PID.compute();
+    if(speedR > gMax_speed){
+        speedR = gMax_speed;
+        }
+    else if(speedR < gMin_speed)
+        speedR = gMin_speed;
+    motor2.speed(speedR);
+
+    speedL = gMax_speed + PID.compute();
+    if(speedL > gMax_speed)
+        speedL = gMax_speed;
+    else if(speedL < gMin_speed)
+        speedL = gMin_speed;
+    motor1.speed(speedL);
+}
 
 /*********************************************************************************************/
 /*********************************************************************************************/
@@ -182,6 +344,14 @@
     //inisialisasi joystick
     ps2.init();
     
+    //inisialisasi PID
+    PID.setInputLimits(-15,15);
+    PID.setOutputLimits(-1.0,1.0);
+    PID.setMode(1.0);
+    PID.setBias(0.0);
+    PID.reset();
+    
+    
     //tambahan power
     //ESC edf(PB_9,20); //p wm esc pin PC_7
     Servo myservo(PB_8); //pwm servo pin PA_8
@@ -212,6 +382,16 @@
                     motor2.speed(speed);
                     pc.printf("maju \n");
         }
+        else if((ps2.read(PS_PAD::PAD_X)==1)&&(ps2.read(PS_PAD::PAD_TOP)==1)){
+                    PIDrunning();
+                    pc.printf("PID \t %f \t ",PID.compute());
+                    
+                    for(i=0;i<13;i++)
+                    {
+                        pc.printf("%i \t",sensor[i]);
+                    }
+                    pc.printf("\n");
+        }        
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
             //MOTOR BELAKANG
             speed = gMax_speed;