Go Down

Topic: Bateau en logique floue (Read 2425 times) previous topic - next topic

cherault

Bonjour à tous,

Je me suis lancé dans un vaste programme !
Faire un petit bateau piloté de façon autonome en logique floue, afin d'éviter des obstacles sur un plan d'eau.
Il est composé de:

1 capteur ultrason fixé à l'avant du bateau et monté fixe.
1 capteur ultrason fixé à l'avant du bateau et monté sur un servo qui suivra le même angle que le gouvernail du bateau.
En gros, lorsque le bateau va à gauche, le servo orientera la capteur ultrason à gauche également.
1 servo pour le gouvernail
1 servo pour la gestion de la vitesse du bateau

J'ai utilisé une librairie spécifique que vous trouverez ici:

http://zerokol.com/product/51e93616e84c5571b7000018/1/en

Le code que je vous donne est laissé avec les paramètres à 0 car il faut adapter ces derniers à vos demandes.
Il n'y a que deux règles en logique floue, mais j'avance.
Pour le moment, c'est un proof of concept, rien d'autre.

Voici le code...

Code: [Select]

#include <FuzzySet.h>
#include <FuzzyIO.h>
#include <FuzzyInput.h>
#include <FuzzyComposition.h>
#include <Fuzzy.h>
#include <FuzzyRule.h>
#include <FuzzyRuleAntecedent.h>
#include <FuzzyOutput.h>
#include <FuzzyRuleConsequent.h>
#include <Servo.h>

//declaration des servos
//----------------------
Servo servo_vit;
Servo servo_dir;
Servo servo_dis;

//pinout capteur de distance
//--------------------------
const int us_centre = 4;
const int us_servo = 2;

//declaration de l'instance "Fuzzy"
//---------------------------------
Fuzzy* fuzzy = new Fuzzy();

//calibration capteur de distance sur servo
//-----------------------------------------
FuzzySet* Svnear = new FuzzySet(0, 0, 0, 0);
FuzzySet* Snear = new FuzzySet(0, 0, 0, 0);
FuzzySet* Smedium = new FuzzySet(0, 0, 0, 0);
FuzzySet* Shigh = new FuzzySet(0, 0, 0, 0);

//calibration capteur de distance central
//---------------------------------------
FuzzySet* Cvnear = new FuzzySet(0, 0, 0, 0);
FuzzySet* Cnear = new FuzzySet(0, 0, 0, 0);
FuzzySet* Cmedium = new FuzzySet(0, 0, 0, 0);
FuzzySet* Chigh = new FuzzySet(0, 0, 0, 0);

//calibration pour le servo gestion vitesse
//-----------------------------------------
FuzzySet* StopIn = new FuzzySet(0, 0, 0, 0);
FuzzySet* LowIn = new FuzzySet(0, 0, 0, 0);
FuzzySet* CruseIn = new FuzzySet(0, 0, 0, 0);
FuzzySet* FastIn = new FuzzySet(0, 0, 0, 0);

void setup()
{
  //pinout des servos
  //-----------------
  servo_vit.attach(10);
  servo_dir.attach(11);
  servo_dis.attach(12);
 
  //calibration fuzzy ENTREE DISTANCE SERVO
  //---------------------------------------
  FuzzyInput* Sdist = new FuzzyInput(1); //ID ENTREE 1
 
  Sdist->addFuzzySet(Svnear);
  Sdist->addFuzzySet(Snear);
  Sdist->addFuzzySet(Smedium);
  Sdist->addFuzzySet(Shigh);
 
  fuzzy->addFuzzyInput(Sdist);
 
  //calibration fuzzy ENTREE DISTANCE CENTRE
  //----------------------------------------
  FuzzyInput* Cdist = new FuzzyInput(2); //ID ENTREE 2
 
  Cdist->addFuzzySet(Cvnear);
  Cdist->addFuzzySet(Cnear);
  Cdist->addFuzzySet(Cmedium);
  Cdist->addFuzzySet(Chigh);
 
  fuzzy->addFuzzyInput(Cdist);
 
  //calibration fuzzy ENTREE VITESSE
  //--------------------------------
  FuzzyInput* Vin = new FuzzyInput(3); //ID ENTREE 3
 
  Vin->addFuzzySet(StopIn);
  Vin->addFuzzySet(LowIn);
  Vin->addFuzzySet(CruseIn);
  Vin->addFuzzySet(FastIn);
 
  fuzzy->addFuzzyInput(Vin);
 
  //calibration fuzzy SORTIE SERVO VITESSE
  //--------------------------------------
  FuzzyOutput* Vout = new FuzzyOutput(1); //ID SORTIE 1
 
  FuzzySet* StopOut = new FuzzySet(0, 0, 0, 0);
  FuzzySet* LowOut = new FuzzySet(0, 0, 0, 0);
  FuzzySet* CruseOut = new FuzzySet(0, 0, 0, 0);
  FuzzySet* FastOut = new FuzzySet(0, 0, 0, 0);
 
  Vout->addFuzzySet(StopOut);
  Vout->addFuzzySet(LowOut);
  Vout->addFuzzySet(CruseOut);
  Vout->addFuzzySet(FastOut);
 
  fuzzy->addFuzzyOutput(Vout);
 
  //calibration fuzzy OUTPUT SERVO SAFRAN
  //-------------------------------------
  FuzzyOutput* Safran = new FuzzyOutput(2); //ID SORTIE 2
 
  FuzzySet* LeftFull = new FuzzySet(0, 0, 0, 0);
  FuzzySet* Left = new FuzzySet(0, 0, 0, 0);
  FuzzySet* Neutral = new FuzzySet(0, 0, 0, 0);
  FuzzySet* Right = new FuzzySet(0, 0, 0, 0);
  FuzzySet* RightFull = new FuzzySet(0, 0, 0, 0);
 
  Safran->addFuzzySet(LeftFull);
  Safran->addFuzzySet(Left);
  Safran->addFuzzySet(Neutral);
  Safran->addFuzzySet(Right);
  Safran->addFuzzySet(RightFull);
 
  fuzzy->addFuzzyOutput(Safran);
 
  //regles 1
  //IF Cdist AND Sdist = VERY NEAR
  //AND VIN = FAST
  //THEN SAFRAN = RIGHT FULL AND VOUT LOW
  //--------------------------------------
  FuzzyRuleAntecedent* CdistCvnearAndSdistSvnear = new FuzzyRuleAntecedent();
  CdistCvnearAndSdistSvnear->joinWithAND(Cvnear, Svnear);
 
  FuzzyRuleAntecedent* VinFastIn = new FuzzyRuleAntecedent();
  VinFastIn->joinSingle(FastIn);
 
  FuzzyRuleAntecedent* ifCdistCvnearAndSdistSvnearAndVinFastIn = new FuzzyRuleAntecedent();
  ifCdistCvnearAndSdistSvnearAndVinFastIn->joinWithAND(CdistCvnearAndSdistSvnear, VinFastIn);
 
  FuzzyRuleConsequent* thenVoutLowOutAndSafranRightFull = new FuzzyRuleConsequent();
  thenVoutLowOutAndSafranRightFull->addOutput(LowOut);
  thenVoutLowOutAndSafranRightFull->addOutput(RightFull);
 
  FuzzyRule* fuzzyRule1 = new FuzzyRule(1, ifCdistCvnearAndSdistSvnearAndVinFastIn, thenVoutLowOutAndSafranRightFull);
  fuzzy->addFuzzyRule(fuzzyRule1);
 
  //regles 2
  //IF Cdist AND Sdist = VERY FAR
  //AND VIN = LOW
  //THEN SAFRAN = NEUTRAL AND VOUT FAST
  //-----------------------------------
  FuzzyRuleAntecedent* CdistChighAndSdistShigh = new FuzzyRuleAntecedent();
  CdistChighAndSdistShigh->joinWithAND(Chigh, Shigh);
 
  FuzzyRuleAntecedent* VinLowIn = new FuzzyRuleAntecedent();
  VinLowIn->joinSingle(LowIn);
 
  FuzzyRuleAntecedent* ifCdistChighAndSdistShighAndVinLowIn = new FuzzyRuleAntecedent();
  ifCdistChighAndSdistShighAndVinLowIn->joinWithAND(CdistChighAndSdistShigh, VinLowIn);
 
  FuzzyRuleConsequent* thenVoutFastOutAndSafranNeutral = new FuzzyRuleConsequent();
  thenVoutFastOutAndSafranNeutral->addOutput(FastOut);
  thenVoutFastOutAndSafranNeutral->addOutput(Neutral);
 
  FuzzyRule* fuzzyRule2 = new FuzzyRule(1, ifCdistChighAndSdistShighAndVinLowIn, thenVoutFastOutAndSafranNeutral);
  fuzzy->addFuzzyRule(fuzzyRule2);
 
  }

void loop()
{
  long duration, dist_c, dist_s;
 
  //pulse capteur distance centre
  //-----------------------------
  pinMode(us_centre, OUTPUT);
  digitalWrite(us_centre, LOW);
  delayMicroseconds(2);
  digitalWrite(us_centre, HIGH);
  delayMicroseconds(5);
  digitalWrite(us_centre, LOW);

  pinMode(us_centre, INPUT);
  duration = pulseIn(us_centre, HIGH);
 
  //pulse capteur distance gauche
  //-----------------------------
  pinMode(us_servo, OUTPUT);
  digitalWrite(us_servo, LOW);
  delayMicroseconds(2);
  digitalWrite(us_servo, HIGH);
  delayMicroseconds(5);
  digitalWrite(us_servo, LOW);

  pinMode(us_servo, INPUT);
  duration = pulseIn(us_servo, HIGH);
 
  dist_c, dist_s = microsecondsToCentimeters(duration);
 
  fuzzy->setInput(1, us_centre);
  fuzzy->setInput(2, us_servo);
 
  fuzzy->fuzzify();
 
  Serial.print("Capteur Centre: ");
  Serial.print(Cvnear->getPertinence());
  Serial.print(", ");
  Serial.print(Cnear->getPertinence());
  Serial.print(", ");
  Serial.print(Cmedium->getPertinence());
  Serial.print(", ");
  Serial.print(Chigh->getPertinence());
  Serial.print(" ");
 
  Serial.print("Capteur Servo: ");
  Serial.print(Svnear->getPertinence());
  Serial.print(", ");
  Serial.print(Snear->getPertinence());
  Serial.print(", ");
  Serial.print(Smedium->getPertinence());
  Serial.print(", ");
  Serial.print(Shigh->getPertinence());
  Serial.print(" ");
 
  float output1 = fuzzy->defuzzify(1);
  float output2 = fuzzy->defuzzify(2);
 
  Serial.print("Vitesse:");
  Serial.println(output1);
 
  servo_vit.write(output1);
 
  Serial.print("Safran:");
  Serial.print(output2);
 
  servo_dir.write(output2);
  servi_dis.write(output2);
 
  delay(100);
 
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}


Amusez-vous bien.

Amitiés,

Chris

50% homme + 50% machine = 100% linux

Go Up