Diff: main.cpp

Revision:

7:cf822f5e7b12

Parent:

6:5632ff2142c0

Child:

8:4a536a26ee07

--- a/main.cpp	Sat Oct 21 06:34:05 2017 +0000
+++ b/main.cpp	Sun Oct 22 03:24:21 2017 +0000
@@ -3,16 +3,13 @@
  * Bismillahirahmanirrahim
  *
  * To do List :
- * - Cari Fungsi Output Open Loop
+ * - Cari konstanta Kanan Kiri
  * - Buat library odometry
+ * - Rapihkan kodingan :)
+ * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :)
  *
- * UPDATED : 18/10
- * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4
- * - Dapet nilai maju Kp = 0.00035, pwm kanan kiri = 0.4
- *
- * Update : 19/10
- * - Inlcude library PID
- * - Lebih rapi
+ * UPDATED : 22/10
+ * - PID maju/mundur enak enak jos
  *
  * Last Edited by : Thrower KRAI
  **/
@@ -36,6 +33,7 @@
 
 //Variabel Program dan Planning
 unsigned short in_program = 1;        //variabel untuk masuk ke looping program
+unsigned short state = 0;
 
 //Primitive Function
 void limitMotor();                    //procedure untuk mengecek limit pwm motor
@@ -81,18 +79,19 @@
 unsigned short ts_base = 12;          //variabel untuk time sampling
 float speed_roty;                     //variabel untuk penambahan pwm PID rotasi y
 float speed_disy;                     //variabel untuk penambahan pwm PID distance y
+float limit_y = 0.5;
 float speed_rotx;                     //variabel untuk penambahan pwm PID rotasi x
 float speed_disx;                     //variabel untuk penambahan pwm PID distance x
 
 //Deklarasi PID
-pidKRAI pidRotY((float) 0.0, (float) 0.0, (float) 0.0, ts_base);   //kp ki kd ts
+pidKRAI pidRotY((float) 0.00334, (float) 0.0, (float) 0.00000, ts_base);   //kp ki kd ts
 pidKRAI pidRotX();         //kp ki kd ts
 
-pidKRAI pidDisY((float)0.0,(float)0.0,(float)0.0,ts_base);         //kp ki kd ts
+pidKRAI pidDisY((float)0.228,(float).005534,(float)0.0,ts_base);           //kp ki kd ts
 pidKRAI pidDisX();         //kp ki kd ts
 
 //Variabel cari jarak
-float target_y[100] = {1100.0, 0.0};          //variabel yang digunakan untuk target jarak y
+float target_y[100] = {-5000.0, -5000.0};          //variabel yang digunakan untuk target jarak y
 float jarak_y;                                //variabel untuk increment jarak y
 
 float target_x[100];                          //variabel yang digunakan untuk target jarak x
@@ -122,30 +121,36 @@
         }/* end of planning program */
 //Looping Program Utama
     while(in_program == 1){
-                current_millis = millis();
-                
-                /* Speed Motor */
-                motor_kanan.speed(pwm_kanan);
-                motor_kiri.speed(pwm_kiri);
-                
-                pc.printf("%.2f \n", delta_kaki);
-                
-                if(current_millis - prev_millis1 >= ts_base){
-                    //pidMotor(1);
-                    pulses_kanan = (float) encMotor_kanan.getPulses();
-                    pulses_kiri = (float) encMotor_kiri.getPulses();
-                    rev_kanan = (float) encMotor_kanan.getRevolutions(); 
-                    rev_kiri = (float) encMotor_kiri.getRevolutions();
+                //current_millis = millis();
+                if((jarak_y <= target_y[state]-(float)10.0)||(jarak_y >= target_y[state]+(float)10.0)){
+                    current_millis = millis();
+                    if(current_millis - prev_millis1 >= 12){
+                        pidMotor(0);
+                        pc.printf("%.2f %.2f\n", jarak_y, delta_kaki);
+                        }
                     
-                    /* masuk ke perhitungan parameter */
-                    jarak_y += getY(rev_kanan, rev_kiri);
-                    delta_kaki = pulses_kanan + pulses_kiri;    
+                    /* Speed Motor */
+                    limitMotor();
+                    motor_kanan.speed(pwm_kanan);
+                    motor_kiri.speed(pwm_kiri);
                     }
-                    
-                if(jarak_y > target_y[0]){
-                    pwm_kiri = 0;
-                    pwm_kanan = 0;
-                    }
+                else{
+                    pwm_kanan = 0.0;
+                    pwm_kiri = 0.0;
+                    motor_kanan.brake(1);
+                    motor_kiri.brake(1);
+                    }    
+            pulses_kanan = (float) encMotor_kanan.getPulses();
+            pulses_kiri = (float) encMotor_kiri.getPulses();
+            rev_kanan = (float) pulses_kanan/540; 
+            rev_kiri = (float) pulses_kiri/540;
+            
+            /* masuk ke perhitungan parameter */
+            jarak_y += getY(rev_kanan, rev_kiri);
+            delta_kaki = pulses_kanan + pulses_kiri;
+            encMotor_kanan.reset();
+            encMotor_kiri.reset();
+            pc.printf("%.2f %.2f\n", jarak_y, delta_kaki);    
             }
         } /* end of main program */
     }
@@ -192,10 +197,10 @@
             jarak_y += getY(rev_kanan, rev_kiri);
             delta_kaki = pulses_kanan + pulses_kiri;
         
-            speed_roty = pidRotY.pidCount(tn_y, delta_kaki);
-            speed_disy = pidDisY.pidCount(target_y[0], jarak_y);
+            speed_roty = pidRotY.pidCount(tn_y, delta_kaki, limit_y);
+            speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_y);
             /* Kondisi untuk menambahkan speed */
-            pwm_kiri = speed_disy + speed_roty;
+            pwm_kiri = -speed_disy + speed_roty;
             pwm_kanan = speed_disy + speed_roty;
             
             prev_millis1 = current_millis;
@@ -203,13 +208,37 @@
             encMotor_kiri.reset();
         
         }
-    else if(x==1){}
+    else if(x==1){
+    /* masuk ke perhitungan ketika sudah pada time sampling */
+            pulses_kanan = (float) encMotor_kanan.getPulses();
+            pulses_kiri = (float) encMotor_kiri.getPulses();
+            pulses_depan = (float) encMotor_depan.getPulses();
+            rev_kanan = (float) pulses_kanan/540; 
+            rev_kiri = (float) pulses_kiri/540;
+            rev_depan = (float) pulses_depan/540;
+            
+            /* masuk ke perhitungan parameter */
+            //jarak_x += getX(rev_kanan+rev_kiri, rev_depan);
+            delta_semua = pulses_kanan + pulses_kiri + pulses_depan;
+        
+            //speed_rotx = pidRotY.pidCount(tn_x, delta_semua);
+            //speed_disx = pidDisY.pidCount(target_y[0], jarak_y);
+            /* Kondisi untuk menambahkan speed */
+            pwm_kiri = speed_disx + speed_rotx;
+            pwm_kanan = speed_disx + speed_rotx;
+            pwm_depan = -(speed_disx + speed_rotx);
+            
+            prev_millis1 = current_millis;
+            encMotor_kanan.reset();
+            encMotor_kiri.reset();
+            encMotor_depan.reset();
+        }
     }/* end of pidMotor */
 
 float getY(float y1, float y2){
 /* fungsi untuk menghitung nilai Y dari roda kanan kiri */
-    float jarak_y1 = (kll_roda*y1)/4;
-    float jarak_y2 = (kll_roda*y2)/4;
+    float jarak_y1 =(float) 1.732*(kll_roda*y1)/2;
+    float jarak_y2 =(float) 1.732*(kll_roda*y2)/2;
 
-    return jarak_y1 - jarak_y2;    
+    return (jarak_y1 - jarak_y2)*2;    
     }/* end of getY */