![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
main.cpp
- Committer:
- calmantara186
- Date:
- 2017-10-17
- Revision:
- 0:a6918b57d3fa
- Child:
- 1:5e0cb74f950e
File content as of revision 0:a6918b57d3fa:
/** * THROWER ROBOT * Bismillahirahmanirrahim * * Updated : * -Perhitungan PID masih error * -encoder kanan sama encoder kiri belum kebaca. ntah kenapa * * To do List : * -Coba ODOMETRY !! * * Last Edited by : Calmantara **/ //LIBRARY #include "mbed.h" #include "Motor.h" #include "encoderKRAI.h" #include "millis.h" /*************************** * Konstanta dan Variabel * ***************************/ //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 Motor float pwm_kanan = 0.3; float pwm_kiri = -0.3; float limit_speed = 0.7; //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_5, PC_4, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType encoderKRAI encMotor_kiri(PC_7, PC_6, 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 //Target nilai encoder float target_depan = 0; //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 max_speed = 0.4; float min_speed = -0.4; float kp_1 = 0.5; //variabel konstanta PID float current_error1, prev_error1; float speed, prev_speed; //Variabel Motor Berhenti unsigned long int prev_millis2; //variabel untuk mendapatkan millis sebelumnya /*************************** * Main Function * ***************************/ int main(){ //Inisiasi Serial pc.baud(115200); //Start Millis //wait_ms(5000); startMillis(); //Looping Program while(1){ current_millis = millis(); /* Speed Motor */ if(pwm_kanan > limit_speed){ pwm_kanan = limit_speed; } else if(pwm_kanan < -limit_speed){ pwm_kanan = -limit_speed; } if(pwm_kiri > limit_speed){ pwm_kiri = limit_speed; } else if(pwm_kiri < -limit_speed){ pwm_kiri = -limit_speed; } motor_kanan.speed(pwm_kanan); motor_kiri.speed(pwm_kiri); //pc.printf("%f\t%f\t%f\n", pulses_depan, pulses_kanan, pulses_kiri); if(current_millis - prev_millis1 >= time_sampling){ /* masuk ke perhitungan ketika sudah pada time sampling */ pulses_depan = (float) encMotor_depan.getRevolutions(); //Mengambil pulses encoder current_error1 = target_depan - pulses_depan; speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1; /* Kondisi untuk menambahkan speed */ if(speed > max_speed){ pwm_kiri = pwm_kiri+max_speed; pwm_kanan = pwm_kanan - max_speed; } else if ( speed < min_speed){ pwm_kiri = pwm_kiri-min_speed; pwm_kanan = pwm_kanan + min_speed; } else { pwm_kiri = pwm_kiri + speed; pwm_kanan = pwm_kanan - speed; } prev_speed = speed; prev_error1 = current_error1; prev_millis1 = current_millis; } } } /* end of main fuction */