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:
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...
#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