Diff: main.cpp

Revision:

6:5632ff2142c0

Parent:

3:4801d83f397c

Child:

7:cf822f5e7b12

--- a/main.cpp	Thu Oct 19 15:55:10 2017 +0000
+++ b/main.cpp	Sat Oct 21 06:34:05 2017 +0000
@@ -2,17 +2,19 @@
  *          THROWER ROBOT
  * Bismillahirahmanirrahim
  *
- * Updated :
- *  -
- *
  * To do List :
- *  -Coba ODOMETRY !!
- *  -Coba baca encoder kanan dan kiri dulu
+ * - Cari Fungsi Output Open Loop
+ * - Buat library odometry
  *
  * UPDATED : 18/10
  * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4
- * - Dapet nilai maju Kp = 0.00035
- * Last Edited by : Calmantara & Robertsen
+ * - Dapet nilai maju Kp = 0.00035, pwm kanan kiri = 0.4
+ *
+ * Update : 19/10
+ * - Inlcude library PID
+ * - Lebih rapi
+ *
+ * Last Edited by : Thrower KRAI
  **/
 
  
@@ -21,6 +23,7 @@
 #include "Motor.h"
 #include "encoderKRAI.h"
 #include "millis.h"
+#include "pidKRAI.h"
 
 /***************************
  *  Konstanta dan Variabel *
@@ -31,9 +34,13 @@
 //Variabel Robot
 float kll_roda = (float) PI*DRODA;  //variabel keliling robot
 
+//Variabel Program dan Planning
+unsigned short in_program = 1;        //variabel untuk masuk ke looping program
+
 //Primitive Function
-void limitMotor();                      //procedure untuk mengecek limit pwm motor
-float getY(float y1, float y2);         //fungsi untuk menghitung jarak Y
+void limitMotor();                    //procedure untuk mengecek limit pwm motor
+void pidMotor(int x);                 //procedure untuk arah gerak motor
+float getY(float y1, float y2);       //fungsi untuk menghitung jarak Y
 
 //Deklarasi Motor
 Motor motor_depan(PB_6, PB_13, PA_10);  //pwm, fwd, rev
@@ -41,44 +48,60 @@
 Motor motor_kiri(PB_9, PA_12, PA_11);   //pwm, fwd, rev
 
 //Variabel Kecepatan
-float pwm_kanan = 0.4;
-float pwm_kiri = -0.4;
-float limit_pwm = 0.8;
+float pwm_kanan;        //pwm motor kanan
+float pwm_kiri;         //pwm motor kiri
+float pwm_depan;        //pwm motor depan
+float limit_pwm = 0.9;  //limit pwm semua motor
 
 //Variabel Serial
 Serial pc(USBTX, USBRX);    //Deklarasi serial pc TX RX
 
 //Deklarasi Rotary Encoder
-encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
-encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
-encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
+encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING);      //chA, chB, revPulse, interruptType
+encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING);    //chA, chB, revPulse, interruptType
+encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING);       //chA, chB, revPulse, interruptType
 
 //Variabel Encoder
 float pulses_depan;    //variabel untuk membaca nilai encoder motor depan
 float pulses_kanan;    //variabel untuk membaca nilai encoder motor kanan
 float pulses_kiri;     //variabel untuk membaca nilai encoder motor kiri
+
 float delta_kaki;      //variabel untuk membaca nilai delta kanan kiri
+float delta_semua;     //variabel untuk membaca nilai delta semua encoder
+
 float rev_depan;       //variabel untuk membaca nilai putaran roda depan
 float rev_kanan;       //variabel untuk membaca nilai putaran roda kanan
-float rev_kiri;       //variabel untuk membaca nilai putaran roda kiri
+float rev_kiri;        //variabel untuk membaca nilai putaran roda kiri
 
 //Target nilai encoder
-float target_depan = 0; //Target untuk jumlah rotary encoder gerakan
+float tn_y = 0;        //target untuk jumlah rotary encoder maju atau mundur
+float tn_x = 0;        //target untuk jumlah rotary encoder kanan atau kiri
 
-//Variabel PID_1
-unsigned long int current_millis;    //variabel untuk mendapatkan millis yang berjalan
-unsigned long int prev_millis1;      //variabel untuk mendapatkan millis sebelumnya
-unsigned short time_sampling = 12;   //variabel untuk time sampling
-float kp_1 = 0.00035;                //variabel konstanta PID
-float current_error1, prev_error1;   //variabel untuk millis PID
-float speed, prev_speed;             //variabel untuk penambahan pwm PID
+//Variabel PID
+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 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 pidRotX();         //kp ki kd ts
+
+pidKRAI pidDisY((float)0.0,(float)0.0,(float)0.0,ts_base);         //kp ki kd ts
+pidKRAI pidDisX();         //kp ki kd ts
 
 //Variabel cari jarak
-float target_jarak = 1100.0;          //variabel yang digunakan untuk jarak odometry
-float jarak_y;                        //variabel untuk increment jarak y
+float target_y[100] = {1100.0, 0.0};          //variabel yang digunakan untuk target jarak y
+float jarak_y;                                //variabel untuk increment jarak y
 
-//Variabel Motor Berhenti
-unsigned long int prev_millis2;      //variabel untuk mendapatkan millis sebelumnya
+float target_x[100];                          //variabel yang digunakan untuk target jarak x
+float jarak_x;                                //variabel untuk increment jarak x
+
+//Variabel Waktu
+unsigned long int current_millis;    //variabel untuk mendapatkan millis yang berjalan
+unsigned long int prev_millis1;      //variabel untuk masuk ke perhitungan PID
+unsigned long int prev_millis2;      //variabel untuk matikan motor
 
 /***************************
  *  Main Function          *
@@ -92,69 +115,101 @@
 //Start Millis
     wait_ms(3000);
     startMillis();
-
-//Looping Program
-    while(1){
-        current_millis = millis();
-        
-        /* Speed Motor */
-        motor_kanan.speed(pwm_kanan);
-        motor_kiri.speed(pwm_kiri);
-        
-        pc.printf("%.2f %.2f %.2f %.2f\n", speed, pulses_kanan, pulses_kiri,jarak_y);
-        
-        if(current_millis - prev_millis1 >= time_sampling){
-        /* masuk ke perhitungan ketika sudah pada time sampling */
-            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;
-            
-            current_error1 = target_depan - delta_kaki;
-            speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1;
-            /* Kondisi untuk menambahkan speed */
-            pwm_kiri = pwm_kiri + speed;
-            pwm_kanan = pwm_kanan + speed;
-            prev_speed = speed;
-            prev_error1 = current_error1;
-            prev_millis1 = current_millis;
-            encMotor_kanan.reset();
-            encMotor_kiri.reset();
+//Looping Utama
+while(1){
+//Looping Program Planning
+    while(in_program == 0){
+        }/* 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();
+                    
+                    /* masuk ke perhitungan parameter */
+                    jarak_y += getY(rev_kanan, rev_kiri);
+                    delta_kaki = pulses_kanan + pulses_kiri;    
+                    }
+                    
+                if(jarak_y > target_y[0]){
+                    pwm_kiri = 0;
+                    pwm_kanan = 0;
+                    }
             }
-            
-        if(jarak_y > target_jarak){
-            pwm_kiri = 0;
-            pwm_kanan = 0;
-            }
+        } /* end of main program */
     }
-} /* end of main fuction */
 
 /************************************
  * Deklarasi Fungsi dan Prosedur    *
  ************************************/
  void limitMotor(){
  /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */    
-     if(pwm_kanan > limit_pwm){
+    if(pwm_kanan > limit_pwm){
             pwm_kanan = limit_pwm;
-            }
-        else if(pwm_kanan < (-limit_pwm)){
-            pwm_kanan = -limit_pwm;
-            } 
-        if(pwm_kiri > limit_pwm){
-            pwm_kiri = limit_pwm;
-            }
-        else if(pwm_kiri < (-limit_pwm)){
-            pwm_kiri = -limit_pwm;
-            } 
+        }
+    else if(pwm_kanan < (-limit_pwm)){
+        pwm_kanan = -limit_pwm;
+        } 
+    if(pwm_kiri > limit_pwm){
+        pwm_kiri = limit_pwm;
+        }
+    else if(pwm_kiri < (-limit_pwm)){
+        pwm_kiri = -limit_pwm;
+        }
+    if(pwm_depan > limit_pwm){
+            pwm_depan = limit_pwm;
+        }
+    else if(pwm_depan < (-limit_pwm)){
+        pwm_depan = -limit_pwm;
+        } 
      }/* end of limitMotor */
 
+void pidMotor(int x){
+/**
+ * prosedur untuk gerak motor
+ * 0 : maju dan mundur
+ * 1 : kanan dan kiri
+ **/
+    if(x==0){
+        /* masuk ke perhitungan ketika sudah pada time sampling */
+            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;
+        
+            speed_roty = pidRotY.pidCount(tn_y, delta_kaki);
+            speed_disy = pidDisY.pidCount(target_y[0], jarak_y);
+            /* Kondisi untuk menambahkan speed */
+            pwm_kiri = speed_disy + speed_roty;
+            pwm_kanan = speed_disy + speed_roty;
+            
+            prev_millis1 = current_millis;
+            encMotor_kanan.reset();
+            encMotor_kiri.reset();
+        
+        }
+    else if(x==1){}
+    }/* 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;
 
     return jarak_y1 - jarak_y2;    
-    }/* end of getY */
\ No newline at end of file
+    }/* end of getY */