#include"Cpu.h"
#include"Events.h"
#include"mqx_tasks.h"
#include"Functions.h"
#define DATARCV 1
#define GAIN 1000
#define NUMBEROFDATA 191
// _______ |
________
// \
/|\ /
//
\/ | \/
// /\ |
/\
// _______/__\|/__\________
// -PAR1 PAR1
//MULTIPLIED BY THE GAIN PARAMETERS FOR MEMBERSHIP FUNCTIONS
#define PAR2 200 //
Change Error Sets
/*-------------------------------*/
/*RULES MATRIX*/
#define NG -6000
#define NM -4000
#define NP -2000
#define ZE 0
#define PP 2000
#define PM 4000
#define PG 6000
int
RULES[4][4]={{NG,NM,NP,0},{NP,ZE,PP,0},{PP,PM,PG,0},{0,0,0,0}};
/*------------*/
#ifdef __cplusplus
extern"C" {
#endif
/*Variables*/
LDD_TDeviceData *PTR_ADC;
pointer PTR_Initialization,PTR_Main_Task;
short position;
int Error_Store[NUMBEROFDATA +
1],Change_Error_Store[NUMBEROFDATA + 1];
int SetPoint,Y,U =
0,Value,dU,MF1[3][2],MF2[3][2],Tnorm_min[9][2],Tnorm[4][2],Error,Last_Error,Change_Error
= 20;
unsignedchar
MatLab_Data[16],ConvertedValues[16],*ConvertedValuesptr,*AP_Data;
/* Use light weight Events */
LWEVENT_STRUCT lwevent;
/* Use light weight semaphores */
LWSEM_STRUCT semaforo;
/* User includes (#include below this line is not maintained by
Processor Expert) */
/*
** ===================================================================
** Event :
Initialization (module mqx_tasks)
**
** Component :
Task1 [MQXLite_task]
** Description :
** MQX task routine. The
routine is generated into mqx_tasks.c
** file.
** Parameters :
** NAME - DESCRIPTION
** task_init_data -
** Returns : Nothing
** ===================================================================
*/
voidInitialization(uint32_t task_init_data)
{
_task_id Initialization;
Initialization = _task_get_id();
PTR_Initialization = _task_get_td(Initialization);
PTR_ADC = AD1_Init(NULL); //INICIALIZA ADC
AD1_SelectSampleGroup(PTR_ADC,0U);
//Create event to Main_Task
_lwevent_create(&lwevent,LWEVENT_AUTO_CLEAR);
_lwsem_create(&semaforo,0);
//Create the fuzzification_task Task
_task_create_at(0, FUZZIFICATION_TASK, 0, fuzzification_task_stack,
FUZZIFICATION_TASK_STACK_SIZE);
//Create the norm_t_min_task Task
_task_create_at(0, NORM_T_MIN_TASK, 0, norm_t_min_task_stack,
NORM_T_MIN_TASK_STACK_SIZE);
//Create the maximum_task Task
_task_create_at(0, MAXIMUM_TASK, 0, maximum_task_stack,
MAXIMUM_TASK_STACK_SIZE);
//Create the defuzzification_task Task
_task_create_at(0, DEFUZZIFICATION_TASK, 0, defuzzification_task_stack,
DEFUZZIFICATION_TASK_STACK_SIZE);
MatLab_Data[15]='\n'; //Escribe '\n' enultimaposicionpara leer
primer dato
_task_abort(Initialization);
while(1) {
}
}
/*
** ===================================================================
** Event :
Main_Task (module mqx_tasks)
**
** Component :
Task2 [MQXLite_task]
** Description :
** MQX task routine. The
routine is generated into mqx_tasks.c
** file.
** Parameters :
** NAME - DESCRIPTION
** task_init_data -
** Returns : Nothing
** ===================================================================
*/
voidMain_Task(uint32_t task_init_data)
{
_task_id Main_Task;
Main_Task = _task_get_id();
PTR_Main_Task =
_task_get_td(Main_Task);
int counter = 0;
//AD1_StartLoopMeasurement(PTR_ADC);
while(1) {
_lwevent_wait_ticks(&lwevent,DATARCV,FALSE,0);
/*ConvierteDatosde MatLab a Entero*/
Y =
str2int(position,MatLab_Data);
/*Lee SetPoint*/
AD1_StartSingleMeasurement(PTR_ADC);
/*Varibale Error and Change_Error*/
Error =
(SetPoint*GAIN) - Y;
Change_Error = Error - Last_Error;
Last_Error = Error;
/*Save*/
if (counter <= NUMBEROFDATA){
Error_Store[counter] = Error;
Change_Error_Store[counter] = Change_Error;
counter++;}
_lwsem_post(&semaforo);
_task_block();
/*PI incremental output*/
U += dU;
int2str(U,8);
ConvertedValuesptr=ConvertedValues;
AS1_SendChar(*ConvertedValuesptr);
}
}
/*
** ===================================================================
** Event :
fuzzification_task (module mqx_tasks)
**
** Component :
Task3 [MQXLite_task]
** Description :
** MQX task routine. The
routine is generated into mqx_tasks.c
** file.
** Parameters :
** NAME - DESCRIPTION
** task_init_data -
** Returns : Nothing
** ===================================================================
*/
voidfuzzification_task(uint32_t task_init_data)
{
int counter = 0;
while(1) {
_lwsem_wait(&semaforo);
GREEN_LED_NegVal(GREEN_LED_DeviceData);
/*Membership Grade*/
membership_grade(Error,Change_Error,PAR1,PAR2);
_lwsem_post(&semaforo);
}
}
/*
** ===================================================================
** Event :
norm_t_min_task (module mqx_tasks)
**
** Component :
Task4 [MQXLite_task]
** Description :
** MQX task routine. The
routine is generated into mqx_tasks.c
** file.
** Parameters :
** NAME - DESCRIPTION
** task_init_data -
** Returns : Nothing
** ===================================================================
*/
voidnorm_t_min_task(uint32_t task_init_data)
{
int counter = 0;
while(1) {
_lwsem_wait(&semaforo);
GREEN_LED_NegVal(GREEN_LED_DeviceData);
minimo(MF1,MF2,RULES);
_lwsem_post(&semaforo);
}
}
/*
** ===================================================================
** Event :
maximum_task (module mqx_tasks)
**
** Component :
Task5 [MQXLite_task]
** Description :
** MQX task routine. The
routine is generated into mqx_tasks.c
** file.
** Parameters :
** NAME - DESCRIPTION
** task_init_data -
** Returns : Nothing
** ===================================================================
*/
voidmaximum_task(uint32_t task_init_data)
{
int counter = 0;
while(1) {
_lwsem_wait(&semaforo);
GREEN_LED_NegVal(GREEN_LED_DeviceData);
/*Ask for rules with same consequent*/
maximo(Tnorm_min);
_lwsem_post(&semaforo);
}
}
/*
** ===================================================================
** Event :
defuzzification_task (module mqx_tasks)
**
** Component :
Task6 [MQXLite_task]
** Description :
** MQX task routine. The
routine is generated into mqx_tasks.c
** file.
** Parameters :
** NAME - DESCRIPTION
** task_init_data -
** Returns : Nothing
** ===================================================================
*/
voiddefuzzification_task(uint32_t task_init_data)
{
int counter = 0;
while(1) {
_lwsem_wait(&semaforo);
GREEN_LED_NegVal(GREEN_LED_DeviceData);
dU = defuzificacion(Tnorm);
_task_ready(PTR_Main_Task);
}
}
/* END mqx_tasks */
#ifdef __cplusplus
}
/* extern
"C" */
#endif
#include"Cpu.h"
#include"Events.h"
#include"mqx_tasks.h"
#define DATARCV 1
#ifdef __cplusplus
extern"C" {
#endif
uint16 ValueADC;
externint SetPoint;
externshort position;
externLDD_TDeviceData *PTR_ADC;
externunsignedchar
MatLab_Data[16];
byte Error,RecvChrError,SendChrError;
externunsignedchar
ConvertedValues[16],*ConvertedValuesptr;
/* Use light weight Events */
LWEVENT_STRUCT lwevent;
** ===================================================================
** Event :
AS1_OnRxChar (module Events)
**
** Component : AS1
[AsynchroSerial]
** Description :
** This event is called
after a correct character is received.
** The event is available
only when the <Interrupt
** service/event>
property is enabled and either the <Receiver>
** property is enabled or
the <SCI output mode> property (if
** supported) is set to
Single-wire mode.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
voidAS1_OnRxChar(void)
{
staticAS1_TComData *AP_BuffInput = MatLab_Data;
staticint i=0;
RecvChrError =
AS1_RecvChar(AP_BuffInput); //Recibedatode canal derecepcion
if (MatLab_Data[i]
== '\n'&& MatLab_Data[(i-1)&0xf] == 'U'){
_lwevent_set(&lwevent,DATARCV); //Habilitaevento DATORECIBIDO
position=i;} //Guardaposiciónpara el apuntador
AP_BuffInput++;
i=(i+1)&0xf;
if(!i)AP_BuffInput
= MatLab_Data;
}
/*
** ===================================================================
** Event :
AS1_OnTxChar (module Events)
**
** Component : AS1
[AsynchroSerial]
** Description :
** This event is called
after a character is transmitted.
** Parameters : None
** Returns : Nothing
** ===================================================================
*/
voidAS1_OnTxChar(void)
{
ConvertedValuesptr++;
if(*ConvertedValuesptr!=NULL){
while
((SendChrError = AS1_SendChar(*ConvertedValuesptr))!=ERR_OK);
}
}
/*
** ===================================================================
** Event :
AD1_OnMeasurementComplete (module Events)
**
** Component : AD1
[ADC_LDD]
*/
/*!
** @brief
** Called after
measurement is done, [Interrupt service/event]
** is enabled,
OnMeasurementComplete event is enabled and ADC
** device is enabled. See
[SetEventMask()] method or [Event
** mask] property group to
enable this event and [Enable]
** method or [Enabled in init.
code] property to enable ADC
** device. If DMA is
enabled , this event is called after the
** configured number of
measurements and DMA transfer is done.
** @param
** UserDataPtr - Pointer to the user or
** RTOS
specific data. The pointer is passed
** as
the parameter of Init method.
*/
/* ===================================================================*/
voidAD1_OnMeasurementComplete(LDD_TUserData *UserDataPtr)
{
AD1_GetMeasuredValues(PTR_ADC,&ValueADC); //Tomavalordel
ADC
SetPoint=(int)ValueADC*20/65000; //Escalavalorde 0 - 20
}
//PROTOTIPO
DE FUNCIONES===========================================================
//void
int16toascii(int);
int
str2int(int pos_actual, unsigned char MatLab_Data[16]);
int
numpow(int,int);
void
int2str(int,int);
void
membership_grade(int,int,int,int);
void
minimo(int MF1[3][2], int MF2[3][2], int RULES[4][4]);
void
maximo(int Tnorm_min[9][2]);
int
defuzificacion(int Tnorm[4][2]);
//=================================================================================
#ifndef
FUNCTIONS_H_
#define
FUNCTIONS_H_
#endif
/* FUNCTIONS_H_ */
#include"Functions.h"
/*Labels*/
#define HIGH_SPEED 0
#define SPEED_OK 1
#define LOW_SPEED 2
#define WITHOUT_MEMBERSHIP 3
#define LOWERING_SPEED 0
#define STEADY_SPEED 1
#define INCREASING_SPEED 2
/*-------------------------*/
#define GAIN 1000
externint
Value,MF1[3][2],MF2[3][2],Tnorm_min[9][2],Tnorm[4][2];
externunsignedchar
ConvertedValues[16];
//=================================================================================
//Funciónconviertecadenadedatos ASCII a entero.
//Parámetro 'posicion' eslaposición i delarreglodondeseencuentra
el
// terminador
\n.
//Parámetro 'MatLab[]' es el arreglo global dondese
están recibiendo losdatos
// serial.
Buffer circular de 16 posiciones.
//=================================================================================
intstr2int(int posicion, unsignedchar
MatLab_Data[16])
{
short pos_actual,i;
unsignedchar *AP_Data;
pos_actual=(posicion-2)&0xf; //Determinalaposicion a enviaralapuntadorposicion-2
debido a 'U' y '\n'
AP_Data=&MatLab_Data[pos_actual]; //Enviaapuntador a posiciondelasunidadesdeldato a leer
Value=*AP_Data-'0'; //Convierte ASCII a Decimal '0'= 30
i=10;
while (
*(AP_Data=&MatLab_Data[(--pos_actual)&0xf]) != '\n'){
//Si el contenidoen el Apuntadordecrementadoesdiferente
a '\n'
if(*AP_Data == '-')Value = -Value; //Sidatoessignonegativo, entonces el datoesnegativo.
else{Value=Value + (*AP_Data - '0')*i; //Si no sumardecenas, centenas y millares.
i=i*10;}
//Multiplicadordedecenas, centenas
y millares.
}
return Value;
}
//=================================================================================
//Funciónconvierteentero a cadenasignada.
//Parámetro value es el valorentero a convertir
//Parámetro p es el numerodedigitosincluyendo el signodelnumero
a convertir
//P. Ejemplo: value=
-12905 p = 6.
//El arregloquesemodificadebeserdeclarado global y debeserde
dimension
//mínima p+2 lafunción no regresavalor.
//=================================================================================
voidint2str(int value,int p)
{
int Aux,i,n,r;
ConvertedValues[p+1]='\n'; //EscribeTerminador
ConvertedValues[p]='U'; //EscribeTerminador
if (value < 0){ //Preguntapornumeronegativo
value=abs(value); //Hace el valorpositivo
ConvertedValues[0] = '-'; //Colocasignonegativoenprimeraposicion.
for(i=2;i<=p;i++){ //infierenumerocomenzandopor el massignificativo
Aux=numpow(10,(p-i)); //Llama a funciónnumpow
ConvertedValues[i-1]=((value/Aux)%10)+48;
}
}
else{
for(i=1;i<=p;i++){ //infierenumerocomenzandopor el massignificativo
Aux=numpow(10,(p-i)); //Llama a funciónnumpow
ConvertedValues[i-1]=((value/Aux)%10)+48;
}
}
}
//=================================================================================
//Funcioneleva 'num'
a potencia 'n'
//=================================================================================
intnumpow(int num,int n)
{
int Aux,i;
Aux=num;
if(n!=0) //Verificaquelapotencia a elevar sea diferente
a cero
for(i=1;i<=n-1;i++)Aux=Aux*num; //Calcula el numero a lapotencia 'n'
else Aux=1;
//si n=0 entonces el resultadoes
1
return Aux; //regresaentero
}
//=====================================================================
/*Funcióngradodemembresía*/
/*Calcula el gradodemembresía según valor y parametros*/
/*Losparámetrosdefinentresfuncionesdemembresíatipo triangular*/
//=====================================================================
voidmembership_grade(int x1,int x2,int A,int B)
{
#define LIMIT1 -100000
#define LIMIT2 100000
/*Universe 1*/
if (x1 <= (-A)
&& x1 >= LIMIT1){
MF1[0][0] =
GAIN*1; MF1[1][0] = 0; MF1[2][0] = 0;
MF1[0][1] =
HIGH_SPEED; MF1[1][1] = WITHOUT_MEMBERSHIP; MF1[2][1] = WITHOUT_MEMBERSHIP;}
if (x1 <= 0
&& x1 > (-A)){
MF1[0][0] =
GAIN* (-x1)/ A; MF1[1][0] = GAIN*(x1 + A)/A; MF1[2][0] = 0;
MF1[0][1] =
HIGH_SPEED; MF1[1][1] = SPEED_OK; MF1[2][1] = WITHOUT_MEMBERSHIP;}
if (x1 <= A
&& x1 > 0){
MF1[0][0] = 0;
MF1[1][0] = GAIN*(A - x1)/A; MF1[2][0] = GAIN*x1/A;
MF1[0][1] =
WITHOUT_MEMBERSHIP; MF1[1][1] = SPEED_OK; MF1[2][1] = LOW_SPEED;}
if(x1 <= LIMIT2
&& x1 > A){
MF1[0][0] = 0;
MF1[1][0] = 0; MF1[2][0] = GAIN*1;
MF1[0][1] =
WITHOUT_MEMBERSHIP; MF1[1][1] = WITHOUT_MEMBERSHIP; MF1[2][1] = LOW_SPEED;}
/*Universe 2*/
if (x2 <= (-B)
&& x2 >= LIMIT1){
MF2[0][0] =
GAIN*1; MF2[1][0] = 0; MF2[2][0] = 0;
MF2[0][1] =
LOWERING_SPEED; MF2[1][1] = WITHOUT_MEMBERSHIP; MF2[2][1] =
WITHOUT_MEMBERSHIP;}
if (x2 <= 0
&& x2 > (-B)){
MF2[0][0] =
GAIN*(-x2)/B; MF2[1][0] = GAIN*(x2 + B)/B; MF2[2][0] = 0;
MF2[0][1] =
LOWERING_SPEED; MF2[1][1] = STEADY_SPEED; MF2[2][1] = WITHOUT_MEMBERSHIP;}
if (x2 <= B
&& x2 > 0){
MF2[0][0] = 0;
MF2[1][0] = GAIN*(B - x2)/B; MF2[2][0] = GAIN*x2/B;
MF2[0][1] =
WITHOUT_MEMBERSHIP; MF2[1][1] = STEADY_SPEED; MF2[2][1] = INCREASING_SPEED;}
if(x2 <= LIMIT2
&& x2 > B){
MF2[0][0] = 0;
MF2[1][0] = 0; MF2[2][0] = GAIN*1;
MF2[0][1] =
WITHOUT_MEMBERSHIP; MF2[1][1] = WITHOUT_MEMBERSHIP; MF2[2][1] =
INCREASING_SPEED;}
}
//=====================================================================
/*Funciónquerealizaoperaciónmínimoparalacombinacióndelos
conjuntos, esdecirparalasreglasdifusas.
PARAMETROS: MF1 y MF2 son losarregloscon
el gradodemembresía*/
/*Modifica el arreglo Tnorm_min el cualdebeser
global*/
//=====================================================================
voidminimo(int MF1[3][2], int MF2[3][2], int RULES[4][4])
{
short i,j,k = 0;
for(i = 0; i <= 2; i++){
for(j = 0; j <=
2; j++){
Tnorm_min[k][0]
= MF1[i][0] < MF2[j][0] ? MF1[i][0] : MF2[j][0];
Tnorm_min[k][1]
= RULES[(MF1[i][1])] [(MF2[j][1])];
k++;
}
}
}
//=====================================================================
/*Funciónquerealizaoperación máximo paraconjuntosrepetidos
Esdecirparareglasconigualconsecuente
PARAMETRO Tnorm_min es el
resultadodefunciónminimo
/*Modifica el arregloTnorm el cualdebeser global*/
voidmaximo(int Tnorm_min[9][2])
{
short i,j,k = 0;
for(i = 0; i <= 8; i++){
if
(Tnorm_min[i][1] != 0){
Tnorm[k][0] =
Tnorm_min[i][0];
Tnorm[k][1] =
Tnorm_min[i][1];
for(j = i+1; j
<= 8; j++){
if
(Tnorm_min[j][1] != 0){
if
(Tnorm_min[i][1] == Tnorm_min[j][1]){
Tnorm[k][0]
= Tnorm[k][0] > Tnorm_min[j][0] ? Tnorm[k][0] : Tnorm_min[j][0];
Tnorm_min[j][0]
= 0; Tnorm_min[j][1] = 0;
}
}
}
k++;
}
}
}
//=====================================================================
/*FunciónquerealizaprocesodeDedifuzificaciónpor método debarras*/
//=====================================================================
intdefuzificacion(int Tnorm[4][2])
{
int COG = 0,i,AUX = 0;
for (i = 0; i <= 3; i++){
AUX += Tnorm[i][0];
}
for (i = 0; i <= 3; i++){
COG +=
Tnorm[i][0]*Tnorm[i][1]/AUX;
}
return COG;
}
No hay comentarios:
Publicar un comentario