![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 6:5632ff2142c0
- Parent:
- 3:4801d83f397c
- Child:
- 7:cf822f5e7b12
diff -r 1604b89b52e6 -r 5632ff2142c0 main.cpp --- a/main.cpp Thu Oct 19 15:55:10 2017 +0000 +++ b/main.cpp Sat Oct 21 06:34:05 2017 +0000 @@ -2,17 +2,19 @@ * THROWER ROBOT * Bismillahirahmanirrahim * - * Updated : - * - - * * To do List : - * -Coba ODOMETRY !! - * -Coba baca encoder kanan dan kiri dulu + * - Cari Fungsi Output Open Loop + * - Buat library odometry * * UPDATED : 18/10 * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4 - * - Dapet nilai maju Kp = 0.00035 - * Last Edited by : Calmantara & Robertsen + * - Dapet nilai maju Kp = 0.00035, pwm kanan kiri = 0.4 + * + * Update : 19/10 + * - Inlcude library PID + * - Lebih rapi + * + * Last Edited by : Thrower KRAI **/ @@ -21,6 +23,7 @@ #include "Motor.h" #include "encoderKRAI.h" #include "millis.h" +#include "pidKRAI.h" /*************************** * Konstanta dan Variabel * @@ -31,9 +34,13 @@ //Variabel Robot float kll_roda = (float) PI*DRODA; //variabel keliling robot +//Variabel Program dan Planning +unsigned short in_program = 1; //variabel untuk masuk ke looping program + //Primitive Function -void limitMotor(); //procedure untuk mengecek limit pwm 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 +float getY(float y1, float y2); //fungsi untuk menghitung jarak Y //Deklarasi Motor Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev @@ -41,44 +48,60 @@ Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev //Variabel Kecepatan -float pwm_kanan = 0.4; -float pwm_kiri = -0.4; -float limit_pwm = 0.8; +float pwm_kanan; //pwm motor kanan +float pwm_kiri; //pwm motor kiri +float pwm_depan; //pwm motor depan +float limit_pwm = 0.9; //limit pwm semua motor //Variabel Serial Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX //Deklarasi Rotary Encoder -encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType -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 +encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType +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 //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 delta_kaki; //variabel untuk membaca nilai delta kanan kiri +float delta_semua; //variabel untuk membaca nilai delta semua encoder + 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 +float rev_kiri; //variabel untuk membaca nilai putaran roda kiri //Target nilai encoder -float target_depan = 0; //Target untuk jumlah rotary encoder gerakan +float tn_y = 0; //target untuk jumlah rotary encoder maju atau mundur +float tn_x = 0; //target untuk jumlah rotary encoder kanan atau kiri -//Variabel PID_1 -unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan -unsigned long int prev_millis1; //variabel untuk mendapatkan millis sebelumnya -unsigned short time_sampling = 12; //variabel untuk time sampling -float kp_1 = 0.00035; //variabel konstanta PID -float current_error1, prev_error1; //variabel untuk millis PID -float speed, prev_speed; //variabel untuk penambahan pwm PID +//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 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.0, (float) 0.0, (float) 0.0, ts_base); //kp ki kd ts +pidKRAI pidRotX(); //kp ki kd ts + +pidKRAI pidDisY((float)0.0,(float)0.0,(float)0.0,ts_base); //kp ki kd ts +pidKRAI pidDisX(); //kp ki kd ts //Variabel cari jarak -float target_jarak = 1100.0; //variabel yang digunakan untuk jarak odometry -float jarak_y; //variabel untuk increment jarak y +float target_y[100] = {1100.0, 0.0}; //variabel yang digunakan untuk target jarak y +float jarak_y; //variabel untuk increment jarak y -//Variabel Motor Berhenti -unsigned long int prev_millis2; //variabel untuk mendapatkan millis sebelumnya +float target_x[100]; //variabel yang digunakan untuk target jarak x +float 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 * @@ -92,69 +115,101 @@ //Start Millis wait_ms(3000); startMillis(); - -//Looping Program - while(1){ - current_millis = millis(); - - /* Speed Motor */ - motor_kanan.speed(pwm_kanan); - motor_kiri.speed(pwm_kiri); - - pc.printf("%.2f %.2f %.2f %.2f\n", speed, pulses_kanan, pulses_kiri,jarak_y); - - if(current_millis - prev_millis1 >= time_sampling){ - /* 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; - /* masuk ke perhitungan parameter */ - jarak_y += getY(rev_kanan, rev_kiri); - delta_kaki = pulses_kanan + pulses_kiri; - - current_error1 = target_depan - delta_kaki; - speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1; - /* Kondisi untuk menambahkan speed */ - pwm_kiri = pwm_kiri + speed; - pwm_kanan = pwm_kanan + speed; - prev_speed = speed; - prev_error1 = current_error1; - prev_millis1 = current_millis; - encMotor_kanan.reset(); - encMotor_kiri.reset(); +//Looping Utama +while(1){ +//Looping Program Planning + while(in_program == 0){ + }/* end of planning program */ +//Looping Program Utama + while(in_program == 1){ + current_millis = millis(); + + /* Speed Motor */ + motor_kanan.speed(pwm_kanan); + motor_kiri.speed(pwm_kiri); + + pc.printf("%.2f \n", delta_kaki); + + if(current_millis - prev_millis1 >= ts_base){ + //pidMotor(1); + pulses_kanan = (float) encMotor_kanan.getPulses(); + pulses_kiri = (float) encMotor_kiri.getPulses(); + rev_kanan = (float) encMotor_kanan.getRevolutions(); + rev_kiri = (float) encMotor_kiri.getRevolutions(); + + /* masuk ke perhitungan parameter */ + jarak_y += getY(rev_kanan, rev_kiri); + delta_kaki = pulses_kanan + pulses_kiri; + } + + if(jarak_y > target_y[0]){ + pwm_kiri = 0; + pwm_kanan = 0; + } } - - if(jarak_y > target_jarak){ - pwm_kiri = 0; - pwm_kanan = 0; - } + } /* end of main program */ } -} /* end of main fuction */ /************************************ * Deklarasi Fungsi dan Prosedur * ************************************/ void limitMotor(){ /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */ - if(pwm_kanan > limit_pwm){ + if(pwm_kanan > limit_pwm){ pwm_kanan = limit_pwm; - } - else if(pwm_kanan < (-limit_pwm)){ - pwm_kanan = -limit_pwm; - } - if(pwm_kiri > limit_pwm){ - pwm_kiri = limit_pwm; - } - else if(pwm_kiri < (-limit_pwm)){ - pwm_kiri = -limit_pwm; - } + } + else if(pwm_kanan < (-limit_pwm)){ + pwm_kanan = -limit_pwm; + } + if(pwm_kiri > limit_pwm){ + pwm_kiri = limit_pwm; + } + else if(pwm_kiri < (-limit_pwm)){ + pwm_kiri = -limit_pwm; + } + if(pwm_depan > limit_pwm){ + pwm_depan = limit_pwm; + } + else if(pwm_depan < (-limit_pwm)){ + pwm_depan = -limit_pwm; + } }/* end of limitMotor */ +void pidMotor(int x){ +/** + * prosedur untuk gerak motor + * 0 : maju dan mundur + * 1 : kanan dan kiri + **/ + 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; + + /* masuk ke perhitungan parameter */ + jarak_y += getY(rev_kanan, rev_kiri); + delta_kaki = pulses_kanan + pulses_kiri; + + speed_roty = pidRotY.pidCount(tn_y, delta_kaki); + speed_disy = pidDisY.pidCount(target_y[0], jarak_y); + /* Kondisi untuk menambahkan speed */ + pwm_kiri = speed_disy + speed_roty; + pwm_kanan = speed_disy + speed_roty; + + prev_millis1 = current_millis; + encMotor_kanan.reset(); + encMotor_kiri.reset(); + + } + else if(x==1){} + }/* end of pidMotor */ + float getY(float y1, float y2){ /* fungsi untuk menghitung nilai Y dari roda kanan kiri */ float jarak_y1 = (kll_roda*y1)/4; float jarak_y2 = (kll_roda*y2)/4; return jarak_y1 - jarak_y2; - }/* end of getY */ \ No newline at end of file + }/* end of getY */