Fuzzy Quadcopter!

[ned]

Hi guys,
Currently i am doing this project on fuzzy logic.The aim is to replace the PID control for auto stabilization in multiwii code with fuzzy logic.I have managed to come out with the fuzzy codes.But i am facing problems with implementing it to the multiwii will source codes. I tried shift all the codes within fuzzy setup into main setup file.I do the same thing for fuzzy void loop into the main code. Below is what i have done.

1) i comment this line under yaw,pitch and roll PID
//axisPID[axis] = PTerm + ITerm - DTerm;
i upload this and try on the uav.Its not spinning at all.Hence, i think that the pid control has been disabled.

2)I shift the following fuzzy codes into main void setup and void loop repsectively.I insert the contents within fuzzy void loop under this command:

errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here

*UAV is not responding at all.Please help me with this.Thanks.If any of you have any better idea of replacing the pid with fuzzy,please feel free to feedback your valuable feedbacks,Thanks


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

// Step 1 - Instantiating an object library
Fuzzy* fuzzy = new Fuzzy();


void setup(){
Serial.begin(9600);

// Step 2 - Creating a FuzzyInput error
FuzzyInput* error = new FuzzyInput(1);// With its ID in param
// Creating the FuzzySet to compond FuzzyInput distance
FuzzySet* NL = new FuzzySet(-15,-15,-10,-5); // Negative Large error
error->addFuzzySet(NL); // Add FuzzySet NL to error
FuzzySet* NS = new FuzzySet(-10,-5,-5,0); // Negative Small error
error->addFuzzySet(NS); // Add FuzzySet NS to error
FuzzySet* Z = new FuzzySet(-5,0,0,5); // Zero error
error->addFuzzySet(Z); // Add FuzzySet Z to error
FuzzySet* PS = new FuzzySet(0,5,5,10); // Positive small error
error->addFuzzySet(PS); // Add FuzzySet positive small to error
FuzzySet* PL = new FuzzySet(10,15,20,20); // Positive large error
error->addFuzzySet(PL); // Add FuzzySet PL to error
fuzzy->addFuzzyInput(error); // Add FuzzyInput to Fuzzy object

// Passo 3 - Creating FuzzyOutput PWM
FuzzyOutput* pwm = new FuzzyOutput(1);// With its ID in param
// Creating FuzzySet to compond FuzzyOutput pwm
FuzzySet* VS= new FuzzySet(-1000,-1000,-750,-500); // Very Small pwm
pwm->addFuzzySet(VS); // Add FuzzySet VS to pwm
FuzzySet* MS = new FuzzySet(-1000,-500,-500,0); // Medium Small pwm
pwm->addFuzzySet(MS); // Add FuzzySet MS to pwm
FuzzySet* NIL = new FuzzySet(-500,0,0,500); // Zero pwm
pwm->addFuzzySet(NIL); // Add FuzzySet NIL to pwm
FuzzySet* MF= new FuzzySet(0,500,500,1000); // Postive small pwm
pwm->addFuzzySet(MF); // Add FuzzySet PS to pwm
FuzzySet* VF = new FuzzySet(500,750,1000,1000); // Postive large pwm
pwm->addFuzzySet(VF); // Add FuzzySet PL to pwm
fuzzy->addFuzzyOutput(pwm);// Add FuzzyOutput to Fuzzy object

FuzzyOutput* pwm2 = new FuzzyOutput(2); // Diferent index
// Creating FuzzySet to compond FuzzyOutput2 pwm
FuzzySet* VS2= new FuzzySet(-1000,-1000,-750,-500); // Very Small pwm
pwm2->addFuzzySet(VS2); // Add FuzzySet VS to pwm
FuzzySet* MS2 = new FuzzySet(-1000,-500,-500,0); // Medium Small pwm
pwm2->addFuzzySet(MS2); // Add FuzzySet MS to pwm
FuzzySet* NIL2 = new FuzzySet(-500,0,0,500); // Zero pwm
pwm2->addFuzzySet(NIL2); // Add FuzzySet NIL to pwm
FuzzySet* MF2= new FuzzySet(0,500,500,1000); // Postive small pwm
pwm2->addFuzzySet(MF2); // Add FuzzySet PS to pwm
FuzzySet* VF2 = new FuzzySet(500,750,1000,1000); // Postive large pwm
pwm2->addFuzzySet(VF2); // Add FuzzySet PL to pwm
fuzzy->addFuzzyOutput(pwm2);

//Passo 4 - Assembly the Fuzzy rules
// FuzzyRule "IF error = netavie large THEN output1 = postive large and output2=netavie large"
FuzzyRuleAntecedent* iferrorNegativeLarge = new FuzzyRuleAntecedent(); // Instantiating an Antecedent to expression
iferrorNegativeLarge->joinSingle(NL); // Adding corresponding FuzzySet to Antecedent object
FuzzyRuleConsequent* thenpwmVerySlowAndpwm2VeryFast = new FuzzyRuleConsequent(); // Instantiating a Consequent to expression
thenpwmVerySlowAndpwm2VeryFast->addOutput(VS);
thenpwmVerySlowAndpwm2VeryFast->addOutput(VF2);// Adding corresponding FuzzySet to Consequent object
// Instantiating a FuzzyRule object
FuzzyRule* fuzzyRule01 = new FuzzyRule(1, iferrorNegativeLarge, thenpwmVerySlowAndpwm2VeryFast); // Passing the Antecedent and the Consequent of expression
fuzzy->addFuzzyRule(fuzzyRule01); // Adding FuzzyRule to Fuzzy object

// FuzzyRule "IF error = netavie small THEN output1 = postive small and output2=postive large"
FuzzyRuleAntecedent* iferrorNegativeSmall = new FuzzyRuleAntecedent(); // Instantiating an Antecedent to expression
iferrorNegativeSmall->joinSingle(NS); // Adding corresponding FuzzySet to Antecedent object
FuzzyRuleConsequent* thenpwmMediumSlowAndpwm2MediumFast = new FuzzyRuleConsequent(); // Instantiating a Consequent to expression
thenpwmMediumSlowAndpwm2MediumFast->addOutput(MS);
thenpwmMediumSlowAndpwm2MediumFast->addOutput(MF2);
// Instantiating a FuzzyRule object
FuzzyRule* fuzzyRule02 = new FuzzyRule(2, iferrorNegativeSmall, thenpwmMediumSlowAndpwm2MediumFast); // Passing the Antecedent and the Consequent of expression
fuzzy->addFuzzyRule(fuzzyRule02); // Adding FuzzyRule to Fuzzy object

// FuzzyRule "IF error = zero THEN output1 = zero and output2=zero"
FuzzyRuleAntecedent* iferrorZero = new FuzzyRuleAntecedent(); // Instantiating an Antecedent to expression
iferrorZero->joinSingle(Z); // Adding corresponding FuzzySet to Antecedent object
FuzzyRuleConsequent* thenpwmNILAndpwm2NIL = new FuzzyRuleConsequent();
thenpwmNILAndpwm2NIL->addOutput(NIL);
thenpwmNILAndpwm2NIL->addOutput(NIL2);// Adding corresponding FuzzySet to Consequent object
// Instantiating a FuzzyRule object
FuzzyRule* fuzzyRule03 = new FuzzyRule(3, iferrorZero, thenpwmNILAndpwm2NIL); // Passing the Antecedent and the Consequent of expression
fuzzy->addFuzzyRule(fuzzyRule03); // Adding FuzzyRule to Fuzzy object

// FuzzyRule "IF error = postive small THEN output1 = negative small and output2=postive small"
FuzzyRuleAntecedent* iferrorPostiveSmall = new FuzzyRuleAntecedent(); // Instantiating an Antecedent to expression
iferrorPostiveSmall->joinSingle(PS); // Adding corresponding FuzzySet to Antecedent object
FuzzyRuleConsequent* thenpwmMediumFastAndpwm2MediumSlow = new FuzzyRuleConsequent(); // Instantiating a Consequent to expression
thenpwmMediumFastAndpwm2MediumSlow->addOutput(MF);
thenpwmMediumFastAndpwm2MediumSlow->addOutput(MS2);// Adding corresponding FuzzySet to Consequent object
// Instantiating a FuzzyRule object
FuzzyRule* fuzzyRule04 = new FuzzyRule(4, iferrorPostiveSmall, thenpwmMediumFastAndpwm2MediumSlow); // Passing the Antecedent and the Consequent of expression
fuzzy->addFuzzyRule(fuzzyRule04); // Adding FuzzyRule to Fuzzy object

// FuzzyRule "IF error = postive large THEN output1 = negative large and output2=postive large"
FuzzyRuleAntecedent* iferrorPostiveLarge = new FuzzyRuleAntecedent(); // Instantiating an Antecedent to expression
iferrorPostiveLarge->joinSingle(PL); // Adding corresponding FuzzySet to Antecedent object
FuzzyRuleConsequent* thenpwmVeryFastAndpwm2VerySlow = new FuzzyRuleConsequent();
thenpwmVeryFastAndpwm2VerySlow->addOutput(VF);
thenpwmVeryFastAndpwm2VerySlow->addOutput(VS2);
// Instantiating a FuzzyRule object
FuzzyRule* fuzzyRule05 = new FuzzyRule(5, iferrorPostiveLarge, thenpwmVeryFastAndpwm2VerySlow); // Passing the Antecedent and the Consequent of expression
fuzzy->addFuzzyRule(fuzzyRule05); // Adding FuzzyRule to Fuzzy object
}

void loop(){

// Step 5 - Report inputs value, passing its ID and value
fuzzy->setInput(1, errorAngle);
// Step 6 - Exe the fuzzification
fuzzy->fuzzify();
// Step 7 - Exe the desfuzzyficação for each output, passing its ID
float output1 = fuzzy->defuzzify(1);
float output2 = fuzzy->defuzzify(2);
float inputToMotor0 = map(output, -1000, 1000, 0, 255);
analogWrite(3, inputToMotor0);
float inputToMotor1 = map(output2, -1000, 1000, 0, 255);
analogWrite(9, inputToMotor1);


}

delay(1000000);
}

[scrat]

What is fuzzy?

[deejayspinz]

Google is your friend: http://en.wikipedia.org/wiki/Fuzzy_logic

[ned]

Anyone has any idea of how it can be done?

[timecop]

You can't use analogWrite with multiwii code.
You would need to dick around with output.cpp to see what is actually needed to set servos.

Alternatively, you can upgrade to baseflight, get a 72MHz 32bit ARM processor to do your fuzzylogic calculations, hardware PWM thats set by a single motor[x] = value, and a proper IDE for development and debugging.

[brm]

Anyone has any idea of how it can be done?

you can ignore all modes in the first step.

errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here

the calculated value "errorAngle" goes into you fuzzy calculation.
then you save them:
axisPID[axis] = fuzzyCorrection; // PTerm + ITerm - DTerm;

where the fuzzycorrection is per axis.
once you have the axispid you simply call the mixer.

interesting - be shure to have enough spare parts

in case you fail - publish the sources.
will be interesting to see how it flies or wobbles....

[copterrichie]

There are three maybe for threads on Fuzzy Logic, I think it would be helpful to consolidate them into one.

Here is some information on Fuzzy Logic used on arduino: http://zerokol.com/post/51e9324ee84c55a1f5000007/1/en

https://github.com/zerokol/eFLL

[brm]

There are three maybe for threads on Fuzzy Logic, I think it would be helpful to consolidate them into one.

Here is some information on Fuzzy Logic used on arduino: http://zerokol.com/post/51e9324ee84c55a1f5000007/1/en

https://github.com/zerokol/eFLL

thanks for the link.
will look at it - it's c++ - throwing the rest away to stick with c++

[ned]

There are three maybe for threads on Fuzzy Logic, I think it would be helpful to consolidate them into one.

Here is some information on Fuzzy Logic used on arduino: http://zerokol.com/post/51e9324ee84c55a1f5000007/1/en

https://github.com/zerokol/eFLL

Hi,i am editing from the example given for my fuzzy codes.

[ned]

Anyone has any idea of how it can be done?

you can ignore all modes in the first step.

errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here

the calculated value "errorAngle" goes into you fuzzy calculation.
then you save them:
axisPID[axis] = fuzzyCorrection; // PTerm + ITerm - DTerm;

where the fuzzycorrection is per axis.
once you have the axispid you simply call the mixer.

interesting - be shure to have enough spare parts

in case you fail - publish the sources.
will be interesting to see how it flies or wobbles....

Hi Friend,i dont understand this part "axisPID[axis] = fuzzyCorrection;" my fuzzy codes are used to bring the copter back to level by producing different outputs to rotor 1 and rotor 3 respectively. I have blend the fuzzy codes into void setup and void loop.
The codes are already posted on top.
How should i rewrite the axisPID[axis] part with the codes shown above?Thanks

[brm]

ned stands for North East Down.

so - what do you do with rol and pitch?!?
you copter would hover only with your code.
what about the radio: desired roll & desired Pitch.
where do you feed it?!?
and now D like down.
you say the uav does nothing .... ever looked how to apply throttle?

you need to think in axes.

but you may write an additional set of rules allowing roles and pitch.
to create the momentum for yaw you need rise and lower the rotation speed of the props in pairs.
you may create the rule for this as well.

but thinking in axes is easier - you break up the problem in pieces which you might be able to handle.
the helper is the sensor fusion.