Diff: main.cpp

Revision:

3:4801d83f397c

Parent:

2:dea57b3f84cd

Child:

6:5632ff2142c0

--- a/main.cpp	Thu Oct 19 12:39:11 2017 +0000
+++ b/main.cpp	Thu Oct 19 13:32:49 2017 +0000
@@ -32,8 +32,8 @@
 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
+void limitMotor();                      //procedure untuk mengecek limit pwm 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
@@ -74,7 +74,7 @@
 float speed, prev_speed;             //variabel untuk penambahan pwm PID
 
 //Variabel cari jarak
-float target_jarak = 2.0;             //variabel yang digunakan untuk jarak odometry
+float target_jarak = 1100.0;          //variabel yang digunakan untuk jarak odometry
 float jarak_y;                        //variabel untuk increment jarak y
 
 //Variabel Motor Berhenti
@@ -101,15 +101,18 @@
         motor_kanan.speed(pwm_kanan);
         motor_kiri.speed(pwm_kiri);
         
-        pc.printf("%f\t%f\t%f\n", speed, pulses_kanan, pulses_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) encMotor_kanan.getRevolutions(); 
-            rev_kiri = (float) encMotor_kiri.getRevolutions();
+            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 */
@@ -122,7 +125,7 @@
             encMotor_kiri.reset();
             }
             
-        if(current_millis - prev_millis2 >= (int)5000){
+        if(jarak_y > target_jarak){
             pwm_kiri = 0;
             pwm_kanan = 0;
             }
@@ -146,4 +149,12 @@
         else if(pwm_kiri < (-limit_pwm)){
             pwm_kiri = -limit_pwm;
             } 
-     }/* end of limitMotor */
\ No newline at end of file
+     }/* end of limitMotor */
+
+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