Logika Rozmyta i robot Line Follower

[piotr3332]

Witam

Zaimplementowałem algorytm logiki rozmytej dla swojego robota. Posiada on 5 sensorw i prędkosc sterowana jest poprzez odjęcie wartości po defuzyfikacji od jednego z motorow. Niestety jesli chodzi o programowanie jestem początkujący. Przy normalnej prędkosci, nie za szybkiej ani za wolnej, moj robot radzi sobie bardzo dobrze. Jednak gdy troche zwiększam prędkośc, robot nie podąrza lini we właściwy sposob i po prostu zazwyczaj gubi linie. Bawiłem sie z wagami ale nie za bardzo to pomaga.

Jezeli ktoś zna sie na logice rozmytej bardzo prosiłbym o pomoc co mogłbym zrobić aby ten problem rozwiązac. Poniżej podaje moj kod

#include "mbed.h"


//------MOTOR----------
I2C i2c(p9, p10);        // sda, scl

PwmOut pwm0(p22);
PwmOut pwm1(p23);





const int addr = 0x40; // define the I2C Address


//----COmmand Definition-----------

#define FORWARD 0x66
#define RIGHT 0x55
#define LEFT  0xAA
#define RIGHT_STOP  0x77
#define LEFT_STOP  0xEE



//-----------LINE SENSOR--------------------

DigitalOut   led(LED1);                                  // status LED
DigitalOut   test(p27);                                  // test pin

DigitalInOut ir_rx0(p11);  // IR recieves
DigitalInOut ir_rx1(p12);  // IR recieves
DigitalInOut ir_rx2(p15);  // IR recieves
DigitalInOut ir_rx3(p16);  // IR recieves
DigitalInOut ir_rx4(p17);  // IR recieves
DigitalInOut ir_rx5(p18);  // IR recieves
DigitalInOut ir_rx6(p25);  // IR recieves
DigitalInOut ir_rx7(p26);  // IR recieves

PwmOut ir_tx(p24);                                       // IR transmitter

Serial pc(USBTX, USBRX);                                 // USB serial port

// Black  : 4.2x200us   = 840us
// White  : 2 x 100us   = 200us
// Silver : 1.2 x 100us = 120us

int findmin(int array[]);


int minspeed = 1500;  //predkosc minimalna
int maxspeed = 8000;  //predkosc maksymalna

int minlimit = 0; //wartosc minimalna dla sensora
int maxlimit = 100; //wartosc maksymalna dla sensora


int main() {

    //************MOTOR INIT******************
 char cmd[2];
 pwm0.period_us(20);          
 pwm1.period_ms(20);   


 int  ir_value[8];

 int Sensor_Value[8]; //Wartosc
 int Sensor_Low[8]; //Prawdziwosc ze napiecie dla sensora x jest niskie 
 int Sensor_High[8]; //Prawdziwosc ze napiecie dla sensora x jest wysokie 

 int current_pos; //aktualna pozycja


 //Dane ze wszystkich sensorow sa obliczane i ulokowane w tablicy tak aby minimalna wartosc mogla zostac obliczona
 int truth_left[5] = {0,0,0,0,0}; 
 int truth_right[5] = {0,0,0,0,0}; 
 int truth_centre[5] = {0,0,0,0,0};
 int truth_hard_right[5] = {0,0,0,0,0};
 int truth_hard_left[5] = {0,0,0,0,0};
 int truth_medium_left[5] = {0,0,0,0,0};
 int truth_medium_right[5] = {0,0,0,0,0};
 int truth_small_left[5] = {0,0,0,0,0};
 int truth_small_right[5] = {0,0,0,0,0};

 //zmiennia dla minimalnej wartosci dla kazdej z zasad
 int min_left; 
 int min_right;
 int min_centre;
 int min_hard_left;
 int min_hard_right;
 int min_small_right;
 int min_small_left;
 int min_medium_left;
 int min_medium_right;

 bool ir_set[8];


 int  ir_buffer;
 int  i, j, k;

 ir_tx.period_ms(20);       // a 20ms period
 ir_tx.pulsewidth_ms(20);    // 

 led  = 1;   
 test = 0;  

 int motorLeft; //predkosc dla motoru lewego
 int motorRight; //predkosc dla motoru prawego

 while(true)
 {  //BASE Speed for motor


   ir_rx0.output();
   ir_rx1.output();
   ir_rx2.output();
   ir_rx3.output(); 
   ir_rx4.output();
   ir_rx5.output();
   ir_rx6.output();
   ir_rx7.output(); 

   ir_rx0 = 1;
   ir_rx1 = 1;
   ir_rx2 = 1;
   ir_rx3 = 1;
   ir_rx4 = 1;
   ir_rx5 = 1;
   ir_rx6 = 1;
   ir_rx7 = 1; 

   for(i=0; i<8; i++)
   {
     ir_set[i]=true;
     ir_value[i]=0;
   }

   wait_us(50);

   ir_rx0.input(); 
   ir_rx1.input();  
   ir_rx2.input(); 
   ir_rx3.input(); 
   ir_rx4.input(); 
   ir_rx5.input();  
   ir_rx6.input(); 
   ir_rx7.input(); 

   test = 1;

   //Assigining a value from 0 to 20 to each sensor the minimum determines the strength
   for(i=0; i<20; i++)
   {
     wait_us(50);

     if ((ir_buffer && 0x01) == 0)
       test = 0;

     if((ir_rx0==0) & ir_set[0])
     {
       ir_set[0]=false; ir_value[0]=i; 
     }  
     if((ir_rx1==0) & ir_set[1])
     {
       ir_set[1]=false; ir_value[1]=i; 
     }  
     if((ir_rx2==0) & ir_set[2])
     {
       ir_set[2]=false; ir_value[2]=i; 
     }  
     if((ir_rx3==0) & ir_set[3])
     {
       ir_set[3]=false; ir_value[3]=i; 
     }  
     if((ir_rx4==0) & ir_set[4])
     {
       ir_set[4]=false; ir_value[4]=i; 
     }  
     if((ir_rx5==0) & ir_set[5])
     {
       ir_set[5]=false; ir_value[5]=i; 
     }  
     if((ir_rx6==0) & ir_set[6])
     {
       ir_set[6]=false; ir_value[6]=i;        
     }  
     if((ir_rx7==0) & ir_set[7])
     {
       ir_set[7]=false; ir_value[7]=i; 

     }  


   }

   //****************FUZZIFICATION*****************
   //Obliczanie wartosci dla danego sensora tak aby przedzial byl pomiedzy 0 a 100. Obliczany jest rowniez stopien prawdopodobiensta ze napiecie dla sensora jest wysokie czyli robot jest na linie albo niskie, robot jest poza linia. 

   Sensor_Value[1] = ((14 - ir_value[1])*16.666666667);
   if(Sensor_Value[1] < minlimit)
   {
   Sensor_Value[1] = minlimit;  
   }
   else if(Sensor_Value[1] > maxlimit)
   {
   Sensor_Value[1] = 100; 
   }
   //low and high values
   Sensor_Low[1] = 100 - Sensor_Value[1];
   Sensor_High[1] = Sensor_Value[1];

   Sensor_Value[2] = ((14 - ir_value[2])*16.666666667);
   if(Sensor_Value[2] < minlimit)
   {
   Sensor_Value[2] = minlimit; 
   }
   else if(Sensor_Value[2] > maxlimit)
   {
   Sensor_Value[2] = maxlimit; 
   }
   //low and high values
   Sensor_Low[2] = 100 - Sensor_Value[2];
   Sensor_High[2] = Sensor_Value[2];


   Sensor_Value[4]= (14 - ir_value[4])*16.6;   
   if(Sensor_Value[4] > maxlimit)
   {
   Sensor_Value[4] = maxlimit;
   }
   Sensor_Low[4] = 100 - Sensor_Value[4];
   Sensor_High[4] = Sensor_Value[4];



   Sensor_Value[5] = (13 - ir_value[5])*15;   
   if(Sensor_Value[5] < minlimit)
   {
   Sensor_Value[5] = minlimit; 
   }
   else if(Sensor_Value[5] > maxlimit)
   {
   Sensor_Value[5] = maxlimit; 
   }
   Sensor_Low[5] = 100 - Sensor_Value[5];
   Sensor_High[5] = Sensor_Value[5];


   Sensor_Value[7] = (13 - ir_value[7])*15;  
   if(Sensor_Value[7] < minlimit)
   {
   Sensor_Value[7] = minlimit; 
   }
   else if(Sensor_Value[7] > maxlimit)
   {
   Sensor_Value[7] = maxlimit; 
   }
   Sensor_Low[7] = 100 - Sensor_Value[7];
   Sensor_High[7] = Sensor_Value[7];




   //***********INFERENCE***************
   //tworzenie zasad i obliczanie minimalnej wartosci ktora zostanie uzyta przy procesie deffuzification

   //10000
   truth_hard_left[0] = Sensor_High[7];
   truth_hard_left[1] = Sensor_Low[5];
   truth_hard_left[2] = Sensor_Low[4];
   truth_hard_left[3] = Sensor_Low[2];
   truth_hard_left[4] = Sensor_Low[1]; 

   //finding minimum
   min_hard_left = findmin(truth_hard_left);

   //11000
   truth_medium_left[0] = Sensor_High[7];
   truth_medium_left[1] = Sensor_High[5];
   truth_medium_left[2] = Sensor_Low[4];
   truth_medium_left[3] = Sensor_Low[2];
   truth_medium_left[4] = Sensor_Low[1]; 

   //finding minimum
   min_medium_left = findmin(truth_medium_left);

   //01000
   truth_left[0] = Sensor_Low[7];
   truth_left[1] = Sensor_High[5];
   truth_left[2] = Sensor_Low[4];
   truth_left[3] = Sensor_Low[2];
   truth_left[4] = Sensor_Low[1]; 

   //finding minimum
   min_left = findmin(truth_left);


   //00010
   truth_right[0] = Sensor_Low[7];
   truth_right[1] = Sensor_Low[5];
   truth_right[2] = Sensor_Low[4];
   truth_right[3] = Sensor_High[2];
   truth_right[4] = Sensor_Low[1];


   //finding minimum
   min_right = findmin(truth_right);


   //00011
   truth_medium_right[0] = Sensor_Low[7];
   truth_medium_right[1] = Sensor_Low[5];
   truth_medium_right[2] = Sensor_Low[4];
   truth_medium_right[3] = Sensor_High[2];
   truth_medium_right[4] = Sensor_High[1];


   //finding minimum
   min_medium_right = findmin(truth_medium_right);

   //00001
   truth_hard_right[0] = Sensor_Low[7];
   truth_hard_right[1] = Sensor_Low[5];
   truth_hard_right[2] = Sensor_Low[4];
   truth_hard_right[3] = Sensor_Low[2];
   truth_hard_right[4] = Sensor_High[1];

   //finding minimum
   min_hard_right = findmin(truth_hard_right);
   printf( " min_left:= %d\n",   min_hard_right);


   //01100
   truth_small_left[0] = Sensor_Low[7];
   truth_small_left[1] = Sensor_High[5];
   truth_small_left[2] = Sensor_High[4];
   truth_small_left[3] = Sensor_Low[2];
   truth_small_left[4] = Sensor_Low[1]; 

   //finding minimum
   min_small_left = findmin(truth_small_left);


   //00110
   truth_small_right[0] = Sensor_Low[7];
   truth_small_right[1] = Sensor_Low[5];
   truth_small_right[2] = Sensor_High[4];
   truth_small_right[3] = Sensor_High[2];
   truth_small_right[4] = Sensor_Low[1];

   //finding minimum
   min_small_right = findmin(truth_small_right);


   //00100
   //Pozycja Centralna
   truth_centre[0] = Sensor_Low[7];
   truth_centre[1] = Sensor_Low[5];
   truth_centre[2] = Sensor_High[4];
   truth_centre[3] = Sensor_Low[2];
   truth_centre[4] = Sensor_Low[1];

   //Obliczanie dla pozycji centralnej
   min_centre = findmin(truth_centre);


   //deffuzification (centroid)

   //aktualna pozycja
   current_pos = ((35 *min_hard_left) + (25 * min_medium_left) + (20 * min_left) + (10 * min_small_left) + (-10 * min_small_right) + (-20 * min_right) + (-25 * min_medium_right)+ (-35 *min_hard_right) + (0 * min_centre)/(min_small_left+min_left+min_right+min_centre+min_hard_right+min_hard_left+min_medium_left +min_small_right + min_medium_right));
   printf( "   current_pos:= %d\n",    current_pos);



   if (current_pos>0)
   { // skret w prawo
   motorLeft = maxspeed;
   motorRight = maxspeed - current_pos;
   if (motorRight < minspeed) motorRight = minspeed;
   }
   else // current_pos <= 0
   { // skret w lewo
   motorLeft = maxspeed + current_pos;
   motorRight = maxspeed;
   if (motorLeft < minspeed) motorLeft = minspeed;
   }    


   //nadanie wartosci dla PWM
   pwm0.pulsewidth_us(motorLeft);

   pwm1.pulsewidth_us(motorRight); 


   cmd[0] = FORWARD;           
   cmd[1] = 0x00;           
   i2c.write(addr, cmd, 1); 


   }

}

//funkcja ktora oblicza wartosc minimalna
int findmin(int array[])
{
   int i, minimum;
   minimum = array[0];
   for(i = 0; i < 5; i++)
   {
       if(minimum > array[i])
       minimum = array[i];
   }
   return minimum;


}

[Sabre]

Po pierwsze popraw błędy bo tego postu nie da się czytać, po drugie nie wiem dlaczego robisz coś czego kompletnie nie rozumiesz i to na robocie, który nawet na PIDzie nie pojedzie dobrze ze względu na wady konstrukcyjne. Po trzecie nie rozumiem po co zaśmiecasz forum kolejnym tematem z cyklu nie działa, zróbcie wszystko za mnie.

[piotr3332]

Post pisałem na szybko, bez dostępu do polskich znakow i stąd te błędy. Juz poprawione. Biorę sie za to ze wzgledu ze muszę uzyć logiki rozmytej do sterowania robota. Pracuję nad projektem na studiach i dostałem gotowego juz robota, niestety to ze ma wady konstrukcji to nie moja wina. Poprawiłem PID i robot radzi sobie całkiem dobrze przy większych prędkosciach a więc poprzedni problem juz rozwiazałem. Zakładam temat po prostu dlatego ze moja wiedza na temat logiki rozmytej jest bardzo podstawowa, i chciałbym sie poradzić kogoś kto na niej zna sie lepiej. Podstawy juz sam napisałem, i tak jak wspomniałem algorytm działa tylko nie przy większych prędkościach.