KATAPANG
/
PID1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Revision 2:a214cf60ce58, committed 2018-11-01
- Comitter:
- arayhan878
- Date:
- Thu Nov 01 17:22:26 2018 +0000
- Parent:
- 1:edd7f45256f9
- Commit message:
- yang baru
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);
}