Scribd
Upload
237 views17 pages

Programacion Fuzzy

This document contains code for a fuzzy logic controller that controls the speed of motors on a robot. The code defines variables and functions related to a 4 input, 2 output fuzzy inference system. It includes definitions for member functions, rule data, and a function to evaluate the fuzzy system and control the motor speeds based on sensor input values.

Uploaded by

Jorge Mora
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
237 views17 pages

Programacion Fuzzy

This document contains code for a fuzzy logic controller that controls the speed of motors on a robot. The code defines variables and functions related to a 4 input, 2 output fuzzy inference system. It includes definitions for member functions, rule data, and a function to evaluate the fuzzy system and control the motor speeds based on sensor input values.

Uploaded by

Jorge Mora
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd

PROGRAMACION FUZZY

//***********************************************************************

// Matlab .fis to arduino C converter v2.0.1.25122016

// - Karthik Nadig, USA

// Please report bugs to:


// [Link]

// If you don't have a GitHub account mail to karthiknadig@[Link]

//***********************************************************************

#include <AFMotor.h>

#include <QTRSensors.h>

#include "fis_header.h"

// Number of inputs to the fuzzy inference system

const int fis_gcI = 4;

// Number of outputs to the fuzzy inference system

const int fis_gcO = 2;

// Number of rules to the fuzzy inference system

const int fis_gcR = 5;

FIS_TYPE g_fisInput[fis_gcI];

FIS_TYPE g_fisOutput[fis_gcO];

#define NUM_SENSORS 6 // Numero de sensores que usa

#define NUM_SAMPLES_PER_SENSOR 5 // Muestras por sensor

#define EMITTER_PIN 2 // emitter is controlled by digital pin 2

// sensors 0 through 5 are connected to analog inputs 0 through 5, respectively

QTRSensorsAnalog qtra((unsigned char[]) {A8, A9, A10, A11, A12, A13},

NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

AF_DCMotor motor1(4);

AF_DCMotor motor2(2);

int velcalibrate = 20;

// Setup routine runs once when you press reset:

void setup()

[Link](9600);
[Link](RELEASE);

[Link](RELEASE);

//-------------Instrucciones para Empezar a hacer la Calibracion de Sensores--------------------------------------//

delay(1500);

[Link](velcalibrate);

[Link](velcalibrate);

for (int counter=0; counter<21; counter++)

if (counter < 6 || counter >= 15)

[Link](FORWARD);

[Link](BACKWARD);

else

[Link](FORWARD);

[Link](BACKWARD);

[Link]();

delay(20);

[Link](RELEASE);

[Link](RELEASE);

//---------------------------Fin Calibracion de Sensores----------------------------------------------------//

delay(1000); // Retardo X segundos antes de Empezar a andar

[Link](FORWARD);

[Link](FORWARD);

void loop()
{

unsigned int position = [Link](sensorValues);

// Read Input: sder

g_fisInput[0] = sensorValues[1];

// Read Input: smed

g_fisInput[1] = sensorValues[2];

// Read Input: smed2

g_fisInput[2] = sensorValues[3];

// Read Input: sizq

g_fisInput[3] = sensorValues[4];

g_fisOutput[0] = 0;

g_fisOutput[1] = 0;

fis_evaluate();

[Link](g_fisOutput[0]);

[Link](g_fisOutput[1]);

/*

[Link](g_fisInput[1]);

[Link]('\t');

[Link](g_fisInput[1]);

[Link]('\t');

[Link](g_fisInput[2]);

[Link]('\t');

[Link](g_fisInput[3]);

[Link](g_fisOutput[0]);

[Link]('\t');

[Link](g_fisOutput[1]);

delay(2000);

*/

}
//***********************************************************************

// Support functions for Fuzzy Inference System

//***********************************************************************

// Double Sigmoid Member Function

FIS_TYPE fis_dsigmf(FIS_TYPE x, FIS_TYPE* p)

FIS_TYPE t = (fis_sigmf(x, p) - fis_sigmf(x, p + 2));

return abs(t);

// Sigmoid Member Function

FIS_TYPE fis_sigmf(FIS_TYPE x, FIS_TYPE* p)

FIS_TYPE a = p[0], c = p[1];

return (FIS_TYPE) (1.0 / (1.0 + exp(-a *(x - c))));

// Gaussian Member Function

FIS_TYPE fis_gaussmf(FIS_TYPE x, FIS_TYPE* p)

FIS_TYPE s = p[0], c = p[1];

FIS_TYPE t = (x - c) / s;

return exp(-(t * t) / 2);

FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)

return min(a, b);

FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)

return max(a, b);

}
FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)

int i;

FIS_TYPE ret = 0;

if (size == 0) return ret;

if (size == 1) return array[0];

ret = array[0];

for (i = 1; i < size; i++)

ret = (*pfnOp)(ret, array[i]);

return ret;

//***********************************************************************

// Data for Fuzzy Inference System

//***********************************************************************

// Pointers to the implementations of member functions

_FIS_MF fis_gMF[] =

fis_dsigmf, fis_sigmf, fis_gaussmf

};

// Count of member function for each Input

int fis_gIMFCount[] = { 3, 3, 3, 3 };

// Count of member function for each Output

int fis_gOMFCount[] = { 3, 3 };
// Coefficients for the Input Member Functions

FIS_TYPE fis_gMFI0Coeff1[] = { 0.02146, -200, 0.02147, 227.9 };

FIS_TYPE fis_gMFI0Coeff2[] = { 169.9, 500 };

FIS_TYPE fis_gMFI0Coeff3[] = { 0.02747, 700, 0.01717, 1200 };

FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3 };

FIS_TYPE fis_gMFI1Coeff1[] = { 0.02146, -200, 0.02147, 227.9 };

FIS_TYPE fis_gMFI1Coeff2[] = { 169.9, 500 };

FIS_TYPE fis_gMFI1Coeff3[] = { 0.02747, 700, 0.01717, 1200 };

FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };

FIS_TYPE fis_gMFI2Coeff1[] = { 0.02146, -200, 0.02147, 227.9 };

FIS_TYPE fis_gMFI2Coeff2[] = { 169.9, 500 };

FIS_TYPE fis_gMFI2Coeff3[] = { 0.02747, 700, 0.01717, 1200 };

FIS_TYPE* fis_gMFI2Coeff[] = { fis_gMFI2Coeff1, fis_gMFI2Coeff2, fis_gMFI2Coeff3 };

FIS_TYPE fis_gMFI3Coeff1[] = { 0.02146, -200, 0.02147, 227.9 };

FIS_TYPE fis_gMFI3Coeff2[] = { 169.9, 500 };

FIS_TYPE fis_gMFI3Coeff3[] = { 0.02747, 700, 0.01717, 1200 };

FIS_TYPE* fis_gMFI3Coeff[] = { fis_gMFI3Coeff1, fis_gMFI3Coeff2, fis_gMFI3Coeff3 };

FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff, fis_gMFI2Coeff, fis_gMFI3Coeff };

// Coefficients for the Output Member Functions

FIS_TYPE fis_gMFO0Coeff1[] = { 0.3433, 20, 0.3433, 40 };

FIS_TYPE fis_gMFO0Coeff2[] = { 8.493, 55 };

FIS_TYPE fis_gMFO0Coeff3[] = { 0.4395, 61.25, 0.6591, 96.64 };

FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3 };

FIS_TYPE fis_gMFO1Coeff1[] = { 0.3433, 20, 0.3433, 40 };

FIS_TYPE fis_gMFO1Coeff2[] = { 8.493, 55 };

FIS_TYPE fis_gMFO1Coeff3[] = { 0.4395, 61.25, 0.6591, 96.64 };

FIS_TYPE* fis_gMFO1Coeff[] = { fis_gMFO1Coeff1, fis_gMFO1Coeff2, fis_gMFO1Coeff3 };

FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff, fis_gMFO1Coeff };

// Input membership function set

int fis_gMFI0[] = { 0, 2, 0 };

int fis_gMFI1[] = { 0, 2, 0 };

int fis_gMFI2[] = { 0, 2, 0 };
int fis_gMFI3[] = { 0, 2, 0 };

int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1, fis_gMFI2, fis_gMFI3};

// Output membership function set

int fis_gMFO0[] = { 0, 2, 0 };

int fis_gMFO1[] = { 0, 2, 0 };

int* fis_gMFO[] = { fis_gMFO0, fis_gMFO1};

// Rule Weights

FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1 };

// Rule Type

int fis_gRType[] = { 1, 1, 1, 1, 1 };

// Rule Inputs

int fis_gRI0[] = { 1, 3, 3, 1 };

int fis_gRI1[] = { 2, 3, 2, 1 };

int fis_gRI2[] = { 1, 2, 3, 2 };

int fis_gRI3[] = { 3, 3, 1, 1 };

int fis_gRI4[] = { 1, 1, 3, 3 };

int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4 };

// Rule Outputs

int fis_gRO0[] = { 2, 2 };

int fis_gRO1[] = { 1, 3 };

int fis_gRO2[] = { 3, 1 };

int fis_gRO3[] = { 1, 2 };

int fis_gRO4[] = { 3, 1 };

int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4 };

// Input range Min

FIS_TYPE fis_gIMin[] = { 0, 0, 0, 0 };

// Input range Max


FIS_TYPE fis_gIMax[] = { 1000, 1000, 1000, 1000 };

// Output range Min

FIS_TYPE fis_gOMin[] = { 30, 30 };

// Output range Max

FIS_TYPE fis_gOMax[] = { 80, 80 };

//***********************************************************************

// Data dependent support functions for Fuzzy Inference System

//***********************************************************************

FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)

FIS_TYPE mfOut;

int r;

for (r = 0; r < fis_gcR; ++r)

int index = fis_gRO[r][o];

if (index > 0)

index = index - 1;

mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);

else if (index < 0)

index = -index - 1;

mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);

else

mfOut = 0;

}
fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);

return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);

FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)

FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);

FIS_TYPE area = 0;

FIS_TYPE momentum = 0;

FIS_TYPE dist, slice;

int i;

// calculate the area under the curve formed by the MF outputs

for (i = 0; i < FIS_RESOLUSION; ++i){

dist = fis_gOMin[o] + (step * i);

slice = step * fis_MF_out(fuzzyRuleSet, dist, o);

area += slice;

momentum += slice*dist;

return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));

//***********************************************************************

// Fuzzy Inference System

//***********************************************************************

void fis_evaluate()

FIS_TYPE fuzzyInput0[] = { 0, 0, 0 };

FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };

FIS_TYPE fuzzyInput2[] = { 0, 0, 0 };

FIS_TYPE fuzzyInput3[] = { 0, 0, 0 };

FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, fuzzyInput2, fuzzyInput3, };


FIS_TYPE fuzzyOutput0[] = { 0, 0, 0 };

FIS_TYPE fuzzyOutput1[] = { 0, 0, 0 };

FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, fuzzyOutput1, };

FIS_TYPE fuzzyRules[fis_gcR] = { 0 };

FIS_TYPE fuzzyFires[fis_gcR] = { 0 };

FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };

FIS_TYPE sW = 0;

// Transforming input to fuzzy Input

int i, j, r, o;

for (i = 0; i < fis_gcI; ++i)

for (j = 0; j < fis_gIMFCount[i]; ++j)

fuzzyInput[i][j] =

(fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);

int index = 0;

for (r = 0; r < fis_gcR; ++r)

if (fis_gRType[r] == 1)

fuzzyFires[r] = FIS_MAX;

for (i = 0; i < fis_gcI; ++i)

index = fis_gRI[r][i];

if (index > 0)

fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);

else if (index < 0)

fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);

else

fuzzyFires[r] = fis_min(fuzzyFires[r], 1);


}

else

fuzzyFires[r] = FIS_MIN;

for (i = 0; i < fis_gcI; ++i)

index = fis_gRI[r][i];

if (index > 0)

fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);

else if (index < 0)

fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);

else

fuzzyFires[r] = fis_max(fuzzyFires[r], 0);

fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];

sW += fuzzyFires[r];

if (sW == 0)

for (o = 0; o < fis_gcO; ++o)

g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);

else

for (o = 0; o < fis_gcO; ++o)

g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);

}
}

PROGRAMACION PID
#include <AFMotor.h>

#include <QTRSensors.h>

//------------------------------------------------------------------------------------//

//Sensores de Linea PD

#define NUM_SENSORS 6 // Numero de sensores que usa

#define NUM_SAMPLES_PER_SENSOR 5 // Muestras por sensor

#define EMITTER_PIN 2 // emitter is controlled by digital pin 2

// sensors 0 through 5 are connected to analog inputs 0 through 5, respectively

QTRSensorsAnalog qtra((unsigned char[]) {A8, A9, A10, A11, A12, A13},

NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);

unsigned int sensorValues[NUM_SENSORS];

AF_DCMotor motor1(4);

AF_DCMotor motor2(2);

//--------------------------------------------------------------------------------------//

//Velocidad Maxima Robot//--------------------------//----------------------------------//

const int maximum = 84;

//--------------------------------------------------------------------------------------//

//VALORES PD//----------//VALORES PD//--------------------------------------------------//

int VProporcional = 6;

int VDerivativo = 2;

//--------------------------------------------------------------------------------------//
//Velocidad de Calibracion

int velcalibrate = 40;

//--------------------------------------------------------------------------------------//

void setup()

[Link](RELEASE);

[Link](RELEASE);

//-------------Instrucciones para Empezar a hacer la Calibracion de Sensores--------------------------------------//

delay(1500);

[Link](velcalibrate);

[Link](velcalibrate);

for (int counter=0; counter<21; counter++)

if (counter < 6 || counter >= 15)

[Link](FORWARD);

[Link](BACKWARD);

else

[Link](FORWARD);

[Link](BACKWARD);

[Link]();

delay(20);

[Link](RELEASE);

[Link](RELEASE);

//---------------------------Fin Calibracion de Sensores----------------------------------------------------//

delay(1000); // Retardo X segundos antes de Empezar a andar

[Link](FORWARD);

[Link](FORWARD);
}

unsigned int last_proportional = 0;

long integral = 0;

void loop()

unsigned int position = [Link](sensorValues); // leemos posicion de la linea en la variable position

// Referencia donde seguira la linea, mitad sensores.

int proportional = (int)position - 2500;

// Calculos PD

int derivative = proportional - last_proportional;

integral += proportional;

last_proportional = proportional;

int power_difference = proportional/VProporcional + integral*0 + derivative*VDerivativo;

if (power_difference > maximum)

power_difference = maximum;

if (power_difference < -maximum)

power_difference = -maximum;

if (power_difference < 0)

[Link](maximum);

[Link](maximum + power_difference);

else

[Link](maximum - power_difference);

[Link](maximum);

}
};

You might also like