Diff: main.cpp

Revision:

0:1d81f3296468

Child:

1:edd7f45256f9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 29 15:59:31 2018 +0000
@@ -0,0 +1,210 @@
+#include "mbed.h"
+
+///////////////////////PID///////////////////////
+int out, e, integral, derivative, preError, output;
+float Dt = 1;
+float Kp = 1;
+float Ki = 0;
+float Kd = 2;
+int setpoint = 0; 
+///////////////////////PID////////////////////////
+
+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);
+
+int count = 0;
+int pos1 = 0;
+int pos2 = 0;
+
+float roda1, roda2, roda3;
+float pw1,pw2,pw3;
+int pwma1, pwma2, pwma3;
+int abs_pwm1, abs_pwm2, abs_pwm3;
+double dx, dy, dz;
+
+void atur(double x, double  y, double the)
+{
+  double a = sqrt(3.);
+  roda1 = 1 * ((-0.5 * x) - (a / 2 * y)) + the;
+  roda2 = 1 * ((-0.5 * x) + (a / 2 * y)) + the;
+  roda3 = 1 * (x) + the;
+
+if (roda1 > 255)
+{pwma1 = 255;}
+else if (roda1 < -255)
+{pwma1 = -255;}
+else pwma1 = roda1;
+
+
+if (roda2 > 255)
+{pwma2 = 255;}
+else if (roda2 < -255)
+{pwma2 = -255;}
+else pwma2 = roda2;
+
+if (roda3 > 255)
+{pwma3 = 255;}
+else if (roda3 < -255)
+{pwma3 = -255;}
+else pwma3 = roda3;
+
+  if (pwma1 < 0) {
+    abs_pwm1 = abs(pwma1);
+    pw1=(float) abs_pwm1/255;
+    dir1 = 1;
+    dir2 = 0;
+  }
+  else {
+    abs_pwm1 = pwma1;
+    pw1=(float) abs_pwm1/255;
+    dir1 = 0;
+    dir2 = 1;
+  }
+  if (pwma2 < 0) {
+    abs_pwm2 = abs(pwma2);
+    pw2=(float) abs_pwm2/255;
+    dir3 = 1;
+    dir4 = 0;
+  }
+  else {
+    abs_pwm2 = pwma2;
+    pw2=(float) abs_pwm2/255;
+    dir3 = 0;
+    dir4 = 1;
+  }
+  if (pwma3 < 0) {
+    abs_pwm3 = abs(pwma3);
+    pw3=(float) abs_pwm3/255;
+    dir5 = 0;
+    dir6 = 1;
+  }
+  else {
+    abs_pwm3 = pwma3;
+    pw3=(float) abs_pwm3/255;
+    dir5 = 1;
+    dir6 = 0;
+  }
+}
+
+void berhenti() {
+  pwm1 = 0.5;
+  pwm2 = 0.5;
+  pwm3 = 0.5;
+  dir1 = 1;
+  dir2 = 1;
+  dir3 = 1;
+  dir4 = 1;
+  dir5 = 1;
+  dir6 = 1;
+}
+
+
+int main() 
+{
+pc.baud(9600);  
+bt.baud(9600);
+pc.printf("saaaarttt");
+/////////////////////////////////PID///////////////////////////////
+preError = 0;
+integral = 0; 
+/////////////////////////////////PID/////////////////////////////////
+pc.printf("starttttttt\n");
+   
+   
+while(1) {
+
+    if(bt.readable())
+    {
+        ch1 = bt.getc();
+        ch = ch1 - 90;
+        if(ch != -34)
+        {
+        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);
+        
+        //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);
+        }
+        else
+        {
+        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");
+     //  atur(-150, 0, 0); //kiri
+     //   pwm1.write(pw1);
+     //   pwm2.write(pw2);
+     //   pwm3.write(pw3);
+        
+    //}
+    //else if (data == 'g') {
+   //   pc.printf("Kiri");
+   //   atur(-60, 0, 0); //kiri
+    //}
+    //else if (data == 'i') {
+   //   pc.printf("Kanan");
+   //   atur(60, 0, 0); //kanan
+    //}
+   
+    //else if (data == 'l')
+    //{
+    //  atur(0, 0, -45); //Putar
+    //  pc.printf("Putar Kanan");
+    //  data = 0;
+    //}
+    //else if (data == 'j')
+    //{
+    //  atur(0, 0, 45); //Putar
+    //  pc.printf("Putar Kiri");
+     // data = 0;
+    //}
+ 
+ // else
+ // {
+ //   berhenti();
+ //   pc.printf("Stop");
+ //   }
+    
+    }
+}