File content as of revision 1:679511199e94:

#include "mbed.h"
#include "SRF05.h"

SRF05 srf(PB_5, PB_3);

///////////////////////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;
        printf("Distance = %.1f\n", srf.read());              
        
        if(ch != -34) // bola ada di kamera tp tdk di tengah
        {
            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);
            
            if(e==0) //bola sudah di tengah
            {
                atur(0, 0, 0); //stop
                pwm1.write(pw1);
                pwm2.write(pw2);
                pwm3.write(pw3);
                wait(0.5f);
                atur(0, -50, 0); //maju samperin bola
                pwm1.write(pw1);
                pwm2.write(pw2);
                pwm3.write(pw3);
            }    
        }
        if((ch == -34)&&(srf.read()>5))  //bola tdk ada di kamera dan ultra tdk mendeteksi apa2
        {
            pc.printf("nope  \n");
            atur(0, 0, -45);
            pwm1.write(pw1);
            pwm2.write(pw2);
            pwm3.write(pw3);
        }
        if((ch == -34)&&(srf.read()<5))
        {
            atur(0, 0, 0);
            pwm1.write(pw1);
            pwm2.write(pw2);
            pwm3.write(pw3);
        }
    }
    else
    {
        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");
 //   }
    
    }
}