KATAPANG
/
PID
bla
Diff: main.cpp
- Revision:
- 0:1d81f3296468
- Child:
- 1:679511199e94
diff -r 000000000000 -r 1d81f3296468 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Aug 29 15:59:31 2018 +0000 @@ -0,0 +1,210 @@ +#include "mbed.h" + +///////////////////////PID/////////////////////// +int out, e, integral, derivative, preError, output; +float Dt = 1; +float Kp = 1; +float Ki = 0; +float Kd = 2; +int setpoint = 0; +///////////////////////PID//////////////////////// + +Serial pc(USBTX, USBRX); +Serial bt(PA_11, PA_12); + +int co, ch, ch1; + +DigitalOut dir1(PA_10); +DigitalOut dir2(PB_3); +DigitalOut dir3(PA_7); +DigitalOut dir4(PB_6); +DigitalOut dir5(PC_7); +DigitalOut dir6(PA_9); +PwmOut pwm1(PA_8); +PwmOut pwm2(PB_10); +PwmOut pwm3(PB_4); + +int count = 0; +int pos1 = 0; +int pos2 = 0; + +float roda1, roda2, roda3; +float pw1,pw2,pw3; +int pwma1, pwma2, pwma3; +int abs_pwm1, abs_pwm2, abs_pwm3; +double dx, dy, dz; + +void atur(double x, double y, double the) +{ + double a = sqrt(3.); + roda1 = 1 * ((-0.5 * x) - (a / 2 * y)) + the; + roda2 = 1 * ((-0.5 * x) + (a / 2 * y)) + the; + roda3 = 1 * (x) + the; + +if (roda1 > 255) +{pwma1 = 255;} +else if (roda1 < -255) +{pwma1 = -255;} +else pwma1 = roda1; + + +if (roda2 > 255) +{pwma2 = 255;} +else if (roda2 < -255) +{pwma2 = -255;} +else pwma2 = roda2; + +if (roda3 > 255) +{pwma3 = 255;} +else if (roda3 < -255) +{pwma3 = -255;} +else pwma3 = roda3; + + if (pwma1 < 0) { + abs_pwm1 = abs(pwma1); + pw1=(float) abs_pwm1/255; + dir1 = 1; + dir2 = 0; + } + else { + abs_pwm1 = pwma1; + pw1=(float) abs_pwm1/255; + dir1 = 0; + dir2 = 1; + } + if (pwma2 < 0) { + abs_pwm2 = abs(pwma2); + pw2=(float) abs_pwm2/255; + dir3 = 1; + dir4 = 0; + } + else { + abs_pwm2 = pwma2; + pw2=(float) abs_pwm2/255; + dir3 = 0; + dir4 = 1; + } + if (pwma3 < 0) { + abs_pwm3 = abs(pwma3); + pw3=(float) abs_pwm3/255; + dir5 = 0; + dir6 = 1; + } + else { + abs_pwm3 = pwma3; + pw3=(float) abs_pwm3/255; + dir5 = 1; + dir6 = 0; + } +} + +void berhenti() { + pwm1 = 0.5; + pwm2 = 0.5; + pwm3 = 0.5; + dir1 = 1; + dir2 = 1; + dir3 = 1; + dir4 = 1; + dir5 = 1; + dir6 = 1; +} + + +int main() +{ +pc.baud(9600); +bt.baud(9600); +pc.printf("saaaarttt"); +/////////////////////////////////PID/////////////////////////////// +preError = 0; +integral = 0; +/////////////////////////////////PID///////////////////////////////// +pc.printf("starttttttt\n"); + + +while(1) { + + if(bt.readable()) + { + ch1 = bt.getc(); + ch = ch1 - 90; + if(ch != -34) + { + pc.printf("yapppp "); + pc.printf(" sudut : %d",ch); + +//////////////////////////////////PID/////////////////////////////// +e = setpoint - ch; +integral = integral + (e * Dt); +derivative = (e - preError) / Dt; +output = (Kp * e) + (Ki * integral) + (Kd * derivative); +preError = e; +//////////////////////////////////PID/////////////////////////////// + out = output*-1; + atur(0, 0, out); + //pc.printf(" pw1 : %f",pw1); + //pc.printf(" pw2 : %f",pw2); + //pc.printf(" pw3 : %f",pw3); + + //pc.printf(" I : %d",integral); + //pc.printf(" D : %d",derivative); + //pc.printf(" error : %d",e); + //pc.printf(" pwm : %d",out); + pc.printf(" rod1 = %0.2f ", roda1 ); + pc.printf(" rod2 = %0.2f ", roda2 ); + pc.printf(" rod3 = %0.2f\n ", roda3 ); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + } + else + { + pc.printf("nope \n"); + atur(0, 0, 0); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + } + } + + + //atur(0, 150, 0); //mundur + //pc.printf("Mundur"); + // pc.printf("Kiri"); + // atur(-150, 0, 0); //kiri + // pwm1.write(pw1); + // pwm2.write(pw2); + // pwm3.write(pw3); + + //} + //else if (data == 'g') { + // pc.printf("Kiri"); + // atur(-60, 0, 0); //kiri + //} + //else if (data == 'i') { + // pc.printf("Kanan"); + // atur(60, 0, 0); //kanan + //} + + //else if (data == 'l') + //{ + // atur(0, 0, -45); //Putar + // pc.printf("Putar Kanan"); + // data = 0; + //} + //else if (data == 'j') + //{ + // atur(0, 0, 45); //Putar + // pc.printf("Putar Kiri"); + // data = 0; + //} + + // else + // { + // berhenti(); + // pc.printf("Stop"); + // } + + } +}