Diff: main.cpp

Revision:

38:3ef6754bd8d8

Parent:

37:67d54563af90

--- a/main.cpp	Tue Feb 14 17:11:20 2017 +0000
+++ b/main.cpp	Fri Feb 17 13:05:02 2017 +0000
@@ -1,7 +1,7 @@
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 14 Februari 2017                          */
+/*                  Last Update : 18 Februari 2017                          */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -54,18 +54,19 @@
 #define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
 // Variable Atas
-double speed, speed2, maxSpeed = 0.95, minSpeed = 0;
-double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
+double speed, speed2;
+const double maxSpeed = 0.95;
+const double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
 double p,i,d;
 double p2,i2,d2;
 double last_error = 0, current_error, sum_error = 0;
 double last_error2 = 0, current_error2, sum_error2 = 0;
-float rpm, target_rpm = 8.0;
-float rpm2, target_rpm2 = 10.0;
-float maxRPM = 40, minRPM = 0;
-float pwmPowerUp        = 0.75;
-float pwmPowerDown      = -0.80;
-bool henti=false, hentis=false; 
+float rpm, rpm2;
+float target_rpm = 12.0, target_rpm2 = 16.0; 
+const float maxRPM = 27, minRPM = 0; // Limit 25 atau 27
+
+const float pwmPowerUp        = 0.78;
+const float pwmPowerDown      = -0.9;
  
 // Variable Bawah
 float Vt;
@@ -93,7 +94,6 @@
 bool isLauncher = false;
 bool isReload = false;
 bool ReloadOn = false;
-int flagLauncher = 1;
 bool flag_Pneu = false;
 
 /*****************************************************/
@@ -170,26 +170,26 @@
         caseJoystick = 4;
          //pc.printf("kiri");       
     } 
-    else if (joystick.segitiga_click && flagLauncher){
+    else if (joystick.segitiga_click){
         // Motor Launcher
         caseJoystick = 5;
     }
-    else if (joystick.R2_click && (target_rpm2 < 40)){
+    else if (joystick.R2_click){
         // Target Pulse PID ++ Motor Depan
          caseJoystick = 6;
     }  
-    else if (joystick.L2_click && (target_rpm2 > 1)){
+    else if (joystick.L2_click){
         // Target Pulse PID -- Motor Depan
          caseJoystick = 7;
     }
-    else if (joystick.R3_click && (target_rpm < 40 )){
+    /*else if (joystick.R3_click){
         // Target Pulse PID ++ Motor Belakang
          caseJoystick = 8;
     }
-    else if (joystick.L3_click && (target_rpm > 1)){
+    else if (joystick.L3_click){
         // Target Pulse PID -- Motor Belakang
          caseJoystick = 9;
-    }
+    }*/
     else if (joystick.silang_click){
         // Pnemuatik ON
         caseJoystick = 10;
@@ -227,7 +227,7 @@
         errorP = 0;
     return (float)Kp*errorP;    
 }
-
+/*
 void init_speed (){
     if (speed>maxSpeed){
         speed = maxSpeed;
@@ -241,7 +241,7 @@
     if (speed2<minSpeed){
         speed2 = minSpeed;
     }
-}
+}*/
 
 void setCenter(){
 // Fungsi untuk menentukan center dari robot
@@ -249,8 +249,8 @@
 }
 
 void init_rpm (){
-    if (target_rpm>maxRPM){
-        target_rpm = maxRPM;
+    if (target_rpm>maxRPM-2){
+        target_rpm = maxRPM-2;
     }
     if (target_rpm<minRPM){
         target_rpm = minRPM;
@@ -258,8 +258,8 @@
     if (target_rpm2>maxRPM){
         target_rpm2 = maxRPM;
     }
-    if (target_rpm2<minRPM){
-        target_rpm2 = minRPM;
+    if (target_rpm2<minRPM+2){
+        target_rpm2 = minRPM+2;
     }
 }
 
@@ -312,6 +312,7 @@
         {
             // Target Pulse PID ++ Motor Depan
             target_rpm2 = target_rpm2++;
+            target_rpm = target_rpm++;
             init_rpm();
             break;
         }
@@ -319,23 +320,22 @@
         {
             // Target Pulse PID -- Motor Depan
             target_rpm2 = target_rpm2--;
+            target_rpm = target_rpm--;
             init_rpm();
             break;
         }
-        case (8) : 
+        /*case (8) : 
         {
             // Target Pulse PID ++ Motor Belakang=
-            target_rpm = target_rpm++;
-            init_rpm();
+            //init_rpm();
             break;
         }
         case (9) : 
         {
             // Target Pulse PID -- Motor Belakang
-            target_rpm = target_rpm--;
-            init_rpm();
+            //init_rpm();
             break;
-        }
+        }*/
         case (10) : 
         {
             // Pneumatik
@@ -414,33 +414,43 @@
         currentMillis2 = millis();
         
         // PID Launcher Depan
-        if (currentMillis-previousMillis>=10)
+        if (currentMillis-previousMillis>=12.5)
         {
             rpm = (float)encLauncherBlk.getPulses();    
             current_error = target_rpm - rpm;
             sum_error = sum_error + current_error;
             p = current_error*kpA;
-            d = (current_error-last_error)*kdA/10.0;
-            i = sum_error*kiA*10.0;
+            d = (current_error-last_error)*kdA/12.5;
+            i = sum_error*kiA*12.5;
             speed = p + d + i;
-            init_speed();
-            launcherBlk.speed(speed);
+            //init_speed();
+            if(speed > maxSpeed){
+                launcherBlk.speed(maxSpeed);
+            }
+            else {
+                launcherBlk.speed(speed);
+            }
             last_error = current_error;
             encLauncherBlk.reset();
           //pc.printf("%.04lf\n",rpm);
             previousMillis = currentMillis;
         }
-        if (currentMillis2-previousMillis2>=10)
+        if (currentMillis2-previousMillis2>=12.5)
         {
             rpm2 = (float)encLauncherDpn.getPulses();    
             current_error2 = target_rpm2 - rpm2;
             sum_error2 = sum_error2 + current_error2;
             p2 = current_error2*kpA;
-            d2 = (current_error2-last_error2)*kdA/10.0;
-            i2 = sum_error2*kiA*10.0;
+            d2 = (current_error2-last_error2)*kdA/12.5;
+            i2 = sum_error2*kiA*12.5;
             speed2 = p2 + d2 + i2;
-            init_speed();
-            launcherDpn.speed(speed2);
+            //init_speed();
+            if (speed2 > maxSpeed){
+                launcherDpn.speed(maxSpeed);
+            }
+            else{
+                launcherDpn.speed(speed2);
+            }
             last_error2 = current_error2;
             encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
@@ -482,7 +492,7 @@
             aktuator();
             launcher();
             reloader();
-            if ((millis()-previousMillis3 >= 350)&&(flag_Pneu)){
+            if ((millis()-previousMillis3 >= 425)&&(flag_Pneu)){
                 pneumatik = 1;
                 flag_Pneu = false;
             }