![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 3:4801d83f397c
- Parent:
- 2:dea57b3f84cd
- Child:
- 6:5632ff2142c0
--- a/main.cpp Thu Oct 19 12:39:11 2017 +0000 +++ b/main.cpp Thu Oct 19 13:32:49 2017 +0000 @@ -32,8 +32,8 @@ float kll_roda = (float) PI*DRODA; //variabel keliling robot //Primitive Function -void limitMotor(); //procedure untuk mengecek limit pwm motor -float getY(); //fungsi untuk menghitung jarak Y +void limitMotor(); //procedure untuk mengecek limit pwm 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 @@ -74,7 +74,7 @@ float speed, prev_speed; //variabel untuk penambahan pwm PID //Variabel cari jarak -float target_jarak = 2.0; //variabel yang digunakan untuk jarak odometry +float target_jarak = 1100.0; //variabel yang digunakan untuk jarak odometry float jarak_y; //variabel untuk increment jarak y //Variabel Motor Berhenti @@ -101,15 +101,18 @@ motor_kanan.speed(pwm_kanan); motor_kiri.speed(pwm_kiri); - pc.printf("%f\t%f\t%f\n", speed, pulses_kanan, pulses_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) encMotor_kanan.getRevolutions(); - rev_kiri = (float) encMotor_kiri.getRevolutions(); + 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 */ @@ -122,7 +125,7 @@ encMotor_kiri.reset(); } - if(current_millis - prev_millis2 >= (int)5000){ + if(jarak_y > target_jarak){ pwm_kiri = 0; pwm_kanan = 0; } @@ -146,4 +149,12 @@ else if(pwm_kiri < (-limit_pwm)){ pwm_kiri = -limit_pwm; } - }/* end of limitMotor */ \ No newline at end of file + }/* end of limitMotor */ + +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