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

--- 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);
         }