Isro Syaeful Iman
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 10:e550e5daed74
- Parent:
- 9:e2c3175936fb
- Child:
- 11:d4cf81f59601
--- 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){
-
- }*/