Diff: main.cpp

Revision:

1:edd7f45256f9

Parent:

0:1d81f3296468

Child:

2:a214cf60ce58

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