![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
bla
Dependencies: mbed
Fork of PID by
Diff: main.cpp
- Revision:
- 2:a214cf60ce58
- Parent:
- 1:edd7f45256f9
--- a/main.cpp Wed Oct 31 15:44:06 2018 +0000 +++ b/main.cpp Thu Nov 01 17:22:26 2018 +0000 @@ -5,9 +5,9 @@ float out, output; float Dt = 0.01; float Kp = 0.13; -float Ki = 0.01; -float Kd = 0.001; -int setpoint = 0; +float Ki = 0.024; +float Kd = 0.0014; +int setpoint = 120; ////////////////////////////////////////////////// ////////////////////bluetooth//////////////////// Serial pc(USBTX, USBRX); @@ -64,37 +64,37 @@ if (pwma1 < 0) { abs_pwm1 = abs(pwma1); - pw1=(float) abs_pwm1/255; + pw1=(float) abs_pwm1/200; dir1 = 1; dir2 = 0; } else { abs_pwm1 = pwma1; - pw1=(float) abs_pwm1/255; + pw1=(float) abs_pwm1/200; dir1 = 0; dir2 = 1; } if (pwma2 < 0) { abs_pwm2 = abs(pwma2); - pw2=(float) abs_pwm2/255; + pw2=(float) abs_pwm2/200; dir3 = 1; dir4 = 0; } else { abs_pwm2 = pwma2; - pw2=(float) abs_pwm2/255; + pw2=(float) abs_pwm2/200; dir3 = 0; dir4 = 1; } if (pwma3 < 0) { abs_pwm3 = abs(pwma3); - pw3=(float) abs_pwm3/255; + pw3=(float) abs_pwm3/200; dir5 = 1; dir6 = 0; } else { abs_pwm3 = pwma3; - pw3=(float) abs_pwm3/255; + pw3=(float) abs_pwm3/200; dir5 = 0; dir6 = 1; } @@ -129,12 +129,10 @@ if(bt.readable()) { ch1 = bt.getc(); - ch = ch1*2 - 240; - if(ch != -240) + ch = ch1; + if(ch != 0) { - pc.printf("yapppp "); - pc.printf(" sudut : %d",ch); - + //////////////////////////////////PID/////////////////////////////// e = setpoint - ch; integral = integral + (e * Dt); @@ -142,25 +140,61 @@ output = (Kp * e) + (Ki * integral) + (Kd * derivative); preError = e; //////////////////////////////////PID/////////////////////////////// + + out = (output*-1); + + if (e>20 || e<-20) + { + pc.printf(" PID"); + pc.printf(" yapppp "); + pc.printf(" sudut : %d",ch); + pc.printf(" out : %f",out); + pc.printf(" e : %f\n",e); + atur(0, 0, out); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + if (out > -10 && out < -1) + { + atur(0, 0, -10); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + wait(0.1); + } + if (out < 10 && out > 1) + { + atur(0, 0, 10); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + wait(0.1); + } + + } + if (e <20 && e>-20) + { + atur(0, -50, 0); + pc.printf(" Maju"); + pc.printf(" yapppp "); + pc.printf(" sudut : %d",ch); + pc.printf(" e : %f\n",e); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + } + // pc.printf(" out : %f",out); + // pc.printf(" pw1 : %f",pw1); + // pc.printf(" pw2 : %f",pw2); + // pc.printf(" pw3 : %f",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(" 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 ); + // pc.printf(" rod1 = %0.2f ", roda1 ); + // pc.printf(" rod2 = %0.2f ", roda2 ); + // pc.printf(" rod3 = %0.2f\n ", roda3 ); // wait(1); }