Diff: main.cpp

Revision:

1:5e0cb74f950e

Parent:

0:a6918b57d3fa

Child:

2:dea57b3f84cd

diff -r a6918b57d3fa -r 5e0cb74f950e main.cpp
--- a/main.cpp	Tue Oct 17 15:43:07 2017 +0000
+++ b/main.cpp	Tue Oct 17 16:27:08 2017 +0000
@@ -8,6 +8,7 @@
  *
  * To do List :
  *  -Coba ODOMETRY !!
+ *  -Coba baca encoder kanan dan kiri dulu
  *
  * Last Edited by : Calmantara
  **/
@@ -21,7 +22,7 @@
 /***************************
  *  Konstanta dan Variabel *
  ***************************/
-
+ 
 //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
@@ -30,7 +31,7 @@
 //Variabel Motor
 float pwm_kanan = 0.3;
 float pwm_kiri = -0.3;
-float limit_speed = 0.7;
+float limit_pwm = 0.7;
 
 //Variabel Serial
 Serial pc(USBTX, USBRX);    //Deklarasi serial pc TX RX
@@ -54,7 +55,7 @@
 unsigned short time_sampling = 12;   //variabel untuk time sampling
 float max_speed = 0.4;
 float min_speed = -0.4;
-float kp_1 = 0.5;                    //variabel konstanta PID
+float kp_1 = 0.1;                    //variabel konstanta PID
 float current_error1, prev_error1;
 float speed, prev_speed;
 
@@ -71,30 +72,30 @@
     pc.baud(115200);
 
 //Start Millis
-    //wait_ms(5000);
+    wait_ms(5000);
     startMillis();
 
 //Looping Program
     while(1){
         current_millis = millis();
+        
         /* Speed Motor */
-        if(pwm_kanan > limit_speed){
-            pwm_kanan = limit_speed;
+        if(pwm_kanan > limit_pwm){
+            pwm_kanan = limit_pwm;
             }
-        else if(pwm_kanan < -limit_speed){
-            pwm_kanan = -limit_speed;
+        else if(pwm_kanan < -limit_pwm){
+            pwm_kanan = -limit_pwm;
             } 
-        if(pwm_kiri > limit_speed){
-            pwm_kiri = limit_speed;
+        if(pwm_kiri > limit_pwm){
+            pwm_kiri = limit_pwm;
             }
-        else if(pwm_kiri < -limit_speed){
-            pwm_kiri = -limit_speed;
+        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);
+        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 */
@@ -103,20 +104,21 @@
             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_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;
+                pwm_kiri = pwm_kiri - min_speed;
+                pwm_kanan = pwm_kanan - min_speed;
             }
             else {
                 pwm_kiri = pwm_kiri + speed;
-                pwm_kanan = pwm_kanan - 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