![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
Diff: main.cpp
- Revision:
- 1:5e0cb74f950e
- Parent:
- 0:a6918b57d3fa
- Child:
- 2:dea57b3f84cd
--- a/main.cpp Tue Oct 17 15:43:07 2017 +0000 +++ b/main.cpp Tue Oct 17 16:27:08 2017 +0000 @@ -8,6 +8,7 @@ * * To do List : * -Coba ODOMETRY !! + * -Coba baca encoder kanan dan kiri dulu * * Last Edited by : Calmantara **/ @@ -21,7 +22,7 @@ /*************************** * 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 @@ -30,7 +31,7 @@ //Variabel Motor float pwm_kanan = 0.3; float pwm_kiri = -0.3; -float limit_speed = 0.7; +float limit_pwm = 0.7; //Variabel Serial Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX @@ -54,7 +55,7 @@ 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 kp_1 = 0.1; //variabel konstanta PID float current_error1, prev_error1; float speed, prev_speed; @@ -71,30 +72,30 @@ pc.baud(115200); //Start Millis - //wait_ms(5000); + wait_ms(5000); startMillis(); //Looping Program while(1){ current_millis = millis(); + /* Speed Motor */ - if(pwm_kanan > limit_speed){ - pwm_kanan = limit_speed; + if(pwm_kanan > limit_pwm){ + pwm_kanan = limit_pwm; } - else if(pwm_kanan < -limit_speed){ - pwm_kanan = -limit_speed; + else if(pwm_kanan < -limit_pwm){ + pwm_kanan = -limit_pwm; } - if(pwm_kiri > limit_speed){ - pwm_kiri = limit_speed; + if(pwm_kiri > limit_pwm){ + pwm_kiri = limit_pwm; } - else if(pwm_kiri < -limit_speed){ - pwm_kiri = -limit_speed; + else if(pwm_kiri < -limit_pwm){ + pwm_kiri = -limit_pwm; } - motor_kanan.speed(pwm_kanan); motor_kiri.speed(pwm_kiri); - //pc.printf("%f\t%f\t%f\n", pulses_depan, pulses_kanan, pulses_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 */ @@ -103,20 +104,21 @@ 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_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; + pwm_kiri = pwm_kiri - min_speed; + pwm_kanan = pwm_kanan - min_speed; } else { pwm_kiri = pwm_kiri + speed; - pwm_kanan = pwm_kanan - speed; + pwm_kanan = pwm_kanan + speed; } prev_speed = speed; prev_error1 = current_error1; prev_millis1 = current_millis; + encMotor_depan.reset(); } } } /* end of main fuction */ \ No newline at end of file