PROGRAMACION FUZZY

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

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

// - Karthik Nadig, USA

// Please report bugs to:

// If you don't have a GitHub account mail to [email protected]

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

#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()

Serial.begin(9600);
motor1.run(RELEASE);

motor2.run(RELEASE);

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

delay(1500);

motor1.setSpeed(velcalibrate);

motor2.setSpeed(velcalibrate);

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

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

motor1.run(FORWARD);

motor2.run(BACKWARD);

else

motor2.run(FORWARD);

motor1.run(BACKWARD);

qtra.calibrate();

delay(20);

motor1.run(RELEASE);

motor2.run(RELEASE);

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

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

motor1.run(FORWARD);

motor2.run(FORWARD);

void loop()
{

unsigned int position = qtra.readLine(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();

motor2.setSpeed(g_fisOutput[0]);

motor1.setSpeed(g_fisOutput[1]);

/*

Serial.print(g_fisInput[1]);

Serial.print('\t');

Serial.print(g_fisInput[1]);

Serial.print('\t');

Serial.print(g_fisInput[2]);

Serial.print('\t');

Serial.println(g_fisInput[3]);

Serial.print(g_fisOutput[0]);

Serial.print('\t');

Serial.println(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

// 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 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()

motor1.run(RELEASE);

motor2.run(RELEASE);

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

delay(1500);

motor1.setSpeed(velcalibrate);

motor2.setSpeed(velcalibrate);

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

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

motor1.run(FORWARD);

motor2.run(BACKWARD);

else

motor2.run(FORWARD);

motor1.run(BACKWARD);

qtra.calibrate();

delay(20);

motor1.run(RELEASE);

motor2.run(RELEASE);

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

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

motor1.run(FORWARD);

motor2.run(FORWARD);
}

unsigned int last_proportional = 0;

long integral = 0;

void loop()

unsigned int position = qtra.readLine(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)

motor1.setSpeed(maximum);

motor2.setSpeed(maximum + power_difference);

else

motor1.setSpeed(maximum - power_difference);

motor2.setSpeed(maximum);

}
};