Diff: main.cpp

Revision:

47:322c5966ee73

Parent:

46:85169ae8659b

Child:

48:5b0d65292fab

--- a/main.cpp	Mon Apr 24 14:12:08 2017 +0000
+++ b/main.cpp	Mon May 08 11:12:04 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 :                                   */
@@ -57,8 +57,8 @@
 // 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 kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
-const double kpA2=0.1293, kdA2=1.007, kiA2=0.0002986;
+const double kpA1=0.1669, kdA1=0.7496, kiA1=0.0006679;
+const double kpA2=0.1505, kdA2=0.9734, kiA2=0.0004086;
 double a1,b1,c1;
 double a2,b2,c2;
 double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
@@ -74,16 +74,12 @@
 const float pwmPowerDown      = -1.0;
 
 double jarak_ping=0;
-double ping_target = 12;
+double ping_target = 13;
 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,7 +89,7 @@
 float tuneR             = 1.0;
 float tuneL             = 1.0;
 float serong            = 0.4;
-float rasio             = 3.0;
+float rasio             = 1.4545;
 float t_new             = 0.25;
 
 /* variable tunning */
@@ -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;
@@ -132,8 +125,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,7 +145,7 @@
 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                
 
@@ -329,28 +320,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){
@@ -569,8 +538,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 +551,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;
@@ -678,6 +647,7 @@
     if(ReloadOn){
         if(isReload){
             powerScrew.speed(pwmPowerDown);
+            pc.printf("%.2f\n", jarak_ping);
             if((!limitBawah)||(!limitBawah1)){
                 isReload = false;
                 ReloadOn = false;
@@ -693,16 +663,16 @@
             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;
-            
+            //if ((ping_pwm<0.5) && (ping_pwm>0.0)) ping_pwm = 0.5;
+            //if ((ping_pwm>-0.5) && (ping_pwm<0.0)) ping_pwm = -0.5;
+            //if (ping_pwm>1) ping_pwm=1;
+            //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 +742,7 @@
             encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
         }
-        pc.printf("%.2f\t%.2f\n",rpm,rpm2);
+        //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
     }
     else
     {
@@ -802,8 +772,6 @@
     // initializing encoder
     pneumatik =1;
     
-    setCenter();
-    
     wait_ms(500);
     
     //initializing PING