KRAI 2017
/
new
dear fatoni
main.cpp@1:14997ad30b4e, 2017-05-15 (annotated)
- Committer:
- Fathoni17
- Date:
- Mon May 15 09:30:15 2017 +0000
- Revision:
- 1:14997ad30b4e
- Parent:
- 0:12479c9c5c72
Untuk Tuning Reloader
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Najib_irvani | 0:12479c9c5c72 | 1 | #include "mbed.h" |
Najib_irvani | 0:12479c9c5c72 | 2 | #include "Ping.h" |
Najib_irvani | 0:12479c9c5c72 | 3 | #include "Motor.h" |
Najib_irvani | 0:12479c9c5c72 | 4 | |
Najib_irvani | 0:12479c9c5c72 | 5 | #define PIN PC_15 |
Najib_irvani | 0:12479c9c5c72 | 6 | |
Najib_irvani | 0:12479c9c5c72 | 7 | Ping ping(PIN); |
Najib_irvani | 0:12479c9c5c72 | 8 | Motor powerScrew(PB_10, PB_14, PB_13); |
Najib_irvani | 0:12479c9c5c72 | 9 | Serial pc(USBTX,USBRX); |
Najib_irvani | 0:12479c9c5c72 | 10 | |
Najib_irvani | 0:12479c9c5c72 | 11 | int jarak; |
Najib_irvani | 0:12479c9c5c72 | 12 | double a,b,c; |
Najib_irvani | 0:12479c9c5c72 | 13 | double current_error, previous_error1 = 0; |
Najib_irvani | 0:12479c9c5c72 | 14 | double pwm, previous_pwm = 0; |
Najib_irvani | 0:12479c9c5c72 | 15 | |
Fathoni17 | 1:14997ad30b4e | 16 | // Atur Variable Tuning |
Fathoni17 | 1:14997ad30b4e | 17 | double target = 90; |
Fathoni17 | 1:14997ad30b4e | 18 | double Kp = -0.003753, Ki = -1.001e-06, Ts=10; |
Fathoni17 | 1:14997ad30b4e | 19 | double batas_pwm_atas = 0.35; |
Fathoni17 | 1:14997ad30b4e | 20 | double batas_pwm_bawah = 0.05; |
Fathoni17 | 1:14997ad30b4e | 21 | |
Najib_irvani | 0:12479c9c5c72 | 22 | int main(){ |
Najib_irvani | 0:12479c9c5c72 | 23 | pc.baud(115200); |
Fathoni17 | 1:14997ad30b4e | 24 | //powerScrew.speed(0.5); |
Najib_irvani | 0:12479c9c5c72 | 25 | while(1){ |
Najib_irvani | 0:12479c9c5c72 | 26 | int jarak; |
Najib_irvani | 0:12479c9c5c72 | 27 | ping.Send(); |
Najib_irvani | 0:12479c9c5c72 | 28 | wait_ms(10); |
Najib_irvani | 0:12479c9c5c72 | 29 | jarak = ping.Read_cm(); |
Najib_irvani | 0:12479c9c5c72 | 30 | |
Fathoni17 | 1:14997ad30b4e | 31 | pc.printf("%d\t%d\n", target, jarak); |
Fathoni17 | 1:14997ad30b4e | 32 | |
Najib_irvani | 0:12479c9c5c72 | 33 | current_error = (double) (target-jarak); |
Najib_irvani | 0:12479c9c5c72 | 34 | a = Kp + Ki*Ts/2; |
Najib_irvani | 0:12479c9c5c72 | 35 | b = -Kp + Ki*Ts/2; |
Najib_irvani | 0:12479c9c5c72 | 36 | |
Najib_irvani | 0:12479c9c5c72 | 37 | pwm = previous_pwm + a*current_error + b*previous_error1; |
Najib_irvani | 0:12479c9c5c72 | 38 | |
Najib_irvani | 0:12479c9c5c72 | 39 | if (pwm>1) pwm=1; |
Najib_irvani | 0:12479c9c5c72 | 40 | if (pwm<-1) pwm=-1; |
Najib_irvani | 0:12479c9c5c72 | 41 | |
Fathoni17 | 1:14997ad30b4e | 42 | if (pwm<batas_pwm_atas && pwm>batas_pwm_bawah){ |
Fathoni17 | 1:14997ad30b4e | 43 | pwm = batas_pwm_atas; |
Fathoni17 | 1:14997ad30b4e | 44 | } |
Fathoni17 | 1:14997ad30b4e | 45 | if (pwm>-1*batas_pwm_atas && pwm<-1*batas_pwm_bawah){ |
Fathoni17 | 1:14997ad30b4e | 46 | pwm = -1*batas_pwm_atas; |
Fathoni17 | 1:14997ad30b4e | 47 | } |
Fathoni17 | 1:14997ad30b4e | 48 | |
Najib_irvani | 0:12479c9c5c72 | 49 | powerScrew.speed(pwm); |
Najib_irvani | 0:12479c9c5c72 | 50 | |
Najib_irvani | 0:12479c9c5c72 | 51 | previous_pwm = pwm; |
Najib_irvani | 0:12479c9c5c72 | 52 | previous_error1 = current_error; |
Fathoni17 | 1:14997ad30b4e | 53 | |
Najib_irvani | 0:12479c9c5c72 | 54 | } |
Najib_irvani | 0:12479c9c5c72 | 55 | } |