File content as of revision 0:1d81f3296468:

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