File content as of revision 1:edd7f45256f9:

#include "mbed.h"

///////////////////////PID///////////////////////
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; 
//////////////////////////////////////////////////
////////////////////bluetooth////////////////////
Serial pc(USBTX, USBRX);
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;
float pwma1, pwma2, pwma3;
float abs_pwm1, abs_pwm2, abs_pwm3;
double dx, dy, dz;

//////////////////////inverse///////////////////////
void atur(float x, float  y, float 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 = 1;
    dir6 = 0;
  }
  else {
    abs_pwm3 = pwma3;
    pw3=(float) abs_pwm3/255;
    dir5 = 0;
    dir6 = 1;
  }
}

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("starttttttt\n");

/////////////////////////////////PID///////////////////////////////
preError = 0;
integral = 0; 
////////////////////////////////////////////////////////////////////
   
while(1) {
    
    if(bt.readable())
    {
        ch1 = bt.getc();
        ch = ch1*2 - 240;
        if(ch != -240)
        {
            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);
            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);
        }
    }
 
       //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;
    //}
 
    }
}