![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 9:e2c3175936fb
- Parent:
- 8:4a536a26ee07
- Child:
- 10:e550e5daed74
diff -r 4a536a26ee07 -r e2c3175936fb main.cpp --- a/main.cpp Sun Oct 22 03:32:02 2017 +0000 +++ b/main.cpp Tue Oct 24 03:53:45 2017 +0000 @@ -3,10 +3,9 @@ * Bismillahirahmanirrahim * * To do List : - * - TAMBAHKAN ENC DEPAN BIAR GA GERAK UNTUK MAJU * - Cari konstanta Kanan Kiri - * - Buat library odometry - * - Rapihkan kodingan :) + * - Cobda library Odometry + * * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :) * * UPDATED : 22/10 @@ -22,24 +21,27 @@ #include "encoderKRAI.h" #include "millis.h" #include "pidKRAI.h" +#include "odometryKRAI.h" /*************************** * Konstanta dan Variabel * ***************************/ #define PI 3.141592 -#define DRODA 100 +#define DRODA 100.0 //Variabel Robot -float kll_roda = (float) PI*DRODA; //variabel keliling robot +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; //Primitive Function -void limitMotor(); //procedure untuk mengecek limit pwm motor -void pidMotor(int x); //procedure untuk arah gerak motor -float getY(float y1, float y2); //fungsi untuk menghitung jarak Y +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 @@ -65,22 +67,21 @@ float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri -float delta_kaki; //variabel untuk membaca nilai delta kanan kiri -float delta_semua; //variabel untuk membaca nilai delta semua encoder +float d_tetha; //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 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri //Target nilai encoder -float tn_y = 0; //target untuk jumlah rotary encoder maju atau mundur -float tn_x = 0; //target untuk jumlah rotary encoder kanan atau kiri +float tn_tetha = 0; //target untuk nilai tetha robot //Variabel PID 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 speed_rotx; //variabel untuk penambahan pwm PID rotasi x float speed_disx; //variabel untuk penambahan pwm PID distance x @@ -92,16 +93,15 @@ pidKRAI pidDisX(); //kp ki kd ts //Variabel cari jarak -float target_y[100] = {-5000.0, -5000.0}; //variabel yang digunakan untuk target jarak y -float jarak_y; //variabel untuk increment jarak y +float target_y[100] = {3000.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 jarak_x; //variabel untuk increment 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; //variabel untuk matikan motor /*************************** * Main Function * @@ -124,10 +124,11 @@ while(in_program == 1){ //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, delta_kaki); + pc.printf("%.2f %.2f\n", jarak_y, d_tetha); } /* Speed Motor */ @@ -135,23 +136,28 @@ motor_kanan.speed(pwm_kanan); motor_kiri.speed(pwm_kiri); } + else if((jarak_x <= target_x[state]-(float)10.0)||(jarak_x >= target_x[state]+(float)10.0)){ + /* masuk ke case sumbu X */ + } else{ + pwm_depan = 0.0; pwm_kanan = 0.0; pwm_kiri = 0.0; + motor_depan.brake(1); motor_kanan.brake(1); motor_kiri.brake(1); - } - pulses_kanan = (float) encMotor_kanan.getPulses(); - pulses_kiri = (float) encMotor_kiri.getPulses(); - rev_kanan = (float) pulses_kanan/540; - rev_kiri = (float) pulses_kiri/540; + } + + /* mengambil data encoder motor */ + getValue(); /* masuk ke perhitungan parameter */ jarak_y += getY(rev_kanan, rev_kiri); - delta_kaki = pulses_kanan + pulses_kiri; + d_tetha = pulses_kanan + pulses_kiri + pulses_depan; + encMotor_depan.reset(); encMotor_kanan.reset(); encMotor_kiri.reset(); - pc.printf("%.2f %.2f\n", jarak_y, delta_kaki); + pc.printf("%.2f %.2f\n", jarak_y, d_tetha); } } /* end of main program */ } @@ -189,57 +195,71 @@ **/ if(x==0){ /* masuk ke perhitungan ketika sudah pada time sampling */ - pulses_kanan = (float) encMotor_kanan.getPulses(); - pulses_kiri = (float) encMotor_kiri.getPulses(); - rev_kanan = (float) pulses_kanan/540; - rev_kiri = (float) pulses_kiri/540; + /* mengambil data encoder motor */ + getValue(); /* masuk ke perhitungan parameter */ jarak_y += getY(rev_kanan, rev_kiri); - delta_kaki = pulses_kanan + pulses_kiri; + d_tetha = pulses_kanan + pulses_kiri + pulses_depan; - speed_roty = pidRotY.pidCount(tn_y, delta_kaki, limit_y); + speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_y); speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_y); + /* Kondisi untuk menambahkan speed */ pwm_kiri = -speed_disy + speed_roty; pwm_kanan = speed_disy + speed_roty; + pwm_depan = speed_roty; prev_millis1 = current_millis; + encMotor_depan.reset(); encMotor_kanan.reset(); encMotor_kiri.reset(); } else if(x==1){ /* masuk ke perhitungan ketika sudah pada time sampling */ - pulses_kanan = (float) encMotor_kanan.getPulses(); - pulses_kiri = (float) encMotor_kiri.getPulses(); - pulses_depan = (float) encMotor_depan.getPulses(); - rev_kanan = (float) pulses_kanan/540; - rev_kiri = (float) pulses_kiri/540; - rev_depan = (float) pulses_depan/540; + /* mengambil data encoder motor */ + getValue(); /* masuk ke perhitungan parameter */ //jarak_x += getX(rev_kanan+rev_kiri, rev_depan); - delta_semua = pulses_kanan + pulses_kiri + pulses_depan; + d_tetha = pulses_kanan + pulses_kiri + pulses_depan; - //speed_rotx = pidRotY.pidCount(tn_x, delta_semua); - //speed_disx = pidDisY.pidCount(target_y[0], jarak_y); + //speed_rotx = pidRotX.pidCount(tn_x, delta_semua); + //speed_disx = pidDisX.pidCount(target_x[0], jarak_x); + /* Kondisi untuk menambahkan speed */ pwm_kiri = speed_disx + speed_rotx; pwm_kanan = speed_disx + speed_rotx; pwm_depan = -(speed_disx + speed_rotx); prev_millis1 = current_millis; + encMotor_depan.reset(); encMotor_kanan.reset(); encMotor_kiri.reset(); - encMotor_depan.reset(); } }/* end of pidMotor */ -float getY(float y1, float y2){ +void getValue(){ +/* + * Procedure yang digunakan untuk menghitung value pulses encoder + * dan revolusi dari motor + */ + 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_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 */ - float jarak_y1 =(float) 1.732*(kll_roda*y1)/2; - float jarak_y2 =(float) 1.732*(kll_roda*y2)/2; + double jarak_y1 = 1.732*(kll_roda*y1)/2; + double jarak_y2 = 1.732*(kll_roda*y2)/2; - return (jarak_y1 - jarak_y2)*2; + return (jarak_y1 - jarak_y2); }/* end of getY */ +/*float getX(float x1, float x2, float x3){ + + }*/