bla
Dependencies: mbed
Fork of PID by
Diff: main.cpp
- Revision:
- 1:edd7f45256f9
- Parent:
- 0:1d81f3296468
- Child:
- 2:a214cf60ce58
diff -r 1d81f3296468 -r edd7f45256f9 main.cpp --- a/main.cpp Wed Aug 29 15:59:31 2018 +0000 +++ b/main.cpp Wed Oct 31 15:44:06 2018 +0000 @@ -1,40 +1,43 @@ #include "mbed.h" ///////////////////////PID/////////////////////// -int out, e, integral, derivative, preError, output; -float Dt = 1; -float Kp = 1; -float Ki = 0; -float Kd = 2; +float e, integral, derivative, preError; +float out, output; +float Dt = 0.01; +float Kp = 0.13; +float Ki = 0.01; +float Kd = 0.001; int setpoint = 0; -///////////////////////PID//////////////////////// - +////////////////////////////////////////////////// +////////////////////bluetooth//////////////////// 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); +Serial bt(PB_6, PB_7); +///////////////////////////////////////////////// +/////////////////////motor/////////////////////// +DigitalOut dir5(PB_8); +DigitalOut dir6(PB_9); +DigitalOut dir3(PC_5); +DigitalOut dir4(PC_8); +DigitalOut dir1(PB_12); +DigitalOut dir2(PA_12); +PwmOut pwm3(PC_9); +PwmOut pwm2(PC_6); +PwmOut pwm1(PA_11); +//////////////////////////////////////////////// int count = 0; int pos1 = 0; int pos2 = 0; +int co, ch, ch1; float roda1, roda2, roda3; float pw1,pw2,pw3; -int pwma1, pwma2, pwma3; -int abs_pwm1, abs_pwm2, abs_pwm3; +float pwma1, pwma2, pwma3; +float abs_pwm1, abs_pwm2, abs_pwm3; double dx, dy, dz; -void atur(double x, double y, double the) +//////////////////////inverse/////////////////////// +void atur(float x, float y, float the) { double a = sqrt(3.); roda1 = 1 * ((-0.5 * x) - (a / 2 * y)) + the; @@ -47,7 +50,6 @@ {pwma1 = -255;} else pwma1 = roda1; - if (roda2 > 255) {pwma2 = 255;} else if (roda2 < -255) @@ -87,14 +89,14 @@ if (pwma3 < 0) { abs_pwm3 = abs(pwma3); pw3=(float) abs_pwm3/255; - dir5 = 0; - dir6 = 1; + dir5 = 1; + dir6 = 0; } else { abs_pwm3 = pwma3; pw3=(float) abs_pwm3/255; - dir5 = 1; - dir6 = 0; + dir5 = 0; + dir6 = 1; } } @@ -109,66 +111,69 @@ dir5 = 1; dir6 = 1; } - +//////////////////////////////////////////////// int main() { pc.baud(9600); bt.baud(9600); -pc.printf("saaaarttt"); +pc.printf("starttttttt\n"); + /////////////////////////////////PID/////////////////////////////// preError = 0; integral = 0; -/////////////////////////////////PID///////////////////////////////// -pc.printf("starttttttt\n"); - +//////////////////////////////////////////////////////////////////// while(1) { - + if(bt.readable()) { ch1 = bt.getc(); - ch = ch1 - 90; - if(ch != -34) + ch = ch1*2 - 240; + if(ch != -240) { - pc.printf("yapppp "); - pc.printf(" sudut : %d",ch); + 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); + //////////////////////////////////PID/////////////////////////////// + e = setpoint - ch; + integral = integral + (e * Dt); + derivative = (e - preError) / Dt; + output = (Kp * e) + (Ki * integral) + (Kd * derivative); + preError = e; + //////////////////////////////////PID/////////////////////////////// - //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); + out = output*-1; + atur(0, 0, out); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + pc.printf(" pre : %f",preError); + pc.printf(" out : %f",out); + pc.printf(" pw1 : %f",pw1); + pc.printf(" pw2 : %f",pw2); + pc.printf(" pw3 : %f",pw3); + + pc.printf(" I : %f",integral); + pc.printf(" D : %f",derivative); + pc.printf(" error : %f",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 ); + + // wait(1); } else { - pc.printf("nope \n"); - atur(0, 0, 0); - pwm1.write(pw1); - pwm2.write(pw2); - pwm3.write(pw3); + 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"); @@ -200,11 +205,5 @@ // data = 0; //} - // else - // { - // berhenti(); - // pc.printf("Stop"); - // } - } }