Diff: main.cpp

Revision:

47:6cac4f1a3c8e

Parent:

46:85169ae8659b

Child:

48:3006386dc8df

--- a/main.cpp	Mon Apr 24 14:12:08 2017 +0000
+++ b/main.cpp	Sat May 13 05:47:10 2017 +0000
@@ -1,8 +1,8 @@
-
+/*tuning motor baru untuk konstanta pid baru                                */
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 16 April 2017                          */
+/*                  Last Update : 16 April 2017                             */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -56,9 +56,9 @@
 // Variable Atas 
 // indek 2 untuk motor depan, 1 untuk motor belakang
 double speed, speed2;
-const double maxSpeed = 0.95, minSpeed = 0.0, Ts = 12.5;
+const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5;
 const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
-const double kpA2=0.1293, kdA2=1.007, kiA2=0.0002986;
+const double kpA2=0.13978, kdA2=0.9222, kiA2=0.00038636;
 double a1,b1,c1;
 double a2,b2,c2;
 double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
@@ -67,23 +67,19 @@
 double previous_speed2 = 0;
 
 float rpm, rpm2;
-double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
+double target_rpm = 18.0, target_rpm2 = 18.0; // selisih 4 bagus, sama bagus
 const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27
 
 const float pwmPowerUp        = 1.0;
 const float pwmPowerDown      = -1.0;
 
 double jarak_ping=0;
-double ping_target = 12;
+double ping_target = 14;
 double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.13, ping_Kd =-0.049, ping_Ts=10;
+double ping_Kp = -0.2747, ping_Kd =0, ping_Ts=10;
 double ping_pwm, ping_previous_pwm = 0; 
 
 // Variable Bawah
-float Vt;
-float keliling_enc      = PI*D_ENCODER;
-float keliling_robot    = PI*D_ROBOT;
-float speedT            = 0.2;
 float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
 float tuneDpn           = 1.0;     // Tunning PWM motor Depan
 float tuneBlk           = 1.0;     // Tunning PWM motor belakang
@@ -93,8 +89,8 @@
 float tuneR             = 1.0;
 float tuneL             = 1.0;
 float serong            = 0.4;
-float rasio             = 3.0;
-float t_new             = 0.25;
+float rasio             = 1.4545;
+float t_new             = 0.1;
 
 /* variable tunning */
 float tunning_L;
@@ -102,9 +98,6 @@
 float tunning_Dpn;
 float tunning_Blk;
 
-/* Variabel Encoder Bawah */
-float errTetha, Tetha;    // Variabel yang didapatkan encoder
-
 /* Deklarasi Variable Millis */
 unsigned long int previousMillis = 0;   // PID launcher
 unsigned long int currentMillis;
@@ -121,6 +114,7 @@
 bool isReload = false;
 bool ReloadOn = false;
 bool flag_Pneu = false;
+bool flag_paku = false;
 bool ready = false;
 
 /*****************************************************/
@@ -132,8 +126,6 @@
 void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
 int case_joystick();                // Mendapatkan case dari 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
 
 /* Inisialisasi  Pin TX-RX Joystik dan PC */
 joysticknucleo joystick(PA_0,PA_1);
@@ -154,12 +146,13 @@
 Motor motorR  (PB_8, PC_6, PC_5);   //(PC_6, PB_4, PB_5);
 
 /* Deklarasi Motor Launcher */
-Motor launcherDpn(PA_5,PB_12,PA_11);    // pwm ,fwd, rev
+Motor launcherDpn(PA_5,PA_11,PB_12);    // pwm ,fwd, rev
 Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev 
 Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev                
 
 /* Deklarasi Penumatik Launcher */
 DigitalOut pneumatik(PA_4, PullUp);
+DigitalOut pneu_paku(PC_2, PullUp);
 
 /*Dekalrasi Limit Switch */
 //DigitalIn infraAtas(PC_9, PullUp);
@@ -320,6 +313,10 @@
         // Power Screw Down
         caseJoystick = 12;       
     } 
+    else if (joystick.L3){
+        // Paku Bumi ON/OFF
+        caseJoystick = 34;
+    }
     else
     {
         tuneAksel = 0.6;
@@ -329,28 +326,6 @@
     return(caseJoystick);
 }
 
-float getTetha(){
-// Fungsi untuk mendapatkan nilai tetha
-    float busur, tetha;
-    busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
-    tetha = busur/keliling_robot*360;
-    
-    return -(tetha);    
-}
-
-float pidBase(float Kp, float Ki, float Kd)
-{
-    int errorP;
-    errorP = getTetha();
-    if (errorP<3.5 && errorP>(-3.5))
-        errorP = 0;
-    return (float)Kp*errorP;    
-}
-
-void setCenter(){
-// Fungsi untuk menentukan center dari robot
-    encoderBase.reset();
-}
 
 void init_rpm (){
     if (target_rpm>maxRPM-2){
@@ -375,8 +350,8 @@
             // Pivot Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT);
-            motorL.speed(-rasio*PIVOT);
+            motorR.speed(-rasio*PIVOT-t_new);
+            motorL.speed(-rasio*PIVOT-t_new);
             break;
         }
         case (2): 
@@ -384,8 +359,8 @@
             // Pivot Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT);
-            motorL.speed(rasio*PIVOT);
+            motorR.speed(rasio*PIVOT+t_new);
+            motorL.speed(rasio*PIVOT+t_new);
             break;
         }
         case (17): 
@@ -393,8 +368,8 @@
             // Kanan + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT);
-            motorL.speed(-rasio*PIVOT);
+            motorR.speed(-rasio*PIVOT-t_new);
+            motorL.speed(-rasio*PIVOT-t_new);
             break;
         }
         case (18): 
@@ -402,8 +377,8 @@
             // Kanan + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT);
-            motorL.speed(rasio*PIVOT);
+            motorR.speed(rasio*PIVOT+t_new);
+            motorL.speed(rasio*PIVOT+t_new);
             break;
         }
         case (19): 
@@ -411,8 +386,8 @@
             // Kiri + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT);
-            motorL.speed(-rasio*PIVOT);
+            motorR.speed(-rasio*PIVOT-t_new);
+            motorL.speed(-rasio*PIVOT-t_new);
             break;
         }
         case (20): 
@@ -420,8 +395,8 @@
             // Kiri + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT);
-            motorL.speed(rasio*PIVOT);
+            motorR.speed(rasio*PIVOT+t_new);
+            motorL.speed(rasio*PIVOT+t_new);
             break;
         }
         case (21): 
@@ -429,8 +404,8 @@
             // Maju + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT);
-            motorL.speed(-rasio*PIVOT);
+            motorR.speed(-rasio*PIVOT-t_new);
+            motorL.speed(-rasio*PIVOT-t_new);
             break;
         }
         case (22): 
@@ -438,8 +413,8 @@
             // Maju + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT);
-            motorL.speed(rasio*PIVOT);
+            motorR.speed(rasio*PIVOT+t_new);
+            motorL.speed(rasio*PIVOT+t_new);
             break;
         }
         case (23): 
@@ -447,8 +422,8 @@
             // Mundur + Rotasi Kanan
             motorDpn.speed(-PIVOT);
             motorBlk.speed(-PIVOT);
-            motorR.speed(-rasio*PIVOT);
-            motorL.speed(-rasio*PIVOT);
+            motorR.speed(-rasio*PIVOT-t_new);
+            motorL.speed(-rasio*PIVOT-t_new);
             break;
         }
         case (24): 
@@ -456,8 +431,8 @@
             // Mundur + Rotasi Kiri
             motorDpn.speed(PIVOT);
             motorBlk.speed(PIVOT);
-            motorR.speed(rasio*PIVOT);
-            motorL.speed(rasio*PIVOT);
+            motorR.speed(rasio*PIVOT+t_new);
+            motorL.speed(rasio*PIVOT+t_new);
             break;
         }
         case (25): 
@@ -569,8 +544,8 @@
             if (aksel>300)
                 tuneAksel = 0.9;
             
-            motorR.speed(tuneAksel+t_new);
-            motorL.speed(-tuneAksel-t_new);
+            motorR.speed(tuneAksel);
+            motorL.speed(-tuneAksel);
             motorDpn.brake(1);
             motorBlk.brake(1);
             break;
@@ -582,8 +557,8 @@
             if (aksel>300)
                 tuneAksel = 0.9;
             
-            motorR.speed(-tuneAksel-t_new);
-            motorL.speed(tuneAksel+t_new);
+            motorR.speed(-tuneAksel);
+            motorL.speed(tuneAksel);
             motorDpn.brake(1);
             motorBlk.brake(1);
             break;
@@ -633,8 +608,8 @@
         case (31) : 
         {
             // start
-            target_rpm2 = 22;
-            target_rpm = 22;
+            target_rpm2 = 23;
+            target_rpm = 23;
             init_rpm();
             break;
         }
@@ -649,8 +624,8 @@
         case (33) : 
         {
             // R3
-            target_rpm2 = 17;
-            target_rpm = 17;
+            target_rpm2 = 18;
+            target_rpm = 18;
             init_rpm();
             break;
         }
@@ -661,6 +636,15 @@
             isReload = true;
             break;
         }
+        case (34) :{
+            pneu_paku = !pneu_paku;
+            wait_ms(50);
+            if (pneu_paku == 1){
+                PIVOT = 0.17;
+            }else{
+                PIVOT = 0.8;
+            }
+        }
         default : 
         {
             tuneAksel = 0.6;
@@ -678,6 +662,7 @@
     if(ReloadOn){
         if(isReload){
             powerScrew.speed(pwmPowerDown);
+            pc.printf("%.2f\n", jarak_ping);
             if((!limitBawah)||(!limitBawah1)){
                 isReload = false;
                 ReloadOn = false;
@@ -693,16 +678,12 @@
             ping_sum_error += ping_current_error*ping_Ts;
             ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;   
             
-            if (ping_pwm>1) ping_pwm=1;
-            if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5;
-            if (ping_pwm<-0.049 && ping_pwm>-0.5) ping_pwm = -0.5;
-            if (ping_pwm<-1) ping_pwm=-1;
-            
+            pc.printf("%.2f\n", jarak_ping);
             powerScrew.speed(ping_pwm);
             
             ping_previous_error1 = ping_current_error;
         }
-        if ((jarak_ping>(ping_target-3))&&(jarak_ping<(ping_target+3))){
+        if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
             ready = true;
         }else{
             ready = false;
@@ -772,7 +753,7 @@
             encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
         }
-        pc.printf("%.2f\t%.2f\n",rpm,rpm2);
+        //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
     }
     else
     {
@@ -802,8 +783,6 @@
     // initializing encoder
     pneumatik =1;
     
-    setCenter();
-    
     wait_ms(500);
     
     //initializing PING