Diff: main.cpp

Revision:

11:d4cf81f59601

Parent:

10:e550e5daed74

Child:

12:84cb23216f78

--- a/main.cpp	Tue Oct 24 23:56:39 2017 +0000
+++ b/main.cpp	Wed Oct 25 15:16:27 2017 +0000
@@ -32,8 +32,10 @@
 double kll_roda = PI*DRODA;  //variabel keliling robot
 
 //Variabel Program dan Planning
-unsigned short in_program = 1;        //variabel untuk masuk ke looping program
-unsigned short state = 0;
+unsigned short in_program = 0;        //variabel untuk masuk ke looping program
+short state =-1;
+
+DigitalIn mybutton(USER_BUTTON);
 
 //Primitive Function
 void limitMotor();                          //procedure untuk mengecek limit pwm motor
@@ -68,7 +70,7 @@
 float pulses_kiri;     //variabel untuk membaca nilai encoder motor kiri
 
 float d_tetha;         //variabel untuk membaca nilai delta kanan kiri depan
-float d_tetha2;        //variabel untuk membaca nilai delta kanan kiri
+//float d_tetha2;        //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
@@ -81,24 +83,24 @@
 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_dis = 0.5;
-float limit_rot = 0.3;
+float limit_dis = 0.4;
+float limit_rot = 0.2;
 
 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.134, (float) 0.0, (float) 0.2123, ts_base);   //kp ki kd ts
-pidKRAI pidRotX((float) 0.134, (float) 0.0, (float) 0.2123, ts_base);   //kp ki kd ts
+pidKRAI pidRotY((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base);   //kp ki kd ts
+pidKRAI pidRotX((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base);   //kp ki kd ts
 
-pidKRAI pidDisY((float)0.228,(float).005534,(float)0.3231,ts_base);     //kp ki kd ts
-pidKRAI pidDisX((float)0.228,(float).005534,(float)0.3231,ts_base);     //kp ki kd ts
+pidKRAI pidDisY((float)0.2208,(float)0.0105,(float)4.26231,ts_base);     //kp ki kd ts
+pidKRAI pidDisX((float)0.228,(float)0.0234,(float)9.6231,ts_base);     //kp ki kd ts
 
 //Variabel cari jarak
-float target_y[100] = {2000.0, -5000.0};      //variabel yang digunakan untuk target jarak y
+float target_y[50] = {2000.0, -2000.0};      //variabel yang digunakan untuk target jarak y
 double jarak_y;                               //variabel untuk increment jarak y
 
-float target_x[100] = {2000.0, -5000.0};      //variabel yang digunakan untuk target jarak x
+float target_x[50] = {2000.0, -2000.0};      //variabel yang digunakan untuk target jarak x
 double jarak_x;                               //variabel untuk increment jarak x
 
 //Variabel Waktu
@@ -116,21 +118,31 @@
     pc.baud(115200);
 
 //Start Millis
-    wait_ms(3000);
+    wait_ms(1000);
     startMillis();
 //Looping Utama
 while(1){
 //Looping Program Planning
     while(in_program == 0){
+        
+        if (mybutton == 0){
+            pidRotY.reset();
+            pidDisY.reset();
+            pidRotX.reset();
+            pidDisX.reset();
+            jarak_y = 0;
+            jarak_x = 0;
+            state+=1;
+            in_program = 1;
+            }
         }/* end of planning program */
 //Looping Program Utama
     while(in_program == 1){
                 current_millis = millis();
-                if((jarak_y <= target_y[state]-(float)10.0)||(jarak_y >= target_y[state]+(float)10.0)){
+                if((jarak_y <= target_y[state]-(float)5.0)||(jarak_y >= target_y[state]+(float)5.0)){
                 /* masuk ke case sumbu Y */
                     if(current_millis - prev_millis1 >= 12){
                         pidMotor(0);
-                        //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, jarak_x, d_tetha, speed_roty, speed_disy);
                         }
                     
                     /* Speed Motor */
@@ -138,22 +150,21 @@
                     motor_depan.speed(pwm_depan);
                     motor_kanan.speed(pwm_kanan);
                     motor_kiri.speed(pwm_kiri);
-                    prev_millis2 = current_millis;
+                    //prev_millis2 = current_millis;
+                    //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_depan, pulses_kanan, pulses_kiri); 
                     }
-                else if((jarak_x <= target_x[state]-(float)10.0)||(jarak_x >= target_x[state]+(float)10.0)){
+                if((jarak_x <= target_x[state]-(float)5.0)||(jarak_x >= target_x[state]+(float)5.0)){
                 /* masuk  ke case sumbu X */
-                    //current_millis = millis();
                     if(current_millis - prev_millis1 >= 12){
                         pidMotor(1);
-                        //pc.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f\n", jarak_x, d_tetha, speed_rotx, speed_disx, pulses_depan, pulses_kanan, pulses_kiri);
                         }
                     
                     /* Speed Motor */
                     limitMotor();
                     motor_depan.speed(pwm_depan);
                     motor_kanan.speed(pwm_kanan);
-                    motor_kiri.speed(pwm_kiri);
-                    prev_millis2 = current_millis;    
+                    motor_kiri.speed(pwm_kiri);    
+                    //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_x, d_tetha, rev_depan, rev_kanan, rev_kiri);
                     }
                 else{
                     pwm_depan = 0.0;
@@ -162,6 +173,7 @@
                     motor_depan.brake(1);
                     motor_kanan.brake(1);
                     motor_kiri.brake(1);
+                    in_program = 0;
                     }
                     
             /* mengambil data encoder motor */
@@ -174,7 +186,7 @@
             encMotor_depan.reset();
             encMotor_kanan.reset();
             encMotor_kiri.reset();
-            pc.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\n", jarak_y, jarak_x, d_tetha, speed_rotx, speed_disx, pulses_depan, pulses_kanan, pulses_kiri);    
+            //pc.printf("%.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_kanan, pulses_kiri);    
             }
         } /* end of main program */
     }
@@ -246,8 +258,8 @@
             speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis);
             
             /* Kondisi untuk menambahkan speed */
-            pwm_kiri = ((float)0.5*speed_disx) + speed_rotx;
-            pwm_kanan = ((float)0.5*speed_disx) + speed_rotx;
+            pwm_kiri = ((float)0.63*speed_disx) + speed_rotx;
+            pwm_kanan = ((float)0.63*speed_disx) + speed_rotx;
             pwm_depan = -(speed_disx) + speed_rotx;
             
             prev_millis1 = current_millis;