![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 10:e550e5daed74
- Parent:
- 9:e2c3175936fb
- Child:
- 11:d4cf81f59601
diff -r e2c3175936fb -r e550e5daed74 main.cpp --- a/main.cpp Tue Oct 24 03:53:45 2017 +0000 +++ b/main.cpp Tue Oct 24 23:56:39 2017 +0000 @@ -4,7 +4,7 @@ * * To do List : * - Cari konstanta Kanan Kiri - * - Cobda library Odometry + * - Cari Konstanta lagi :( * * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :) * @@ -26,7 +26,6 @@ /*************************** * Konstanta dan Variabel * ***************************/ -#define PI 3.141592 #define DRODA 100.0 //Variabel Robot @@ -40,8 +39,6 @@ void limitMotor(); //procedure untuk mengecek limit pwm motor void pidMotor(int x); //procedure untuk arah gerak motor void getValue(); //procedure untuk menghitung pulses dan rev -double getY(float y1, float y2); //fungsi untuk menghitung jarak Y -//double getX(float x1, float x2, float x3); //fungsi untuk menghitung jarak X //Deklarasi Motor Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev @@ -62,12 +59,16 @@ 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 +//Deklarasi Odometry +odometryKRAI odometry(DRODA); //deklarasi odometry dengan input diameter roda + //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 d_tetha; //variabel untuk membaca nilai delta kanan kiri +float d_tetha; //variabel untuk membaca nilai delta kanan kiri depan +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 @@ -80,28 +81,30 @@ 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_y = 0.5; +float limit_dis = 0.5; +float limit_rot = 0.3; 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.00334, (float) 0.0, (float) 0.00000, ts_base); //kp ki kd ts -pidKRAI pidRotX(); //kp ki kd ts +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 pidDisY((float)0.228,(float).005534,(float)0.0,ts_base); //kp ki kd ts -pidKRAI pidDisX(); //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 //Variabel cari jarak -float target_y[100] = {3000.0, -5000.0}; //variabel yang digunakan untuk target jarak y +float target_y[100] = {2000.0, -5000.0}; //variabel yang digunakan untuk target jarak y double jarak_y; //variabel untuk increment jarak y -float target_x[100]; //variabel yang digunakan untuk target jarak x +float target_x[100] = {2000.0, -5000.0}; //variabel yang digunakan untuk target jarak x double 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; /*************************** * Main Function * @@ -122,22 +125,35 @@ }/* end of planning program */ //Looping Program Utama while(in_program == 1){ - //current_millis = millis(); + current_millis = millis(); if((jarak_y <= target_y[state]-(float)10.0)||(jarak_y >= target_y[state]+(float)10.0)){ /* masuk ke case sumbu Y */ - current_millis = millis(); if(current_millis - prev_millis1 >= 12){ pidMotor(0); - pc.printf("%.2f %.2f\n", jarak_y, d_tetha); + //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, jarak_x, d_tetha, speed_roty, speed_disy); } /* Speed Motor */ limitMotor(); + motor_depan.speed(pwm_depan); motor_kanan.speed(pwm_kanan); motor_kiri.speed(pwm_kiri); + prev_millis2 = current_millis; } else if((jarak_x <= target_x[state]-(float)10.0)||(jarak_x >= target_x[state]+(float)10.0)){ - /* masuk ke case sumbu X */ + /* 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; } else{ pwm_depan = 0.0; @@ -152,12 +168,13 @@ getValue(); /* masuk ke perhitungan parameter */ - jarak_y += getY(rev_kanan, rev_kiri); - d_tetha = pulses_kanan + pulses_kiri + pulses_depan; + jarak_y += odometry.getY(rev_kanan, rev_kiri); + jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri); + d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri); encMotor_depan.reset(); encMotor_kanan.reset(); encMotor_kiri.reset(); - pc.printf("%.2f %.2f\n", jarak_y, d_tetha); + 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); } } /* end of main program */ } @@ -199,11 +216,11 @@ getValue(); /* masuk ke perhitungan parameter */ - jarak_y += getY(rev_kanan, rev_kiri); - d_tetha = pulses_kanan + pulses_kiri + pulses_depan; + jarak_y += odometry.getY(rev_kanan, rev_kiri); + d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri); - speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_y); - speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_y); + speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_rot); + speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_dis); /* Kondisi untuk menambahkan speed */ pwm_kiri = -speed_disy + speed_roty; @@ -222,16 +239,16 @@ getValue(); /* masuk ke perhitungan parameter */ - //jarak_x += getX(rev_kanan+rev_kiri, rev_depan); - d_tetha = pulses_kanan + pulses_kiri + pulses_depan; + jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri); + d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri); - //speed_rotx = pidRotX.pidCount(tn_x, delta_semua); - //speed_disx = pidDisX.pidCount(target_x[0], jarak_x); + speed_rotx = pidRotX.pidCount(tn_tetha, d_tetha, limit_rot); + speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis); /* Kondisi untuk menambahkan speed */ - pwm_kiri = speed_disx + speed_rotx; - pwm_kanan = speed_disx + speed_rotx; - pwm_depan = -(speed_disx + speed_rotx); + pwm_kiri = ((float)0.5*speed_disx) + speed_rotx; + pwm_kanan = ((float)0.5*speed_disx) + speed_rotx; + pwm_depan = -(speed_disx) + speed_rotx; prev_millis1 = current_millis; encMotor_depan.reset(); @@ -248,18 +265,7 @@ pulses_depan = (float)encMotor_depan.getPulses(); pulses_kanan = (float) encMotor_kanan.getPulses(); pulses_kiri = (float) encMotor_kiri.getPulses(); - rev_depan = (float) pulses_kanan/540; + rev_depan = (float) pulses_depan/540; rev_kanan = (float) pulses_kanan/540; rev_kiri = (float) pulses_kiri/540; }/* end of getValue */ - -double getY(float y1, float y2){ -/* fungsi untuk menghitung nilai Y dari roda kanan kiri */ - double jarak_y1 = 1.732*(kll_roda*y1)/2; - double jarak_y2 = 1.732*(kll_roda*y2)/2; - - return (jarak_y1 - jarak_y2); - }/* end of getY */ -/*float getX(float x1, float x2, float x3){ - - }*/