1) Code vi iu khin Arduino Robot Omni

#include <TimerOne.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <LiquidCrystal.h>
LiquidCrystal lcd(48, 46,53,51,49,47 );
HMC5883L compass;
float t=0;
int error = 0;
const int trig_x0 = 5;
const int inecho_x0 = 4;

const int trig_y0 = 7;

const int inecho_y0 = 6;

const int trig_y1 = 11;

const int inecho_y1 = 12;

int in1_1 = 32;

int in2_1 = 35;

int in1_2 = 33;

int in2_2 = 36;

int in1_3 = 31;

int in2_3 = 34;

int pwm_1 = 9; // xuat xung dc 1

int pwm_2 = 10;
int pwm_3 = 8;

long dem_1 = 0;
long dem_2 = 0;
long dem_3 = 0;

int vantoc_3 , vantoc_2, vantoc_1;

unsigned long tgian_x0, tgian_y0, tgian_y1;

int x_thuc, y_thuc, y_tam , kc_x0, kc_y0, kc_y1;
int preError_x = 0, preError_y = 0 ;
float kp , ki , kd ;
int x_dat, y_dat, a1, a2, x, y ;

int val, bientam;

// dien ap cap cho dong co

int u1 = 0, u2 = 0, u3 = 0;//int
int v1 = 0, v2 = 0, v3 = 0;
int duty1 = 0 , duty2 = 0, duty3 = 0; // int
float gocdat = 200 , gochientai;

int error_x, error_y;

float quangduonght, quangduongqk, de;

#include <FuzzyRule.h>
#include <FuzzyComposition.h>
#include <Fuzzy.h>
#include <FuzzyRuleConsequent.h>
#include <FuzzyOutput.h>
#include <FuzzyInput.h>
#include <FuzzyIO.h>
#include <FuzzySet.h>
#include <FuzzyRuleAntecedent.h>
Fuzzy* fuzzy = new Fuzzy();
float tam1, tam2;
FuzzySet* rnp = new FuzzySet(0, 0, 0, 0.25);
FuzzySet* np = new FuzzySet(0, 0.25, 0.25, 0.5);
FuzzySet* vp = new FuzzySet(0.25, 0.5, 0.5, 0.75);
FuzzySet* xp = new FuzzySet(0.5, 0.75, 0.75, 1);
FuzzySet* rxp = new FuzzySet(0.75, 1, 1, 1);

FuzzySet* anp = new FuzzySet(-1, -1, -1, -0.5);

FuzzySet* avp = new FuzzySet(-1, -0.5, -0.5, 0);
FuzzySet* zrp = new FuzzySet(-0.5, 0, 0, 0.5);
FuzzySet* dvp = new FuzzySet(0, 0.50, 0.50, 1);
FuzzySet* dnp = new FuzzySet(0.50, 1, 1, 1);

//******khau D**************

FuzzySet* rnd = new FuzzySet(0, 0, 0, 0.25);

FuzzySet* nd = new FuzzySet(0, 0.25, 0.25, 0.5);
FuzzySet* vd = new FuzzySet(0.25, 0.5, 0.5, 0.75);
FuzzySet* xd = new FuzzySet(0.5, 0.75, 0.75, 1);
FuzzySet* rxd = new FuzzySet(0.75, 1, 1, 1);

FuzzySet* an_d = new FuzzySet(-1, -1, -1, -0.5);

FuzzySet* avd = new FuzzySet(-1, -0.5, -0.5, 0);
FuzzySet* zrd = new FuzzySet(-0.5, 0, 0, 0.5);
FuzzySet* dvd = new FuzzySet(0, 0.50, 0.50, 1);
FuzzySet* dnd = new FuzzySet(0.50, 1, 1, 1);
//******khau I**************
FuzzySet* rni = new FuzzySet(0, 0, 0, 0.25);
FuzzySet* ni = new FuzzySet(0, 0.25, 0.25, 0.5);
FuzzySet* vi = new FuzzySet(0.25, 0.5, 0.5, 0.75);
FuzzySet* xi = new FuzzySet(0.5, 0.75, 0.75, 1);
FuzzySet* rxi = new FuzzySet(0.75, 1, 1, 1);

FuzzySet* ani = new FuzzySet(-1, -1, -1, -0.5);

FuzzySet* avi = new FuzzySet(-1, -0.5, -0.5, 0);
FuzzySet* zri = new FuzzySet(-0.5, 0, 0, 0.5);
FuzzySet* dvi = new FuzzySet(0, 0.50, 0.50, 1);
FuzzySet* dni = new FuzzySet(0.50, 1, 1, 1);

void DemEncoder_1();
void DemEncoder_2();
void DemEncoder_3();
void vantoc();
float goc();
float goc()
{
MagnetometerRaw raw = compass.ReadRawAxis();
MagnetometerScaled scaled = compass.ReadScaledAxis();
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
float heading = atan2(scaled.YAxis, scaled.XAxis);
float declinationAngle = 0.0457;
 heading += declinationAngle;
if(heading < 0)
heading += 2*PI;
if(heading > 2*PI)
heading -= 2*PI;
float headingDegrees = heading * 180/M_PI;
//Serial.print(" goc : ");
//Serial.println(headingDegrees);
return headingDegrees;
}
float khoangcach_x0()
{
digitalWrite(trig_x0, LOW);
delayMicroseconds(2); //tt chn pht
digitalWrite(trig_x0, HIGH);
delayMicroseconds(1000); // xut tn hiu 10us
digitalWrite(trig_x0, LOW); //tt nhn
tgian_x0 = pulseIn(inecho_x0, HIGH); // tgian nhn tn hiu

kc_x0 = 0.017 * tgian_x0; // khong cch n vt.(s = v(0,034) *

t(tgian/2) )
//delay(01);
return kc_x0;
}

float khoangcach_y0()
{
digitalWrite(trig_y0, LOW);
delayMicroseconds(2); //tt chn pht
 digitalWrite(trig_y0, HIGH);
delayMicroseconds(1000); // xut tn hiu 10us
digitalWrite(trig_y0, LOW); //tt nhn

tgian_y0 = pulseIn(inecho_y0, HIGH); // tgian nhn tn hiu

kc_y0 = 0.017 * tgian_y0; // khong cch n vt.[s = v(0,034) *

t(tgian/2)]
//delay(20);
return kc_y0;
}
//**********************************************************
**
//**********************************************************
**
void ngattimer()
{
de = (quangduonght - quangduongqk)/ 10;
quangduongqk = quangduonght;
t = t + 0.002;
if(t > 1000000){
t=1000000;
}
}

void setup()
{
Serial.begin (9600);
Wire.begin();
Serial.println("Bat dau: ");
pinMode(52,OUTPUT);

lcd.begin(16, 2);
lcd.print("Bat dau: ");
compass = HMC5883L();
error = compass.SetScale(1.3);
if(error != 0){
};
error = compass.SetMeasurementMode(Measurement_Continuous);
if(error != 0){
};
Timer1.initialize(10000);
Timer1.attachInterrupt( ngattimer );
pinMode(A0, INPUT);

pinMode(trig_x0, OUTPUT); // chn trig s pht tn hiu

pinMode(inecho_x0, INPUT); // chn echo s nhn tn hiu

pinMode(trig_y0, OUTPUT); // chn trig s pht tn hiu

pinMode(inecho_y0, INPUT);

pinMode(in1_1, OUTPUT); // dc1

pinMode(in2_1, OUTPUT);

pinMode(in1_2, OUTPUT);
pinMode(in2_2, OUTPUT);

pinMode(in1_3, OUTPUT);
pinMode(in2_3, OUTPUT);

pinMode(pwm_1, OUTPUT);
pinMode(pwm_2, OUTPUT);
pinMode(pwm_3, OUTPUT);

// FuzzyInput P(1)
FuzzyInput* ep = new FuzzyInput(1);
ep->addFuzzySet(rnp);
ep->addFuzzySet(np);
ep->addFuzzySet(vp);
ep->addFuzzySet(xp);
ep->addFuzzySet(rxp);

fuzzy->addFuzzyInput(ep);

// FuzzyInput P (2)
FuzzyInput* dep = new FuzzyInput(2);
dep->addFuzzySet(anp);
dep->addFuzzySet(avp);
dep->addFuzzySet(zrp);
dep->addFuzzySet(dvp);
dep->addFuzzySet(dnp);

fuzzy->addFuzzyInput(dep);

// FuzzyInput D (3)
FuzzyInput* ed = new FuzzyInput(3);
ed->addFuzzySet(rnd);
ed->addFuzzySet(nd);
ed->addFuzzySet(vd);
ed->addFuzzySet(xd);
ed->addFuzzySet(rxd);

fuzzy->addFuzzyInput(ed);
// FuzzyInput D (4)
FuzzyInput* ded = new FuzzyInput(4);
ded->addFuzzySet(an_d);
ded->addFuzzySet(avd);
ded->addFuzzySet(zrd);
ded->addFuzzySet(dvd);
ded->addFuzzySet(dnd);

fuzzy->addFuzzyInput(ded);
// FuzzyInput i (5)
FuzzyInput* ei = new FuzzyInput(5);
ei->addFuzzySet(rni);
ei->addFuzzySet(ni);
ei->addFuzzySet(vi);
ei->addFuzzySet(xi);
ei->addFuzzySet(rxi);

fuzzy->addFuzzyInput(ei);
// FuzzyInput i (6)
FuzzyInput* dei = new FuzzyInput(6);
dei->addFuzzySet(ani);
dei->addFuzzySet(avi);
dei->addFuzzySet(zri);
dei->addFuzzySet(dvi);
dei->addFuzzySet(dni);

fuzzy->addFuzzyInput(dei);

// FuzzyOutput
FuzzyOutput* khaup = new FuzzyOutput(1);

FuzzySet* khp = new FuzzySet(0, 0, 0, 0.25);

khaup->addFuzzySet(khp);
FuzzySet* nhp = new FuzzySet(0, 0.25, 0.25, 0.5);
khaup->addFuzzySet(nhp);
FuzzySet* tbp = new FuzzySet(0.25, 0.5, 0.5, 0.75);
khaup->addFuzzySet(tbp);
FuzzySet* lp = new FuzzySet(0.5, 0.75, 0.75, 1);
khaup->addFuzzySet(lp);
FuzzySet* rlp = new FuzzySet(0.75, 1, 1, 1);
khaup->addFuzzySet(rlp);

fuzzy->addFuzzyOutput(khaup);

// FuzzyOutput 2
FuzzyOutput* khaud = new FuzzyOutput(2);

FuzzySet* khd = new FuzzySet(0, 0, 0, 0.25);

khaud->addFuzzySet(khd);
FuzzySet* nhd = new FuzzySet(0, 0.25, 0.25, 0.5);
khaud->addFuzzySet(nhd);
 FuzzySet* tbd = new FuzzySet(0.25, 0.5, 0.5, 0.75);
khaud->addFuzzySet(tbd);
FuzzySet* ld = new FuzzySet(0.5, 0.75, 0.75, 1);
khaud->addFuzzySet(ld);
FuzzySet* rld = new FuzzySet(0.75, 1, 1, 1);
khaud->addFuzzySet(rld);

fuzzy->addFuzzyOutput(khaud);

// FuzzyOutput 3
FuzzyOutput* khaui = new FuzzyOutput(3);

FuzzySet* khi = new FuzzySet(0, 0, 0, 0.25);

khaui->addFuzzySet(khi);
FuzzySet* nhi = new FuzzySet(0, 0.25, 0.25, 0.5);
khaui->addFuzzySet(nhi);
FuzzySet* tbi = new FuzzySet(0.25, 0.5, 0.5, 0.75);
khaui->addFuzzySet(tbi);
FuzzySet* li = new FuzzySet(0.5, 0.75, 0.75, 1);
khaui->addFuzzySet(li);
FuzzySet* rli = new FuzzySet(0.75, 1, 1, 1);
khaui->addFuzzySet(rli);

fuzzy->addFuzzyOutput(khaui);

// Building FuzzyRule 1 "rnp and anp = khp1"

FuzzyRuleAntecedent* IfEpRnpAndDepAnp = new

FuzzyRuleAntecedent();
 IfEpRnpAndDepAnp->joinWithAND(rnp, anp);

FuzzyRuleConsequent* thenKhaupKhp1 = new

FuzzyRuleConsequent();
thenKhaupKhp1->addOutput(khp);

FuzzyRule* fuzzyRule01 = new FuzzyRule(1, IfEpRnpAndDepAnp,

thenKhaupKhp1);
fuzzy->addFuzzyRule(fuzzyRule01);

// Building FuzzyRule 2 "rnp and avp = nhp1"

FuzzyRuleAntecedent* IfEpRnpAndDepAvp = new
FuzzyRuleAntecedent();
IfEpRnpAndDepAvp->joinWithAND(rnp, avp);

FuzzyRuleConsequent* thenKhaupNhp1 = new

FuzzyRuleConsequent();
thenKhaupNhp1->addOutput(nhp);

FuzzyRule* fuzzyRule02 = new FuzzyRule(2, IfEpRnpAndDepAvp,

thenKhaupNhp1);
fuzzy->addFuzzyRule(fuzzyRule02);

// Building FuzzyRule 3 "rnp and zrp = khp2"

FuzzyRuleAntecedent* IfEpRnpAndDepZrp = new
FuzzyRuleAntecedent();
IfEpRnpAndDepZrp->joinWithAND(rnp, zrp);

FuzzyRuleConsequent* thenKhaupKhp2 = new

FuzzyRuleConsequent();
thenKhaupKhp2->addOutput(khp);
 FuzzyRule* fuzzyRule03 = new FuzzyRule(3, IfEpRnpAndDepZrp,
thenKhaupKhp2);
fuzzy->addFuzzyRule(fuzzyRule03);

// Building FuzzyRule 4 "rnp and dvp = nhp2"

FuzzyRuleAntecedent* IfEpRnpAndDepDvp = new
FuzzyRuleAntecedent();
IfEpRnpAndDepDvp->joinWithAND(rnp, dvp);

FuzzyRuleConsequent* thenKhaupNhp2 = new

FuzzyRuleConsequent();
thenKhaupNhp2->addOutput(nhp);

FuzzyRule* fuzzyRule04 = new FuzzyRule(4, IfEpRnpAndDepDvp,

thenKhaupNhp2);
fuzzy->addFuzzyRule(fuzzyRule04);

// Building FuzzyRule 5 "rnp and dnp = khp3"

FuzzyRuleAntecedent* IfEpRnpAndDepDnp = new
FuzzyRuleAntecedent();
IfEpRnpAndDepDnp->joinWithAND(rnp, dnp);

FuzzyRuleConsequent* thenKhaupKhp3 = new

FuzzyRuleConsequent();
thenKhaupKhp3->addOutput(khp);

FuzzyRule* fuzzyRule05 = new FuzzyRule(5, IfEpRnpAndDepDnp,

thenKhaupKhp3);
fuzzy->addFuzzyRule(fuzzyRule05);
 // Building FuzzyRule 6 "np and anp = nhp3"
FuzzyRuleAntecedent* IfEpNpAndDepAnp = new
FuzzyRuleAntecedent();
IfEpNpAndDepAnp->joinWithAND(np, anp);

FuzzyRuleConsequent* thenKhaupNhp3 = new

FuzzyRuleConsequent();
thenKhaupNhp3->addOutput(nhp);

FuzzyRule* fuzzyRule06 = new FuzzyRule(6, IfEpNpAndDepAnp,

thenKhaupNhp3);
fuzzy->addFuzzyRule(fuzzyRule06);

// Building FuzzyRule 7 "np and avp = tbp1"

FuzzyRuleAntecedent* IfEpNpAndDepAvp = new
FuzzyRuleAntecedent();
IfEpNpAndDepAvp->joinWithAND(np, avp);

FuzzyRuleConsequent* thenKhaupTbp1 = new FuzzyRuleConsequent();

thenKhaupTbp1->addOutput(tbp);

FuzzyRule* fuzzyRule07 = new FuzzyRule(7, IfEpNpAndDepAvp,

thenKhaupTbp1);
fuzzy->addFuzzyRule(fuzzyRule07);

// Building FuzzyRule 8 "np and zrp = nhp4"

FuzzyRuleAntecedent* IfEpNpAndDepZrp = new
FuzzyRuleAntecedent();
IfEpNpAndDepZrp->joinWithAND(np, zrp);
 FuzzyRuleConsequent* thenKhaupNhp4 = new
FuzzyRuleConsequent();
thenKhaupNhp4->addOutput(nhp);

FuzzyRule* fuzzyRule08 = new FuzzyRule(8, IfEpNpAndDepZrp,

thenKhaupNhp4);
fuzzy->addFuzzyRule(fuzzyRule08);

// Building FuzzyRule 9 "np and dvp = tbp2"

FuzzyRuleAntecedent* IfEpNpAndDepDvp = new
FuzzyRuleAntecedent();
IfEpNpAndDepDvp->joinWithAND(np, dvp);

FuzzyRuleConsequent* thenKhaupTbp2 = new FuzzyRuleConsequent();

thenKhaupTbp2->addOutput(tbp);

FuzzyRule* fuzzyRule09 = new FuzzyRule(9, IfEpNpAndDepDvp,

thenKhaupTbp2);
fuzzy->addFuzzyRule(fuzzyRule09);

// Building FuzzyRule 10"np and dnp = nhp5"

FuzzyRuleAntecedent* IfEpNpAndDepDnp = new
FuzzyRuleAntecedent();
IfEpNpAndDepDnp->joinWithAND(np, dnp);

FuzzyRuleConsequent* thenKhaupNhp5 = new

FuzzyRuleConsequent();
thenKhaupNhp5->addOutput(nhp);

FuzzyRule* fuzzyRule10 = new FuzzyRule(10, IfEpNpAndDepDnp,

thenKhaupNhp5);
 fuzzy->addFuzzyRule(fuzzyRule10);

// Building FuzzyRule 11 "vp and anp = tbp3"

FuzzyRuleAntecedent* IfEpVpAndDepAnp = new
FuzzyRuleAntecedent();
IfEpVpAndDepAnp->joinWithAND(vp, anp);

FuzzyRuleConsequent* thenKhaupTbp3 = new FuzzyRuleConsequent();

thenKhaupTbp3->addOutput(tbp);

FuzzyRule* fuzzyRule11 = new FuzzyRule(11, IfEpNpAndDepDnp,

thenKhaupTbp3);
fuzzy->addFuzzyRule(fuzzyRule11);

// Building FuzzyRule 12 "vp and avp = lp1"

FuzzyRuleAntecedent* IfEpVpAndDepAvp = new
FuzzyRuleAntecedent();
IfEpVpAndDepAvp->joinWithAND(vp, avp);

FuzzyRuleConsequent* thenKhaupLp1 = new FuzzyRuleConsequent();

thenKhaupLp1->addOutput(lp);

FuzzyRule* fuzzyRule12 = new FuzzyRule(12, IfEpRnpAndDepAvp,

thenKhaupLp1);
fuzzy->addFuzzyRule(fuzzyRule12);

// Building FuzzyRule 13 "vp and zrp = rlp1"

FuzzyRuleAntecedent* IfEpVpAndDepZrp = new
FuzzyRuleAntecedent();
IfEpVpAndDepZrp->joinWithAND(vp, zrp);
 FuzzyRuleConsequent* thenKhaupRlp1 = new FuzzyRuleConsequent();
thenKhaupRlp1->addOutput(rlp);

FuzzyRule* fuzzyRule13 = new FuzzyRule(13, IfEpVpAndDepZrp,

thenKhaupRlp1);
fuzzy->addFuzzyRule(fuzzyRule13);

// Building FuzzyRule 14 "vp and dvp = lp2"

FuzzyRuleAntecedent* IfEpVpAndDepDvp = new
FuzzyRuleAntecedent();
IfEpVpAndDepDvp->joinWithAND(vp, dvp);

FuzzyRuleConsequent* thenKhaupLp2 = new FuzzyRuleConsequent();

thenKhaupLp2->addOutput(lp);

FuzzyRule* fuzzyRule14 = new FuzzyRule(14, IfEpVpAndDepDvp,

thenKhaupLp2);
fuzzy->addFuzzyRule(fuzzyRule14);

// Building FuzzyRule 15 "vp and dnp = tbp4"

FuzzyRuleAntecedent* IfEpVpAndDepDnp = new
FuzzyRuleAntecedent();
IfEpVpAndDepDnp->joinWithAND(vp, dnp);

FuzzyRuleConsequent* thenKhaupTbp4 = new FuzzyRuleConsequent();

thenKhaupTbp4->addOutput(tbp);
 FuzzyRule* fuzzyRule15 = new FuzzyRule(15, IfEpVpAndDepDnp,
thenKhaupTbp4);
fuzzy->addFuzzyRule(fuzzyRule15);

// Building FuzzyRule 16 "xp and anp = lp3"

FuzzyRuleAntecedent* IfEpXpAndDepAnp = new
FuzzyRuleAntecedent();
IfEpXpAndDepAnp->joinWithAND(xp, anp);

FuzzyRuleConsequent* thenKhaupLp3 = new FuzzyRuleConsequent();

thenKhaupLp3->addOutput(lp);

FuzzyRule* fuzzyRule16 = new FuzzyRule(16, IfEpXpAndDepAnp,

thenKhaupLp3);
fuzzy->addFuzzyRule(fuzzyRule16);

// Building FuzzyRule 17 "xp and avp = rlp2"

FuzzyRuleAntecedent* IfEpXpAndDepAvp = new
FuzzyRuleAntecedent();
IfEpXpAndDepAvp->joinWithAND(xp, avp);

FuzzyRuleConsequent* thenKhaupRlp2 = new FuzzyRuleConsequent();

thenKhaupRlp2->addOutput(rlp);

FuzzyRule* fuzzyRule17 = new FuzzyRule(17, IfEpXpAndDepAvp,

thenKhaupRlp2);
fuzzy->addFuzzyRule(fuzzyRule17);

// Building FuzzyRule 18 "xp and zrp = rlp3"

 FuzzyRuleAntecedent* IfEpXpAndDepZrp = new
FuzzyRuleAntecedent();
IfEpXpAndDepZrp->joinWithAND(xp, zrp);

FuzzyRuleConsequent* thenKhaupRlp3 = new FuzzyRuleConsequent();

thenKhaupRlp3->addOutput(rlp);

FuzzyRule* fuzzyRule18 = new FuzzyRule(18, IfEpXpAndDepZrp,

thenKhaupRlp3);
fuzzy->addFuzzyRule(fuzzyRule18);

// Building FuzzyRule 19 "xp and dvp = rlp4"

FuzzyRuleAntecedent* IfEpXpAndDepDvp = new
FuzzyRuleAntecedent();
IfEpXpAndDepDvp->joinWithAND(xp, dvp);

FuzzyRuleConsequent* thenKhaupRlp4 = new FuzzyRuleConsequent();

thenKhaupRlp4->addOutput(rlp);

FuzzyRule* fuzzyRule19 = new FuzzyRule(19, IfEpXpAndDepDvp,

thenKhaupRlp4);
fuzzy->addFuzzyRule(fuzzyRule19);

// ************Building FuzzyRule 20 "xp and dnp =

rlp5"***************
FuzzyRuleAntecedent* IfEpXpAndDepDnp = new
FuzzyRuleAntecedent();
IfEpXpAndDepDnp->joinWithAND(xp, dnp);

FuzzyRuleConsequent* thenKhaupRlp5 = new FuzzyRuleConsequent();

 thenKhaupRlp5->addOutput(rlp);

FuzzyRule* fuzzyRule20 = new FuzzyRule(20, IfEpXpAndDepDnp,

thenKhaupRlp5);
fuzzy->addFuzzyRule(fuzzyRule20);

// Building FuzzyRule 21 "rxp and anp = rlp6"

FuzzyRuleAntecedent* IfEpRxpAndDepAnp = new
FuzzyRuleAntecedent();
IfEpRxpAndDepAnp->joinWithAND(rxp, anp);

FuzzyRuleConsequent* thenKhaupRlp6 = new FuzzyRuleConsequent();

thenKhaupRlp6->addOutput(rlp);

FuzzyRule* fuzzyRule21 = new FuzzyRule(21, IfEpRxpAndDepAnp,

thenKhaupRlp6);
fuzzy->addFuzzyRule(fuzzyRule21);

// Building FuzzyRule 22 "rxp and avp = rlp7"

FuzzyRuleAntecedent* IfEpRxpAndDepAvp = new
FuzzyRuleAntecedent();
IfEpRxpAndDepAvp->joinWithAND(rxp, avp);

FuzzyRuleConsequent* thenKhaupRlp7 = new FuzzyRuleConsequent();

thenKhaupRlp7->addOutput(rlp);

FuzzyRule* fuzzyRule22 = new FuzzyRule(22, IfEpRxpAndDepAvp,

thenKhaupRlp7);
fuzzy->addFuzzyRule(fuzzyRule22);
 // Building FuzzyRule 23"rxp and zrp = rlp8"
FuzzyRuleAntecedent* IfEpRxpAndDepZrp = new
FuzzyRuleAntecedent();
IfEpRxpAndDepZrp->joinWithAND(rxp, zrp);

FuzzyRuleConsequent* thenKhaupRlp8 = new FuzzyRuleConsequent();

thenKhaupRlp8->addOutput(rlp);

FuzzyRule* fuzzyRule23 = new FuzzyRule(23, IfEpRxpAndDepZrp,

thenKhaupRlp8);
fuzzy->addFuzzyRule(fuzzyRule23);

// Building FuzzyRule 24"rxp and dvp = rlp9"

FuzzyRuleAntecedent* IfEpRxpAndDepDvp = new
FuzzyRuleAntecedent();
IfEpRxpAndDepDvp->joinWithAND(rxp, dvp);

FuzzyRuleConsequent* thenKhaupRlp9 = new FuzzyRuleConsequent();

thenKhaupRlp9->addOutput(rlp);

FuzzyRule* fuzzyRule24 = new FuzzyRule(24, IfEpRxpAndDepDvp,

thenKhaupRlp9);
fuzzy->addFuzzyRule(fuzzyRule24);

// Building FuzzyRule 25"rxp and dnp = rlp10"

FuzzyRuleAntecedent* IfEpRxpAndDepDnp = new
FuzzyRuleAntecedent();
IfEpRxpAndDepDnp->joinWithAND(rxp, dnp);
 FuzzyRuleConsequent* thenKhaupRlp10 = new
FuzzyRuleConsequent();
thenKhaupRlp10->addOutput(rlp);

FuzzyRule* fuzzyRule25 = new FuzzyRule(25, IfEpRxpAndDepDnp,

thenKhaupRlp10);
fuzzy->addFuzzyRule(fuzzyRule25);

//**********************************************************
**************
// ******************************** luat mo khau D ***********

//**********************************************************
**************
// Building FuzzyRule 26 "rnd and and = rld1"

FuzzyRuleAntecedent* IfEdRndAndDedAn_d = new

FuzzyRuleAntecedent();
IfEdRndAndDedAn_d->joinWithAND(rnd, an_d);

FuzzyRuleConsequent* thenKhaudRld1 = new FuzzyRuleConsequent();

thenKhaudRld1->addOutput(rld);

FuzzyRule* fuzzyRule26 = new FuzzyRule(26, IfEdRndAndDedAn_d,

thenKhaudRld1);
fuzzy->addFuzzyRule(fuzzyRule26);

// Building FuzzyRule 27 "rnd and avd = ld1"

 FuzzyRuleAntecedent* IfEdRndAndDedAvd = new
FuzzyRuleAntecedent();
IfEdRndAndDedAvd->joinWithAND(rnd, avd);

FuzzyRuleConsequent* thenKhaudLd1 = new FuzzyRuleConsequent();

thenKhaudLd1->addOutput(ld);

FuzzyRule* fuzzyRule27 = new FuzzyRule(27, IfEdRndAndDedAvd,

thenKhaudLd1);
fuzzy->addFuzzyRule(fuzzyRule27);

// Building FuzzyRule 28 "rnd and zrd = khd1"

FuzzyRuleAntecedent* IfEdRndAndDedZrd = new
FuzzyRuleAntecedent();
IfEdRndAndDedZrd->joinWithAND(rnd, zrd);

FuzzyRuleConsequent* thenKhaudKhd1 = new

FuzzyRuleConsequent();
thenKhaudKhd1->addOutput(khd);

FuzzyRule* fuzzyRule28 = new FuzzyRule(28, IfEdRndAndDedZrd,

thenKhaudKhd1);
fuzzy->addFuzzyRule(fuzzyRule28);

// Building FuzzyRule 29 "rnd and dvd = ld2"

FuzzyRuleAntecedent* IfEdRndAndDedDvd = new
FuzzyRuleAntecedent();
IfEdRndAndDedDvd->joinWithAND(rnd, dvd);

FuzzyRuleConsequent* thenKhaudLd2 = new FuzzyRuleConsequent();

 thenKhaudLd2->addOutput(ld);

FuzzyRule* fuzzyRule29 = new FuzzyRule(29, IfEdRndAndDedDvd,

thenKhaudLd2);
fuzzy->addFuzzyRule(fuzzyRule29);

// Building FuzzyRule 30 "rnd and dnd = rld2"

FuzzyRuleAntecedent* IfEdRndAndDedDnd = new
FuzzyRuleAntecedent();
IfEdRndAndDedDnd->joinWithAND(rnd, dnd);

FuzzyRuleConsequent* thenKhaudRld2 = new FuzzyRuleConsequent();

thenKhaudRld2->addOutput(rld);

FuzzyRule* fuzzyRule30 = new FuzzyRule(30, IfEdRndAndDedDnd,

thenKhaudRld2);
fuzzy->addFuzzyRule(fuzzyRule30);

// Building FuzzyRule 31 "nd and an_d = rld3"

FuzzyRuleAntecedent* IfEdNdAndDedAn_d = new
FuzzyRuleAntecedent();
IfEdNdAndDedAn_d->joinWithAND(np, an_d);

FuzzyRuleConsequent* thenKhaudrld3 = new FuzzyRuleConsequent();

thenKhaudrld3->addOutput(nhp);

FuzzyRule* fuzzyRule31 = new FuzzyRule(31, IfEdNdAndDedAn_d,

thenKhaudrld3);
fuzzy->addFuzzyRule(fuzzyRule31);
 // Building FuzzyRule 32 "nd and avd = tbd1"
FuzzyRuleAntecedent* IfEdNdAndDedAvd = new
FuzzyRuleAntecedent();
IfEdNdAndDedAvd->joinWithAND(nd, avd);

FuzzyRuleConsequent* thenKhaudTbd1 = new FuzzyRuleConsequent();

thenKhaudTbd1->addOutput(tbp);

FuzzyRule* fuzzyRule32 = new FuzzyRule(32, IfEdNdAndDedAvd,

thenKhaudTbd1);
fuzzy->addFuzzyRule(fuzzyRule32);

// Building FuzzyRule 33 "nd and zrd = khd2"

FuzzyRuleAntecedent* IfEdNdAndDedZrd = new
FuzzyRuleAntecedent();
IfEdNdAndDedZrd->joinWithAND(nd, zrd);

FuzzyRuleConsequent* thenKhaudKhd2 = new

FuzzyRuleConsequent();
thenKhaudKhd2->addOutput(khd);

FuzzyRule* fuzzyRule33 = new FuzzyRule(33, IfEdNdAndDedZrd,

thenKhaudKhd2);
fuzzy->addFuzzyRule(fuzzyRule33);

// Building FuzzyRule 34 "nd and dvd = tbd2"

FuzzyRuleAntecedent* IfEdNdAndDedDvd = new
FuzzyRuleAntecedent();
IfEdNdAndDedDvd->joinWithAND(np, dvd);
 FuzzyRuleConsequent* thenKhaudTbd2 = new FuzzyRuleConsequent();
thenKhaudTbd2->addOutput(tbd);

FuzzyRule* fuzzyRule34 = new FuzzyRule(34, IfEdNdAndDedDvd,

thenKhaudTbd2);
fuzzy->addFuzzyRule(fuzzyRule34);

// Building FuzzyRule 35 "nd and dnd = rld4"

FuzzyRuleAntecedent* IfEdNdAndDedDnd = new
FuzzyRuleAntecedent();
IfEdNdAndDedDnd->joinWithAND(nd, dnd);

FuzzyRuleConsequent* thenKhaudRld4 = new FuzzyRuleConsequent();

thenKhaudRld4->addOutput(rld);

FuzzyRule* fuzzyRule35 = new FuzzyRule(35, IfEpNpAndDepDnp,

thenKhaudRld4);
fuzzy->addFuzzyRule(fuzzyRule35);

// Building FuzzyRule 36 "vd and an_d = rld5"

FuzzyRuleAntecedent* IfEdVdAndDedAn_d = new
FuzzyRuleAntecedent();
IfEdVdAndDedAn_d->joinWithAND(vd, an_d);

FuzzyRuleConsequent* thenKhaudRld5 = new FuzzyRuleConsequent();

thenKhaudRld5->addOutput(rld);

FuzzyRule* fuzzyRule36 = new FuzzyRule(36, IfEdVdAndDedAn_d,

thenKhaudRld5);
fuzzy->addFuzzyRule(fuzzyRule36);
 // Building FuzzyRule 37 "vd and avd = tbd3"
FuzzyRuleAntecedent* IfEdVdAndDedAvd = new
FuzzyRuleAntecedent();
IfEdVdAndDedAvd->joinWithAND(vd, avd);

FuzzyRuleConsequent* thenKhaudTbd3 = new FuzzyRuleConsequent();

thenKhaudTbd3->addOutput(lp);

FuzzyRule* fuzzyRule37 = new FuzzyRule(37, IfEdVdAndDedAvd,

thenKhaudTbd3);
fuzzy->addFuzzyRule(fuzzyRule37);

// Building FuzzyRule 38 "vd and zrd = khd3"

FuzzyRuleAntecedent* IfEdVdAndDedZrd = new
FuzzyRuleAntecedent();
IfEdVdAndDedZrd->joinWithAND(vd, zrd);

FuzzyRuleConsequent* thenKhaudKhd3 = new

FuzzyRuleConsequent();
thenKhaudKhd3->addOutput(khd);

FuzzyRule* fuzzyRule38 = new FuzzyRule(38, IfEdVdAndDedZrd,

thenKhaudKhd3);
fuzzy->addFuzzyRule(fuzzyRule38);

// Building FuzzyRule 39 "vd and dvd = tbd4"

FuzzyRuleAntecedent* IfEdVdAndDedDvd = new
FuzzyRuleAntecedent();
 IfEdVdAndDedDvd->joinWithAND(vd, dvd);

FuzzyRuleConsequent* thenKhaudTbd4 = new FuzzyRuleConsequent();

thenKhaudTbd4->addOutput(tbd);

FuzzyRule* fuzzyRule39 = new FuzzyRule(39, IfEdVdAndDedDvd,

thenKhaudTbd4);
fuzzy->addFuzzyRule(fuzzyRule39);

// Building FuzzyRule 40 "vd and dnd = rld6"

FuzzyRuleAntecedent* IfEdVdAndDedDnd = new
FuzzyRuleAntecedent();
IfEdVdAndDedDnd->joinWithAND(vd, dnd);

FuzzyRuleConsequent* thenKhaudRld6 = new FuzzyRuleConsequent();

thenKhaudRld6->addOutput(rld);

FuzzyRule* fuzzyRule40 = new FuzzyRule(40, IfEdVdAndDedDnd,

thenKhaudRld6);
fuzzy->addFuzzyRule(fuzzyRule40);

// Building FuzzyRule 41 "xd and an_d = rld7"

FuzzyRuleAntecedent* IfEdXdAndDedAn_d = new
FuzzyRuleAntecedent();
IfEdXdAndDedAn_d->joinWithAND(xd, an_d);

FuzzyRuleConsequent* thenKhaudRld7 = new FuzzyRuleConsequent();

thenKhaudRld7->addOutput(rld);
 FuzzyRule* fuzzyRule41 = new FuzzyRule(41, IfEdXdAndDedAn_d,
thenKhaudRld7);
fuzzy->addFuzzyRule(fuzzyRule41);

// Building FuzzyRule 42 "xd and avd = tbd5"

FuzzyRuleAntecedent* IfEdXdAndDedAvd = new
FuzzyRuleAntecedent();
IfEdXdAndDedAvd->joinWithAND(xd, avd);

FuzzyRuleConsequent* thenKhaudTbd5 = new FuzzyRuleConsequent();

thenKhaudTbd5->addOutput(tbd);

FuzzyRule* fuzzyRule42 = new FuzzyRule(42, IfEdXdAndDedAvd,

thenKhaudTbd5);
fuzzy->addFuzzyRule(fuzzyRule42);

// Building FuzzyRule 43 "xpd and zrd = khd4"

FuzzyRuleAntecedent* IfEdXdAndDedZrd = new
FuzzyRuleAntecedent();
IfEdXdAndDedZrd->joinWithAND(xd, zrd);

FuzzyRuleConsequent* thenKhaudKhd4 = new

FuzzyRuleConsequent();
thenKhaudKhd4->addOutput(khd);

FuzzyRule* fuzzyRule43 = new FuzzyRule(43, IfEdXdAndDedZrd,

thenKhaudKhd4);
fuzzy->addFuzzyRule(fuzzyRule43);
 // Building FuzzyRule 44 "xd and dvd = tbd6"
FuzzyRuleAntecedent* IfEdXdAndDedDvd = new
FuzzyRuleAntecedent();
IfEdXdAndDedDvd->joinWithAND(xd, dvd);

FuzzyRuleConsequent* thenKhaudTbd6 = new FuzzyRuleConsequent();

thenKhaudTbd6->addOutput(tbd);

FuzzyRule* fuzzyRule44 = new FuzzyRule(44, IfEdXdAndDedDvd,

thenKhaudTbd6);
fuzzy->addFuzzyRule(fuzzyRule44);

// Building FuzzyRule 45 "xd and dnd = rld8"***************

FuzzyRuleAntecedent* IfEdXdAndDedDnd = new
FuzzyRuleAntecedent();
IfEdXdAndDedDnd->joinWithAND(xd, dnd);

FuzzyRuleConsequent* thenKhaudRld8 = new FuzzyRuleConsequent();

thenKhaudRld8->addOutput(rld);

FuzzyRule* fuzzyRule45 = new FuzzyRule(45, IfEdXdAndDedDnd,

thenKhaudRld8);
fuzzy->addFuzzyRule(fuzzyRule45);

// Building FuzzyRule 46 "rxd and an_d = rld9"

FuzzyRuleAntecedent* IfEdRxdAndDedAn_d = new
FuzzyRuleAntecedent();
IfEdRxdAndDedAn_d->joinWithAND(rxd, an_d);
 FuzzyRuleConsequent* thenKhaudRld9 = new FuzzyRuleConsequent();
thenKhaupRlp6->addOutput(rld);

FuzzyRule* fuzzyRule46 = new FuzzyRule(46, IfEdRxdAndDedAn_d,

thenKhaupRlp6);
fuzzy->addFuzzyRule(fuzzyRule46);

// Building FuzzyRule 47 "rxd and avd = tbd7"

FuzzyRuleAntecedent* IfEdRxdAndDedAvd = new
FuzzyRuleAntecedent();
IfEdRxdAndDedAvd->joinWithAND(rxd, avd);

FuzzyRuleConsequent* thenKhaudTbd7 = new FuzzyRuleConsequent();

thenKhaudTbd7->addOutput(tbd);

FuzzyRule* fuzzyRule47 = new FuzzyRule(47, IfEdRxdAndDedAvd,

thenKhaudTbd7);
fuzzy->addFuzzyRule(fuzzyRule47);

// Building FuzzyRule 48 "rxd and zrd = khd5"

FuzzyRuleAntecedent* IfEdRxdAndDedZrd = new
FuzzyRuleAntecedent();
IfEdRxdAndDedZrd->joinWithAND(rxd, zrd);

FuzzyRuleConsequent* thenKhaudKhd5 = new

FuzzyRuleConsequent();
thenKhaudKhd5->addOutput(khd);

FuzzyRule* fuzzyRule48 = new FuzzyRule(48, IfEdRxdAndDedZrd,

thenKhaudKhd5);
 fuzzy->addFuzzyRule(fuzzyRule48);

// Building FuzzyRule 49"rxd and dvd = tbd8"

FuzzyRuleAntecedent* IfEdRxdAndDedDvd = new
FuzzyRuleAntecedent();
IfEdRxdAndDedDvd->joinWithAND(rxd, dvd);

FuzzyRuleConsequent* thenKhaudTbd8 = new FuzzyRuleConsequent();

thenKhaudTbd8->addOutput(tbd);

FuzzyRule* fuzzyRule49 = new FuzzyRule(49, IfEdRxdAndDedDvd,

thenKhaudTbd8);
fuzzy->addFuzzyRule(fuzzyRule49);

// Building FuzzyRule 50 "rxd and dnd = rld10"

FuzzyRuleAntecedent* IfEdRxdAndDedDnd = new
FuzzyRuleAntecedent();
IfEdRxdAndDedDnd->joinWithAND(rxd, dnd);

FuzzyRuleConsequent* thenKhaudRld10 = new

FuzzyRuleConsequent();
thenKhaudRld10->addOutput(rlp);

FuzzyRule* fuzzyRule50 = new FuzzyRule(50, IfEdRxdAndDedDnd,

thenKhaudRld10);
fuzzy->addFuzzyRule(fuzzyRule50);

// ***************** khau I *****************************

//*********************************************************
 // Building FuzzyRule 51 "rni and ani = Nhi1"

FuzzyRuleAntecedent* IfEiRniAndDeiAni = new

FuzzyRuleAntecedent();
IfEiRniAndDeiAni->joinWithAND(rni, ani);

FuzzyRuleConsequent* thenKhauiNhi1 = new FuzzyRuleConsequent();

thenKhauiNhi1->addOutput(nhi);

FuzzyRule* fuzzyRule51 = new FuzzyRule(51, IfEiRniAndDeiAni,

thenKhauiNhi1);
fuzzy->addFuzzyRule(fuzzyRule51);

// Building FuzzyRule 52 "rni and avi = li1"

FuzzyRuleAntecedent* IfEiRniAndDeiAvi = new
FuzzyRuleAntecedent();
IfEiRniAndDeiAvi->joinWithAND(rni, avi);

FuzzyRuleConsequent* thenKhauiLi1 = new FuzzyRuleConsequent();

thenKhauiLi1->addOutput(li);

FuzzyRule* fuzzyRule52 = new FuzzyRule(52, IfEiRniAndDeiAvi,

thenKhauiLi1);
fuzzy->addFuzzyRule(fuzzyRule52);

// Building FuzzyRule 53 "rni and zri = rli1"

FuzzyRuleAntecedent* IfEiRniAndDeiZri = new
FuzzyRuleAntecedent();
IfEiRniAndDeiZri->joinWithAND(rni, zri);
 FuzzyRuleConsequent* thenKhauiRli1 = new FuzzyRuleConsequent();
thenKhauiRli1->addOutput(rli);

FuzzyRule* fuzzyRule53 = new FuzzyRule(53, IfEiRniAndDeiZri,

thenKhauiRli1);
fuzzy->addFuzzyRule(fuzzyRule53);

// Building FuzzyRule 54 "rni and dvi = li2"

FuzzyRuleAntecedent* IfEiRniAndDeiDvi = new
FuzzyRuleAntecedent();
IfEiRniAndDeiDvi->joinWithAND(rni, dvi);

FuzzyRuleConsequent* thenKhauiLi2 = new FuzzyRuleConsequent();

thenKhauiLi2->addOutput(li);

FuzzyRule* fuzzyRule54 = new FuzzyRule(54, IfEiRniAndDeiDvi,

thenKhauiLi2);
fuzzy->addFuzzyRule(fuzzyRule54);

// Building FuzzyRule 55 "rni and dni =nhi2"

FuzzyRuleAntecedent* IfEiRniAndDeiDni = new
FuzzyRuleAntecedent();
IfEiRniAndDeiDni->joinWithAND(rni, dni);

FuzzyRuleConsequent* thenKhauiNhi2 = new FuzzyRuleConsequent();

thenKhauiNhi2->addOutput(nhi);

FuzzyRule* fuzzyRule55 = new FuzzyRule(55, IfEiRniAndDeiDni,

thenKhauiNhi2);
fuzzy->addFuzzyRule(fuzzyRule55);
 // Building FuzzyRule 56 "ni and ani =nhi3"
FuzzyRuleAntecedent* IfEiNiAndDeiAni = new
FuzzyRuleAntecedent();
IfEiNiAndDeiAni->joinWithAND(ni, ani);

FuzzyRuleConsequent* thenKhauiNhi3 = new FuzzyRuleConsequent();

thenKhauiNhi3->addOutput(nhi);

FuzzyRule* fuzzyRule56 = new FuzzyRule(56, IfEiNiAndDeiAni,

thenKhauiNhi3);
fuzzy->addFuzzyRule(fuzzyRule56);

// Building FuzzyRule 57 "ni and avi = tbi1"

FuzzyRuleAntecedent* IfEiNiAndDeiAvi = new
FuzzyRuleAntecedent();
IfEiNiAndDeiAvi->joinWithAND(ni, avi);

FuzzyRuleConsequent* thenKhauiTbi1 = new FuzzyRuleConsequent();

thenKhauiTbi1->addOutput(tbi);

FuzzyRule* fuzzyRule57 = new FuzzyRule(57, IfEiNiAndDeiAvi,

thenKhauiTbi1);
fuzzy->addFuzzyRule(fuzzyRule57);

// Building FuzzyRule 58 "ni and zri = rli2"

FuzzyRuleAntecedent* IfEiNiAndDeiZri = new
FuzzyRuleAntecedent();
IfEiNiAndDeiZri->joinWithAND(ni, zri);
 FuzzyRuleConsequent* thenKhauiRli2 = new FuzzyRuleConsequent();
thenKhauiRli2->addOutput(rli);

FuzzyRule* fuzzyRule58 = new FuzzyRule(58, IfEiNiAndDeiZri,

thenKhauiRli2);
fuzzy->addFuzzyRule(fuzzyRule58);

// Building FuzzyRule 59 "ni and dvi = tbi2"

FuzzyRuleAntecedent* IfEiNiAndDeiDvi = new
FuzzyRuleAntecedent();
IfEiNiAndDeiDvi->joinWithAND(ni, dvi);

FuzzyRuleConsequent* thenKhauiTbi2 = new FuzzyRuleConsequent();

thenKhauiTbi2->addOutput(tbi);

FuzzyRule* fuzzyRule59 = new FuzzyRule(59, IfEiNiAndDeiDvi,

thenKhauiTbi2);
fuzzy->addFuzzyRule(fuzzyRule59);

// Building FuzzyRule 60"ni and dni = nhi4"

FuzzyRuleAntecedent* IfEiNiAndDeiDni = new
FuzzyRuleAntecedent();
IfEiNiAndDeiDni->joinWithAND(ni, dni);

FuzzyRuleConsequent* thenKhauiNhi4 = new FuzzyRuleConsequent();

thenKhauiNhi4->addOutput(nhi);

FuzzyRule* fuzzyRule60 = new FuzzyRule(60, IfEiNiAndDeiDni,

thenKhauiNhi4);
fuzzy->addFuzzyRule(fuzzyRule60);
 // Building FuzzyRule 61 "vi and ani = nhi5"
FuzzyRuleAntecedent* IfEiViAndDeiAni = new
FuzzyRuleAntecedent();
IfEiViAndDeiAni->joinWithAND(vi, ani);

FuzzyRuleConsequent* thenKhauiNhi5 = new FuzzyRuleConsequent();

thenKhauiNhi5->addOutput(nhi);

FuzzyRule* fuzzyRule61 = new FuzzyRule(61, IfEiNiAndDeiDni,

thenKhauiNhi5);
fuzzy->addFuzzyRule(fuzzyRule61);

// Building FuzzyRule 12 "vi and avi =nhi6"

FuzzyRuleAntecedent* IfEiViAndDeiAvi = new
FuzzyRuleAntecedent();
IfEiViAndDeiAvi->joinWithAND(vi, avi);

FuzzyRuleConsequent* thenKhauiNhi6 = new FuzzyRuleConsequent();

thenKhauiNhi6->addOutput(nhi);

FuzzyRule* fuzzyRule62 = new FuzzyRule(62, IfEiRniAndDeiAvi,

thenKhauiNhi6);
fuzzy->addFuzzyRule(fuzzyRule62);

// Building FuzzyRule 63 "vi and zri = nhi7"

FuzzyRuleAntecedent* IfEiViAndDeiZri = new
FuzzyRuleAntecedent();
IfEiViAndDeiZri->joinWithAND(vi, zri);
 FuzzyRuleConsequent* thenKhauiNhi7 = new FuzzyRuleConsequent();
thenKhauiNhi7->addOutput(nhi);

FuzzyRule* fuzzyRule63 = new FuzzyRule(63, IfEiViAndDeiZri,

thenKhauiNhi7);
fuzzy->addFuzzyRule(fuzzyRule63);

// Building FuzzyRule 64 "vi and dvi = nhi8"

FuzzyRuleAntecedent* IfEiViAndDeiDvi = new
FuzzyRuleAntecedent();
IfEiViAndDeiDvi->joinWithAND(vi, dvi);

FuzzyRuleConsequent* thenKhauiNhi8 = new FuzzyRuleConsequent();

thenKhauiNhi8->addOutput(nhi);

FuzzyRule* fuzzyRule64 = new FuzzyRule(64, IfEiViAndDeiDvi,

thenKhauiNhi8);
fuzzy->addFuzzyRule(fuzzyRule64);

// Building FuzzyRule 65 "vi and dni = nhi9"

FuzzyRuleAntecedent* IfEiViAndDeiDni = new
FuzzyRuleAntecedent();
IfEiViAndDeiDni->joinWithAND(vi, dni);

FuzzyRuleConsequent* thenKhauiNhi9 = new FuzzyRuleConsequent();

thenKhauiNhi9->addOutput(nhi);
 FuzzyRule* fuzzyRule65 = new FuzzyRule(65, IfEiViAndDeiDni,
thenKhauiNhi9);
fuzzy->addFuzzyRule(fuzzyRule65);

// Building FuzzyRule 16 "xi and ani = nhi10"

FuzzyRuleAntecedent* IfEiXiAndDeiAni = new
FuzzyRuleAntecedent();
IfEiXiAndDeiAni->joinWithAND(xi, ani);

FuzzyRuleConsequent* thenKhauiNhi10 = new

FuzzyRuleConsequent();
thenKhauiNhi10->addOutput(nhi);

FuzzyRule* fuzzyRule66 = new FuzzyRule(66, IfEiXiAndDeiAni,

thenKhauiNhi10);
fuzzy->addFuzzyRule(fuzzyRule66);

// Building FuzzyRule 67 "xi and avi = nhi11"

FuzzyRuleAntecedent* IfEiXiAndDeiAvi = new
FuzzyRuleAntecedent();
IfEiXiAndDeiAvi->joinWithAND(xi, avi);

FuzzyRuleConsequent* thenKhauiNhi11 = new

FuzzyRuleConsequent();
thenKhauiNhi11->addOutput(nhi);

FuzzyRule* fuzzyRule67 = new FuzzyRule(67, IfEiXiAndDeiAvi,

thenKhauiNhi11);
fuzzy->addFuzzyRule(fuzzyRule67);
 // Building FuzzyRule 68 "xi and zri = nhi12"
FuzzyRuleAntecedent* IfEiXiAndDeiZri = new
FuzzyRuleAntecedent();
IfEiXiAndDeiZri->joinWithAND(xi, zri);

FuzzyRuleConsequent* thenKhauiNhi12 = new

FuzzyRuleConsequent();
thenKhauiNhi12->addOutput(nhi);

FuzzyRule* fuzzyRule68 = new FuzzyRule(68, IfEiXiAndDeiZri,

thenKhauiNhi12);
fuzzy->addFuzzyRule(fuzzyRule68);

// Building FuzzyRule 69 "xi and dvi = nhi13"

FuzzyRuleAntecedent* IfEiXiAndDeiDvi = new
FuzzyRuleAntecedent();
IfEiXiAndDeiDvi->joinWithAND(xi, dvi);

FuzzyRuleConsequent* thenKhauiNhi13 = new

FuzzyRuleConsequent();
thenKhauiNhi13->addOutput(nhi);

FuzzyRule* fuzzyRule69 = new FuzzyRule(69, IfEiXiAndDeiDvi,

thenKhauiNhi13);
fuzzy->addFuzzyRule(fuzzyRule69);

// Building FuzzyRule 70 "xi and dni = nhi14 "***************

FuzzyRuleAntecedent* IfEiXiAndDeiDni = new
FuzzyRuleAntecedent();
IfEiXiAndDeiDni->joinWithAND(xi, dni);
 FuzzyRuleConsequent* thenKhauiNhi14 = new
FuzzyRuleConsequent();
thenKhauiNhi14->addOutput(nhi);

FuzzyRule* fuzzyRule70 = new FuzzyRule(70, IfEiXiAndDeiDni,

thenKhauiNhi14);
fuzzy->addFuzzyRule(fuzzyRule70);

// Building FuzzyRule 71 "rxi and ani = nhi15"

FuzzyRuleAntecedent* IfEiRxiAndDeiAni = new
FuzzyRuleAntecedent();
IfEiRxiAndDeiAni->joinWithAND(rxi, ani);

FuzzyRuleConsequent* thenKhauiNhi15 = new

FuzzyRuleConsequent();
thenKhauiNhi15->addOutput(nhi);

FuzzyRule* fuzzyRule71 = new FuzzyRule(71, IfEiRxiAndDeiAni,

thenKhauiNhi15);
fuzzy->addFuzzyRule(fuzzyRule71);

// Building FuzzyRule 72 "rxi and avi = nhi16"

FuzzyRuleAntecedent* IfEiRxiAndDeiAvi = new
FuzzyRuleAntecedent();
IfEiRxiAndDeiAvi->joinWithAND(rxi, avi);

FuzzyRuleConsequent* thenKhauiNhi16 = new

FuzzyRuleConsequent();
thenKhauiNhi16->addOutput(nhi);
 FuzzyRule* fuzzyRule72 = new FuzzyRule(72, IfEiRxiAndDeiAvi,
thenKhauiNhi16);
fuzzy->addFuzzyRule(fuzzyRule72);

// Building FuzzyRule 73"rxi and zri = nhi17"

FuzzyRuleAntecedent* IfEiRxiAndDeiZri = new
FuzzyRuleAntecedent();
IfEiRxiAndDeiZri->joinWithAND(rxi, zri);

FuzzyRuleConsequent* thenKhauiNhi17 = new

FuzzyRuleConsequent();
thenKhauiNhi17->addOutput(nhi);

FuzzyRule* fuzzyRule73 = new FuzzyRule(73, IfEiRxiAndDeiZri,

thenKhauiNhi17);
fuzzy->addFuzzyRule(fuzzyRule73);

// Building FuzzyRule 74 " rxi and dvi = nhi18"

FuzzyRuleAntecedent* IfEiRxiAndDeiDvi = new
FuzzyRuleAntecedent();
IfEiRxiAndDeiDvi->joinWithAND(rxi, dvi);

FuzzyRuleConsequent* thenKhauiNhi18= new FuzzyRuleConsequent();

thenKhauiNhi18->addOutput(nhi);

FuzzyRule* fuzzyRule74 = new FuzzyRule(74, IfEiRxiAndDeiDvi,

thenKhauiNhi18);
fuzzy->addFuzzyRule(fuzzyRule74);

// Building FuzzyRule 75"rxi and dni = nhi19"

 FuzzyRuleAntecedent* IfEiRxiAndDeiDni = new
FuzzyRuleAntecedent();
IfEiRxiAndDeiDni->joinWithAND(rxi, dni);

FuzzyRuleConsequent* thenKhauiNhi19 = new

FuzzyRuleConsequent();
thenKhauiNhi19->addOutput(nhi);

FuzzyRule* fuzzyRule75 = new FuzzyRule(75, IfEiRxiAndDeiDni,

thenKhauiNhi19);
fuzzy->addFuzzyRule(fuzzyRule75);

void loop()
{
if (Serial.available()) // nhn d liu t my tnh
{
while (Serial.available () > 0)
{
bientam = Serial.read();

/*
/////////////////////////////////////////////////////////////////////////////////////////
gn bin tm
///////////////////////////////////////////////////////////////////////////////////////////
*/
if (bientam == 'l') // tien
{
val = 0;
}

if (bientam == 'x') // lui

{
val = 1;
}

if (bientam == 'p') // phai

{
val = 2;
}

if (bientam == 't')// trai

{
val = 3;
}

if (bientam == 'd')// dung

{
val = 4;
}

if (bientam == 'q')// thuan

{
val = 5;
}
if (bientam == 'n') //nghch
{
val = 6;
}

if (bientam == '1')
{
x = 10, y = 10, a1 = 0, a2= 0, t=0;
val = 10;
}

if (bientam == '2')
{
x = 20, y = 20, a1 = 0, t=0;
val = 10;
}

if (bientam == '3')
{
x = 30, y = 30, a1 = 0, a2= 0, t=0;
val = 10;
}
if (bientam == '4')
{
x = 40, y = 40, a1 = 0, a2= 0, t=0;
val = 10;
}
if (bientam == '5')
{
x = 50, y = 50, a1 = 0, a2= 0, t=0;
val = 10;
}
if (bientam == '6')
{
x = 10, y = 50, a1 = 0, a2= 0, t=0;
val = 10;
}
if (bientam == '7')
{
x = 50, y = 20, a1 = 0, a2= 0, t=0;
val = 10;
}
if (bientam == '8')
{
x = 50, y = 10, a1 = 0, a2= 0, t=0;
val = 10;
}
if(bientam == 'o')
{
a1 = 1, a2= 0, x= 0, y=0;
val = 10;
}
if(bientam == 'b')
{
a1 = 0, a2= 1, x= 0, y=0;
val = 10;
}
lcd.clear();
/*
/////////////////////////////////////////////////////////////////////////////////////////
iu khin
///////////////////////////////////////////////////////////////////////////////////////////
*/

if (val == 0) // tien
{
Serial.println(" ,Tien ");
lcd.setCursor(0, 0);
lcd.print("Tien ");
digitalWrite(52,0);

digitalWrite(in1_1, 1); // dc1

digitalWrite(in2_1, 0);
analogWrite(pwm_1, 160 );

digitalWrite(in1_2, 1);
digitalWrite(in2_2, 0); // dc2
analogWrite(pwm_2, 80 );

digitalWrite(in1_3, 0);
digitalWrite(in2_3, 1); // dc3
analogWrite(pwm_3, 80 );
}
if (val == 1) // lui
{
 Serial.println(" ,Lui");
//dco quay thuan ;
lcd.setCursor(0, 0);
lcd.print("Lui ");
digitalWrite(52,0);

digitalWrite(in1_1, 0); // dc1

digitalWrite(in2_1, 1);
analogWrite(pwm_1, 160 );

digitalWrite(in1_2, 0);
digitalWrite(in2_2, 1); // dc2
analogWrite(pwm_2, 80 );

digitalWrite(in1_3, 1);
digitalWrite(in2_3, 0); // dc3
analogWrite(pwm_3, 80 );

if (val == 2) // sang phai

{
Serial.println(" ,Sang phai ");
//dco quay thuan ;
lcd.setCursor(0, 0);
lcd.print("Sang phai ");
digitalWrite(52,0);

analogWrite(pwm_1, 0 ); // dc1
 digitalWrite(in1_2, 1);
digitalWrite(in2_2, 0); // dc2
analogWrite(pwm_2, 255 );

digitalWrite(in1_3, 1);
digitalWrite(in2_3, 0); // dc3
analogWrite(pwm_3, 255 );
}

if (val == 3) // sang trai

{
Serial.println(" ,Sang trai");
//dco quay thuan ;
lcd.setCursor(0, 0);
lcd.print("Sang trai ");
digitalWrite(52,0);
analogWrite(pwm_1, 0 ); // dc1

digitalWrite(in1_2, 0);
digitalWrite(in2_2, 1); // dc2
analogWrite(pwm_2, 255 );

digitalWrite(in1_3, 0);
digitalWrite(in2_3, 1); // dc3
analogWrite(pwm_3, 255 );
}

if (val == 4) // dung
{
Serial.println(" ,Dung ");
lcd.setCursor(0, 0);
lcd.print("Dung ");
analogWrite(pwm_1, 0 );
analogWrite(pwm_2, 0 );
analogWrite(pwm_3, 0 );
t=0;

//dco quay thuan ;

}
if (val == 5) // thuan
{
Serial.println(" ,Quay Thuan ");
//dco quay thuan ;
lcd.setCursor(0, 0);
lcd.print("Quay Thuan ");
digitalWrite(52,0);

digitalWrite(in1_1, 1); // dc1

digitalWrite(in2_1, 0);
analogWrite(pwm_1, 100 );

digitalWrite(in1_2, 0);
digitalWrite(in2_2, 1); // dc2
analogWrite(pwm_2, 100 );

digitalWrite(in1_3, 1);
digitalWrite(in2_3, 0); // dc3
 analogWrite(pwm_3, 100 );
}

if (val == 6) // nghich
{
Serial.println(" ,Quay nghich ");
//dco quay nghich
lcd.setCursor(0, 0);
lcd.print("Quay Nghich ");
digitalWrite(52,0);

digitalWrite(in1_1, 0); // dc1

digitalWrite(in2_1, 1);
analogWrite(pwm_1, 100 );

digitalWrite(in1_2, 1);
digitalWrite(in2_2, 0); // dc2
analogWrite(pwm_2, 100 );

digitalWrite(in1_3, 0);
digitalWrite(in2_3, 1); // dc3
analogWrite(pwm_3, 100 );

}
}
}

// gan gia tri

if (val == 10)
{
digitalWrite(52,0);
//lcd.clear();

Serial.print(","); // x0 0
Serial.print(x_dat);
Serial.print(",");// 1
Serial.print(x_thuc);

Serial.print(","); // y0 2
Serial.print(y_dat);
Serial.print(",");// 3
Serial.print(y_thuc);

Serial.print(","); // goc 4
Serial.print(gochientai);

Serial.print(",");// kp 5
Serial.print(kp);

Serial.print(","); //ki 6
Serial.print(ki);

Serial.print(","); // kd 7
Serial.print(kd);
 Serial.print(","); // u1 8
Serial.print(u1);

Serial.print(","); // u2 9
Serial.print(u2);

Serial.print(","); // u3 10
Serial.println(u3);

lcd.setCursor(0, 0);
lcd.print("x=");
lcd.print(x_thuc);

lcd.setCursor(0, 1);
lcd.print("y=");
lcd.print(y_thuc);

x_dat = a1*(25*sin(t)+35)+ a2*(25*sin(t)+35)+x; // A1 =0, x =

y_dat = a1*(25*cos(t)+35)+ a2*(25*sin(0.5*t)+35)+y; // t= 0, x = -50
+...

fuzzy->setInput(1, quangduonght);
fuzzy->setInput(2, de);
fuzzy->setInput(3, quangduonght);
fuzzy->setInput(4, de);
fuzzy->setInput(5, quangduonght);
fuzzy->setInput(6, de);
fuzzy->fuzzify();

float outputP_tam = fuzzy->defuzzify(1);

float outputD_tam = fuzzy->defuzzify(2);
float outputI_tam = fuzzy->defuzzify(3);

kp = outputP_tam *10 ;
ki = outputI_tam *0.001;
kd = outputD_tam *100;

x_thuc = khoangcach_x0();
y_thuc = khoangcach_y0();

if(x_thuc >100) {
x_thuc = 100;
}
if(y_thuc >100) {
y_thuc = 100;
}
//sua tam
gochientai = goc();
//gochientai = 237;
error_x = x_thuc - x_dat;
error_y = y_thuc - y_dat;

quangduonght = sqrt(error_x*error_x + error_y*error_y);

quangduonght = quangduonght / 100;
if(quangduonght > 1){
 quangduonght = 1;
}
if(quangduonght < 0){
quangduonght = 0;
}
v1 =(kp*error_y)+ki*(error_y+preError_y)+kd*(error_y-preError_y);
v2 =-(sqrt(3)/2)*kp*error_x+ki*(error_x+preError_x)+kd*(error_x-
preError_x)+(-0.5)*(kp*error_y)+ki*(error_y+preError_y)+kd*(error_y-
preError_y);
v3 = (sqrt(3)/2)*kp*error_x+ki*(error_x+preError_x)+kd*(error_x-
preError_x)+(-0.5)*(kp*error_y)+ki*(error_y+preError_y)+kd*(error_y-
preError_y);

preError_y = error_y;
preError_x = error_x;

duty1 = abs(v1);
duty2 = abs(v2);
duty3 = abs(v3);
if(duty1 > 1500){
duty1 = 1500;
}
if(duty2 > 1500){
duty2 = 1500;
}
if(duty3 > 1500){
duty3 = 1500;
}
u1 = map(duty1, 0, 1500, 60 , 250);
u2 = map(duty2, 0, 1500, 60 , 250);
u3 = map(duty3, 0, 1500, 60 , 250);
// chuong trinh chinh

//11111111111111//////////////*****************///////////////
if (gochientai < (gocdat + 10))
{
//dco quay nghich
digitalWrite(in1_1, 0);
digitalWrite(in2_1, 1);
analogWrite(pwm_1, 100 ); // dc1

digitalWrite(in1_2, 1);
digitalWrite(in2_2, 0);
analogWrite(pwm_2,100 ); //dc2

digitalWrite(in1_3, 0);
digitalWrite(in2_3, 1);
analogWrite(pwm_3, 100); // dc3
}
//22222222222222222222222222222222222222222
if (gochientai > (gocdat - 10))
{
//dco quay thuan ;

digitalWrite(in1_1, 1);
digitalWrite(in2_1, 0);
analogWrite(pwm_1, 100); // dc1
 digitalWrite(in1_2, 0);
digitalWrite(in2_2, 1);
analogWrite(pwm_2, 100); //dc2

digitalWrite(in1_3, 1);
digitalWrite(in2_3, 0);
analogWrite(pwm_3, 100); // dc3
}

//3333333333333333333333333333
if (gochientai <= (gocdat + 10) && gochientai >= (gocdat - 10) )
{
// bam xung cho dong co 1
if (v1 > 0)
{
digitalWrite(in1_1, 1); // dc1
digitalWrite(in2_1, 0);
analogWrite(pwm_1, u1 +3);
}
if (v1 < 0)
{
digitalWrite(in1_1, 0); // dc1
digitalWrite(in2_1, 1);
analogWrite(pwm_1, u1+3);
}
if (v1 == 0)
{
analogWrite(pwm_1, 0 );
}
// dong co 2
if (v2 > 0)
{
digitalWrite(in1_2, 0);
digitalWrite(in2_2, 1); // dc2
analogWrite(pwm_2, u2 );
}
if (v2 < 0)
{
digitalWrite(in1_2, 1);
digitalWrite(in2_2, 0); // dc2
analogWrite(pwm_2, u2 );
}
if (v2 == 0)
{
analogWrite(pwm_2, 0 );
}
// dong co 3
if (v3 > 0)
{
digitalWrite(in1_3, 1);
digitalWrite(in2_3, 0); // dc3
analogWrite(pwm_3, u3 );
}
if (v3 < 0)
{
digitalWrite(in1_3, 0);
digitalWrite(in2_3, 1); // dc3
 analogWrite(pwm_3, u3);
}

if (v3 == 0)
{
analogWrite(pwm_3, 0 );
}
}
}
}
2) Code Pidc.m
V = kp* [ex;ey] + ki*[tpex;tpey] + kd * [dhex;dhey]
u1 = V'*[0;1] + pdthe;
u2 = V'*[-sqrt(3)/2;-0.5] + pdthe;
u3 = V'*[sqrt(3)/2;-0.5] + pdthe;
sys(1) = u1;
sys(2) = u2;
sys(3) = u3;
3) Code Quangduong.m
ex = u(1);
ey = u(2);
u = sqrt(ex*ex + ey*ey)
sys(1) = u;