Diff: main.cpp

Revision:

1:c9f11055fb12

Parent:

0:ac7353383a8e

Child:

2:2f7bed7fb055

--- a/main.cpp	Tue Feb 09 14:15:11 2016 +0000
+++ b/main.cpp	Mon Feb 15 17:36:33 2016 +0000
@@ -20,6 +20,7 @@
 #include "NewTextLCD.h"
 #include "PS_PAD.h"
 #include "PID.h"
+#include "millis.h"
 
 /*********************************************************************************************/
 /**     DEKLARASI INPUT OUTPUT                                                              **/
@@ -34,7 +35,7 @@
 PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss)
 
 // PID sensor garis 
-PID PID(0.432,0.000,0.31,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 gripper(PB_6, PB_8, PB_9);
@@ -73,13 +74,15 @@
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
 /*********************************************************************************************/
-float gMax_speed=0.3; //nilai maksimum kecepatan motor
+float gMax_speed=0.7; //nilai maksimum kecepatan motor
 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
+float gTuning = 0.36;
 
 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
 unsigned char gCase=0;  //variabel keadaan proses
 
 unsigned char i; // variabel iterasi
+int over=0;
 
 
   
@@ -88,7 +91,7 @@
 /*********************************************************************************************/     
 void init_sensor()
 {
-    if(button.read()== 0)
+    if((ps2.read(PS_PAD::PAD_CIRCLE)==1))
     {
         calibrate=0;
     }
@@ -116,7 +119,7 @@
 void PIDrunning()   //menjalankan perintah untuk line follower
 {  
     int pv;
-    int over=0;
+
     float speedR,speedL;
     
     //init_sensor();
@@ -198,7 +201,9 @@
     else if (sensor[6]){
         pv = 0;
     }
-    ///////////////// robot bergerak keluar dari sensor/////////////////////
+    else
+    {
+        ///////////////// robot bergerak keluar dari sensor/////////////////////
         if(over==1){
             /*if(speed_ka > 0){
                 wait_ms(10);
@@ -229,7 +234,7 @@
             motor2.brake(1);
             //wait_ms(100);
         }
-    
+    } 
     PID.setProcessValue(pv);
     PID.setSetPoint(0);
     
@@ -337,6 +342,7 @@
             if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==0))
             {               
                 gCase++;
+                wait_ms(200);
             }
             break;   
         }
@@ -416,8 +422,8 @@
                     if (speed >= gMax_speed)
                         speed = gMax_speed;
                         
-                    motor1.speed(-speed*k*0.7);
-                    motor2.speed(speed*k*0.7);
+                    motor1.speed(speed*k*0.7-gTuning);
+                    motor2.speed(-speed*k*0.7);
                     pc.printf("piv kanan \n");
                 }
                 else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0))
@@ -440,16 +446,19 @@
                 if (gMode==1)
                 {
                        gMode=0;
+                       wait_ms(200);
                 }
                 else
                 {
                     gMode=1;
+                    wait_ms(200);
                 }
             }
         
             if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_SELECT)==0))
             {
                     gCase--;
+                    wait_ms(200);
             }
                 
             break;
@@ -477,10 +486,16 @@
     pc.baud(115200);
     
     //multitasking untuk menampilkan layar
-    timer.attach(&showLCD,0.2);
+    startMillis();
+    uint32_t lastTime = 0;
+    int lcd_refresh_rate = 10;
     
     while(1)
     {  
+        if(millis() - lastTime >= lcd_refresh_rate)
+        {
+            showLCD;
+        }
         init_sensor();
         ps2.poll();    
         running();