Diff: main.cpp

Revision:

2:dea57b3f84cd

Parent:

1:5e0cb74f950e

Child:

3:4801d83f397c

diff -r 5e0cb74f950e -r dea57b3f84cd main.cpp
--- a/main.cpp	Tue Oct 17 16:27:08 2017 +0000
+++ b/main.cpp	Thu Oct 19 12:39:11 2017 +0000
@@ -3,15 +3,18 @@
  * Bismillahirahmanirrahim
  *
  * Updated :
- *  -Perhitungan PID masih error
- *  -encoder kanan sama encoder kiri belum kebaca. ntah kenapa
+ *  -
  *
  * To do List :
  *  -Coba ODOMETRY !!
  *  -Coba baca encoder kanan dan kiri dulu
  *
- * Last Edited by : Calmantara
+ * 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
  **/
+
  
 //LIBRARY
 #include "mbed.h"
@@ -22,42 +25,57 @@
 /***************************
  *  Konstanta dan Variabel *
  ***************************/
- 
+#define PI 3.141592
+#define DRODA 100
+
+//Variabel Robot
+float kll_roda = (float) PI*DRODA;  //variabel keliling robot
+
+//Primitive Function
+void limitMotor();  //procedure untuk mengecek limit pwm motor
+float getY();         //fungsi untuk menghitung jarak Y
+
 //Deklarasi Motor
 Motor motor_depan(PB_6, PB_13, PA_10);  //pwm, fwd, rev
 Motor motor_kanan(PB_8, PA_9, PA_5);    //pwm, fwd, rev
 Motor motor_kiri(PB_9, PA_12, PA_11);   //pwm, fwd, rev
 
-//Variabel Motor
-float pwm_kanan = 0.3;
-float pwm_kiri = -0.3;
-float limit_pwm = 0.7;
+//Variabel Kecepatan
+float pwm_kanan = 0.4;
+float pwm_kiri = -0.4;
+float limit_pwm = 0.8;
 
 //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_5, PC_4, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
-encoderKRAI encMotor_kiri(PC_7, PC_6, 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 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
 
 //Target nilai encoder
-float target_depan = 0;
+float target_depan = 0; //Target untuk jumlah rotary encoder gerakan
 
 //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 max_speed = 0.4;
-float min_speed = -0.4;
-float kp_1 = 0.1;                    //variabel konstanta PID
-float current_error1, prev_error1;
-float speed, prev_speed;
+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 cari jarak
+float target_jarak = 2.0;             //variabel yang digunakan untuk jarak odometry
+float jarak_y;                        //variabel untuk increment jarak y
 
 //Variabel Motor Berhenti
 unsigned long int prev_millis2;      //variabel untuk mendapatkan millis sebelumnya
@@ -72,7 +90,7 @@
     pc.baud(115200);
 
 //Start Millis
-    wait_ms(5000);
+    wait_ms(3000);
     startMillis();
 
 //Looping Program
@@ -80,45 +98,52 @@
         current_millis = millis();
         
         /* Speed Motor */
-        if(pwm_kanan > limit_pwm){
+        motor_kanan.speed(pwm_kanan);
+        motor_kiri.speed(pwm_kiri);
+        
+        pc.printf("%f\t%f\t%f\n", speed, pulses_kanan, pulses_kiri);
+        
+        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) encMotor_kanan.getRevolutions(); 
+            rev_kiri = (float) encMotor_kiri.getRevolutions();
+            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();
+            }
+            
+        if(current_millis - prev_millis2 >= (int)5000){
+            pwm_kiri = 0;
+            pwm_kanan = 0;
+            }
+    }
+} /* end of main fuction */
+
+/************************************
+ * Deklarasi Fungsi dan Prosedur    *
+ ************************************/
+ void limitMotor(){
+ /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */    
+     if(pwm_kanan > limit_pwm){
             pwm_kanan = limit_pwm;
             }
-        else if(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){
+        else if(pwm_kiri < (-limit_pwm)){
             pwm_kiri = -limit_pwm;
             } 
-        motor_kanan.speed(pwm_kanan);
-        motor_kiri.speed(pwm_kiri);
-        
-        pc.printf("%f\t%f\t%f\n", pulses_depan, pulses_kanan, pulses_kiri);
-        
-        if(current_millis - prev_millis1 >= time_sampling){
-        /* masuk ke perhitungan ketika sudah pada time sampling */
-            pulses_depan = (float) encMotor_depan.getRevolutions();  //Mengambil pulses encoder
-            current_error1 = target_depan - pulses_depan;
-            speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1;
-            /* Kondisi untuk menambahkan speed */
-            if(speed > max_speed){
-                pwm_kiri = pwm_kiri - max_speed;
-                pwm_kanan = pwm_kanan - max_speed;
-            }
-            else if ( speed < min_speed){
-                pwm_kiri = pwm_kiri - min_speed;
-                pwm_kanan = pwm_kanan - min_speed;
-            }
-            else {
-                pwm_kiri = pwm_kiri + speed;
-                pwm_kanan = pwm_kanan + speed;
-            }
-            prev_speed = speed;
-            prev_error1 = current_error1;
-            prev_millis1 = current_millis;
-            encMotor_depan.reset();
-            }
-    }
-} /* end of main fuction */
\ No newline at end of file
+     }/* end of limitMotor */
\ No newline at end of file