KRAI 2017 / Mbed 2 deprecated new

dear fatoni

Dependencies:   Motor Ping mbed

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?

UserRevisionLine numberNew 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 }