KRAI 2017
/
new
dear fatoni
Diff: main.cpp
- Revision:
- 0:12479c9c5c72
- Child:
- 1:14997ad30b4e
diff -r 000000000000 -r 12479c9c5c72 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 15 06:59:16 2017 +0000 @@ -0,0 +1,44 @@ +#include "mbed.h" +#include "Ping.h" +#include "Motor.h" + +#define PIN PC_15 + +Ping ping(PIN); +Motor powerScrew(PB_10, PB_14, PB_13); +Serial pc(USBTX,USBRX); + +int jarak; +double target = 90; +double a,b,c; +double current_error, previous_error1 = 0; +double Kp = -0.003753, Ki = -1.001e-06, Ts=30; +double pwm, previous_pwm = 0; + +int main(){ + pc.baud(115200); + powerScrew.speed(0.5); + while(1){ + int jarak; + ping.Send(); + wait_ms(10); + jarak = ping.Read_cm(); + + pc.printf("%d\n", jarak); + /* + current_error = (double) (target-jarak); + a = Kp + Ki*Ts/2; + b = -Kp + Ki*Ts/2; + + pwm = previous_pwm + a*current_error + b*previous_error1; + + if (pwm>1) pwm=1; + if (pwm<-1) pwm=-1; + + powerScrew.speed(pwm); + + previous_pwm = pwm; + previous_error1 = current_error; + */ + } +} \ No newline at end of file