utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
main.cpp
- Committer:
- gatulz
- Date:
- 2018-03-31
- Revision:
- 12:84cb23216f78
- Parent:
- 11:d4cf81f59601
File content as of revision 12:84cb23216f78:
/******************************************* * THROWER ROBOT * * Bismillahirahmanirrahim * * * * To do List : * - Cari konstanta Kanan Kiri * - Cari Konstanta lagi :( * * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :) * * UPDATED : 22/10 * - PID maju/mundur enak enak jos * * Last Edited by : Thrower KRAI ********************************************/ //LIBRARY #include "mbed.h" #include "Motor.h" #include "encoderKRAI.h" #include "millis.h" #include "pidKRAI.h" #include "odometryKRAI.h" #include "Ultrasonic.h" /*************************** * Konstanta dan Variabel * ***************************/ #define DRODA 100.0 //ultrasonic Ultrasonic US(PA_7, PD_2); //Variabel Robot double kll_roda = PI*DRODA; //variabel keliling robot //Variabel Program dan Planning 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 void pidMotor(int x); //procedure untuk arah gerak motor void getValue(); //procedure untuk menghitung pulses dan rev //Deklarasi Motor Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev //Variabel Kecepatan 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 //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 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 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri //Target nilai encoder 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_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.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.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[50] = {2000.0, -2000.0}; //variabel yang digunakan untuk target jarak y double jarak_y; //variabel untuk increment jarak y 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 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 * ***************************/ int main(){ //Inisiasi Serial pc.baud(115200); //Start Millis 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)5.0)||(jarak_y >= target_y[state]+(float)5.0)){ /* masuk ke case sumbu Y */ if(current_millis - prev_millis1 >= 12){ pidMotor(0); } /* Speed Motor */ limitMotor(); motor_depan.speed(pwm_depan); motor_kanan.speed(pwm_kanan); motor_kiri.speed(pwm_kiri); //prev_millis2 = current_millis; pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_depan, pulses_kanan, pulses_kiri); } if((jarak_x <= target_x[state]-(float)5.0)||(jarak_x >= target_x[state]+(float)5.0)){ /* masuk ke case sumbu X */ if(current_millis - prev_millis1 >= 12){ pidMotor(1); } /* Speed Motor */ limitMotor(); motor_depan.speed(pwm_depan); motor_kanan.speed(pwm_kanan); 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; pwm_kanan = 0.0; pwm_kiri = 0.0; motor_depan.brake(1); motor_kanan.brake(1); motor_kiri.brake(1); in_program = 0; } /* mengambil data encoder motor */ getValue(); /* masuk ke perhitungan parameter */ 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 %.2f %.2f\n", jarak_y, d_tetha, pulses_kanan, pulses_kiri); } } /* end of main program */ } /************************************ * Deklarasi Fungsi dan Prosedur * ************************************/ void limitMotor(){ /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */ 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; } 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 */ /* mengambil data encoder motor */ getValue(); /* masuk ke perhitungan parameter */ 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_rot); speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_dis); /* 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 */ /* mengambil data encoder motor */ getValue(); /* masuk ke perhitungan parameter */ 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_tetha, d_tetha, limit_rot); speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis); /* Kondisi untuk menambahkan speed */ 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; encMotor_depan.reset(); encMotor_kanan.reset(); encMotor_kiri.reset(); } }/* end of pidMotor */ 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_depan/540; rev_kanan = (float) pulses_kanan/540; rev_kiri = (float) pulses_kiri/540; }/* end of getValue */