Diff: main.cpp

Revision:

33:69d266bc3fe9

Parent:

32:581d4a2373f0

Child:

34:1cfd9b1c7d27

--- a/main.cpp	Mon Feb 13 13:49:50 2017 +0000
+++ b/main.cpp	Tue Feb 14 10:34:39 2017 +0000
@@ -1,7 +1,7 @@
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 13 Februari 2017                          */
+/*                  Last Update : 14 Februari 2017                          */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -62,8 +62,8 @@
 double last_error2 = 0, current_error2, sum_error2 = 0;
 float rpm, target_rpm = 8.0;
 float rpm2, target_rpm2 = 8.0;
-float pwmPowerUp        = 0.5;
-float pwmPowerDown      = -0.5;
+float pwmPowerUp        = 0.70;
+float pwmPowerDown      = -0.80;
 bool henti=false, hentis=false; 
  
 // Variable Bawah
@@ -72,19 +72,19 @@
 float keliling_robot    = PI*D_ROBOT;
 float speedT            = 0.2;
 float vpid              = 0;
-float PIVOT             = 0.4;      // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn           = 0.365;    // Tunning PWM motor Depan
-float tuneBlk           = 0.46;      // Tunning PWM motor belakang
+float PIVOT             = 0.27;      // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn           = 0.35;    // Tunning PWM motor Depan
+float tuneBlk           = 0.3;      // Tunning PWM motor belakang
 
 /* Variabel Encoder Bawah */
 float errTetha, Tetha;    // Variabel yang didapatkan encoder
 
 /* Deklarasi Variable Millis */
-unsigned long int previousMillis = 0;
+unsigned long int previousMillis = 0;   // PID launcher
 unsigned long int currentMillis;
-unsigned long int previousMillis2 = 0;
+unsigned long int previousMillis2 = 0;  // PID launcher
 unsigned long int currentMillis2;
-unsigned long int previousMillis3 = 0;
+unsigned long int previousMillis3 = 0;  // Pneumatik
 
 /* Variabel Stick */
 //Logic untuk masuk aktuator
@@ -102,7 +102,7 @@
 void init_speed();                  // 
 void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
 int case_joystick();                // Mendapatkan case dari joystick
-//void speedKalibrasiMotor();         // Pertambahan target RPM motor atas melalui joystick
+//void speedKalibrasiMotor();       // Pertambahan target RPM motor atas melalui joystick
 void setCenter();                   // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
 float getTetha();                   // Fungsi mendapatkan error Tetha
 
@@ -161,12 +161,12 @@
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         caseJoystick = 3;
-         pc.printf("kanan");
+         //pc.printf("kanan");
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
         caseJoystick = 4;
-         pc.printf("kiri");       
+         //pc.printf("kiri");       
     } 
     else if (joystick.segitiga_click && flagLauncher){
         // Motor Launcher
@@ -221,6 +221,8 @@
 {
     int errorP;
     errorP = getTetha();
+    if (errorP<3.5 && errorP>(-3.5))
+        errorP = 0;
     return (float)Kp*errorP;    
 }
 
@@ -266,15 +268,21 @@
         case (3) : 
         {
             // Kanan
-            motorDpn.speed(-(tuneDpn) + pidBase(0.09,0,0));
-            motorBlk.speed((tuneBlk) + pidBase(0.09,0,0));
+            motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
+            motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
+            //speedDpn = tuneDpn + pidBase(0.009,0,0)
+            //speedBlk = tuneBlk + pidBase(0.009,0,0)
+            //motorDpn.speed(-tuneDpn);
+            //motorBlk.speed(tuneBlk);
             break;
         }
         case (4) : 
         {
             // Kiri
-            motorDpn.speed(tuneDpn + pidBase(0.09,0,0));
-            motorBlk.speed(-tuneBlk + pidBase(0.09,0,0));
+            motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
+            motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
+            //motorDpn.speed(tuneDpn);
+            //motorBlk.speed(-tuneBlk);
             break;
         }
         case (5) :