Revision 3:5f9fbb53c8d5, committed 2016-04-22

Comitter:

Najib_irvani

Date:

Fri Apr 22 23:58:19 2016 +0000

Parent:

2:df6c49846367

Commit message:

tunningLinefollowerversion2

Changed in this revision

diff -r df6c49846367 -r 5f9fbb53c8d5 main.cpp
--- a/main.cpp	Fri Apr 22 08:39:32 2016 +0000
+++ b/main.cpp	Fri Apr 22 23:58:19 2016 +0000
@@ -21,12 +21,12 @@
 //motor (PWM, forward, reverse)
 Motor motor1(PA_8, PB_0, PC_15);
 Motor motor2(PA_11, PA_6, PA_5);
-//Motor gripper(PB_6, PA_7, PB_12);
+//Motor motor4(PB_6, PA_7, PB_12);
 Motor motor3(PA_9, PC_2, PC_3);
-Motor motor4(PB_7, PA_14, PA_15);
+Motor motorS(PB_7, PA_14, PA_15);
 Motor motorC1(PB_9, PA_12, PC_5);
 Motor motorC2(PB_8, PB_1, PA_13);
-Motor motorS(PA_10, PB_5, PB_4);
+Motor motor4(PA_10, PB_5, PB_4);
 
 /* coba coba
 //motor (PWM, forward, reverse)
@@ -53,7 +53,7 @@
 DigitalIn limit4(PC_0 ,PullUp);
 
 //PID line follower(P, I, D, Time Sampling)
-PID PID(0.92,0.000,0.61,0.001);
+PID PID(0.32,0.000,0.1,0.001);
 
 //servo(PWM)
 Servo servoEDF(PC_8);
@@ -79,9 +79,9 @@
 const float ax = 0.0006;
 
 const float tuning1 = 0.0;
-const float tuning2 = 0.16;
-const float tuning3 = 0.03;
-const float tuning4 = 0.22;
+const float tuning2 = 0.04;
+const float tuning3 = 0.0;
+const float tuning4 = 0.04;
 
 
 float vcurr = v0;
@@ -101,7 +101,7 @@
 
 //data sensor garis dan line following
 int datasensor[6];
-int over;
+int g_flag;
 
 ///////
 void aktuator();
@@ -109,6 +109,10 @@
 void init_sensor();
 void line(float speed);
 
+void majulf(float i, float j, float k, float l);
+void kirilf(float e, float f, float g, float h);
+void kananlf(float a, float b, float c, float d);
+
 /*********************************************************************************************/
 /*********************************************************************************************/
 /**     PROGRAM UTAMA                                                                       **/
@@ -147,14 +151,14 @@
     
     //kondisi robot
     bool manual=true; 
-    /*
+    
     while(1)
     {
         ps2.poll();
         init_sensor();
         
         if(ps2.read(PS_PAD::PAD_X)==1){    
-            line(0.3);
+            line(0.35);
         }
         else{
             motor1.brake(1);
@@ -169,7 +173,7 @@
             }
             pc.printf("\n");
 
-    }*/
+    }
     
     while(manual){
         
@@ -181,6 +185,34 @@
         if(limit3==0)    manual = false;
         
     }
+    /*
+    while(1){
+        kananlf(0.1,0.1,0.1,0.1);
+        wait_ms(3000);
+        motor1.brake(1);
+        motor2.brake(1);
+        motor3.brake(1);
+        motor4.brake(1);
+        wait_ms(3000);
+        kirilf(0.1,0.1,0.1,0.1);
+        wait_ms(3000);
+        motor1.brake(1);
+        motor2.brake(1);
+        motor3.brake(1);
+        motor4.brake(1);
+        wait_ms(3000);
+        majulf(0.1,0.1,0.1,0.1);
+        wait_ms(3000);   
+        motor1.brake(1);
+        motor2.brake(1);
+        motor3.brake(1);
+        motor4.brake(1);
+        wait_ms(3000);
+    }*/
+    
+    //bool lf = true;
+    //int v_sensor;
+    
     
     motor1.brake(1);
     motor2.brake(1);
@@ -210,6 +242,7 @@
 /*********************************************************************************************/
 /**     ALGORITMA PROSEDUR DAN FUNGSI                                                       **/
 /*********************************************************************************************/ 
+
 void aktuator(){
     float speed = vcurr;
     
@@ -217,20 +250,20 @@
     
         if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
             //pivot kiri                      
-            motor2.speed((float)0.2*(speed-tuning2));
-            motor4.speed((float)-0.2*(speed-tuning4));
-            motor1.speed((float)0.2*(speed-tuning1));
-            motor3.speed((float)-0.2*(speed-tuning3));
+            motor2.speed((float)0.5*(speed-tuning2));
+            motor4.speed((float)-0.5*(speed-tuning4));
+            motor1.speed((float)0.5*(speed-tuning1));
+            motor3.speed((float)-0.5*(speed-tuning3));
             pc.printf("pivot kiri \n");
             
             vcurr += ax;
         }
         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
             //pivot kanan                      
-            motor2.speed((float)-0.2*(speed-tuning2));
-            motor4.speed((float)0.2*(speed-tuning4));
-            motor1.speed((float)-0.2*(speed-tuning1));
-            motor3.speed((float)0.2*(speed-tuning3));
+            motor2.speed((float)-0.5*(speed-tuning2));
+            motor4.speed((float)0.5*(speed-tuning4));
+            motor1.speed((float)-0.5*(speed-tuning1));
+            motor3.speed((float)0.5*(speed-tuning3));
             pc.printf("pivot kanan \n");
             
             vcurr += ax;
@@ -391,6 +424,7 @@
         }
 }
 
+
 void edf_servo(){
         if(ps2.read(PS_PAD::PAD_X)==1){
             //PWM ++
@@ -457,28 +491,28 @@
             over=2;
         }
         else if(datasensor[0] && datasensor[1]){
-            pv = -4;
+            pv = -3.5;
         }
         else if(datasensor[4] && datasensor[5]){
-            pv = 4;
+            pv = 3.5;
         }
         else if(datasensor[1]){
-            pv = -3;
+            pv = -2;
         }
         else if(datasensor[4]){
-            pv = 3;
+            pv = 2;
         }
         else if(datasensor[1] && datasensor[2]){
-            pv = -2;
+            pv = -1;
         }
         else if(datasensor[3] && datasensor[4]){
-            pv = 2;
+            pv = 1;
         }
         else if(datasensor[2]){
-            pv = -1;
+            pv = -0.5;
         }
         else if(datasensor[3]){
-            pv = 1;
+            pv = 0.5;
         }
         else if(datasensor[2] && datasensor[3]){
             pv = 0;
@@ -487,50 +521,70 @@
         {
             ///////////////// 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);
-                motor4.brake(1);
+                //motor1.brake(1);
+                //motor4.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);*/
-                motor3.brake(1);
-                motor2.brake(1);
+                
+                //motor3.brake(1);
+                //motor2.brake(1);
                 //wait_ms(100);
             }
         } 
+        /*
+        if(pv > 1){
+            //kanan
+            speed1 = -(0.4-tuning1);
+            speed2 = 0;
+            speed3 = 0;
+            speed4 = -(0.5-tuning4);   
+            motor3.brake(1);
+            motor2.brake(1);  
+            motor1.speed(speed1);
+            motor4.speed(speed4);  
+        }
+        else if(pv < -1){
+            //kiri
+            speed1 = 0;
+            speed2 = -(0.4-tuning2);
+            speed3 = -(0.4-tuning3);
+            speed4 = 0;
+            
+            motor3.brake(1);
+            motor2.speed(speed2);  
+            motor1.speed(speed1);
+            motor4.brake(1);
+        }
+        else{
+            //gaspoll
+            speed1 = -(0.7-tuning1);
+            speed2 = -(0.7-tuning2);
+            speed3 = -(0.7-tuning3);
+            speed4 = -(0.7-tuning4);
+            
+            motor3.speed(speed3);
+            motor2.speed(speed2);  
+            motor1.speed(speed1);
+            motor4.speed(speed4);
+        } */
         PID.setProcessValue(pv);
         PID.setSetPoint(0);
         
         // memulai perhitungan PID
+        //speed1 = speed - tuning1 + PID.compute();
+        //speed2 = speed - tuning2 - PID.compute();
+        //speed3 = speed - tuning3 - PID.compute();
+        //speed4 = speed - tuning4 + PID.compute();
         
-         
-        speed2 = speed - 0.0 + PID.compute();
-        speed1 = speed - 0.0 + PID.compute();
-        speed3 = speed - 0.2 - PID.compute();
-        speed4 = speed - 0.3 - PID.compute();
-        
-        if(speed1 > speed)      speed1 = speed;
+        speed1 = -0.74;
+        speed2 = -0.74;
+        speed3 = -0.8;
+        speed4 = -0.8;
+
+        /*if(speed1 > speed)      speed1 = speed;
         else if(speed1 < gMin_speed)    speed1 = gMin_speed;
         
         if(speed2 > speed)     speed2 = speed;
@@ -541,10 +595,15 @@
         
         if(speed4 > speed)      speed4 = speed;
         else if(speed4 < gMin_speed)    speed4 = gMin_speed;
-        
-        motor3.speed(-(speed3));
-        motor2.speed(-(speed2));
+        */
         
-        motor1.speed(-(speed1));
-        motor4.speed(-(speed4));
-}
\ No newline at end of file
+        //motor3.speed(-speed3);
+        //motor2.speed(-speed2);  
+        //motor1.speed(-speed1);
+        //motor4.speed(-speed4);
+        
+        motor3.speed(speed3);
+        motor2.speed(speed2);  
+        motor1.speed(speed1);
+        motor4.speed(speed4);
+}