aibicom.c

Go to the documentation of this file.

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

Aibicom (Asynchronous Interpreter of Binary Commands) is a C
subroutine library for the develpment of binary control applications.

Copyright (C) 2006-2007 by Jorge Silva
e-mail: jorge.silva@komodoopenlab.com

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

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

#include<stdlib.h>
#include<math.h>

#include "aibicom.h"

double   *SS_DOMAIN;     
double   *SS_MASK;       
double   *SS_ESTIMATE;   
double   *ESTIMATE;
double   SS_RD1;          
double   SS_RD2;          
int    SS_OUTI;        
void SS_LinSpace(void)
{
    int i;

    for (i = 0; i < SS_RES; i++)
        SS_DOMAIN[i] = ((((double)i / (SS_RES - 1)) * (SS_RD1)) + SS_MIND1);
}

double SS_GetD1(int i)
{
    return SS_DOMAIN[i];
}

double SS_GetD2(int i)
{
    double tmp;

    tmp = (SS_C * SS_DOMAIN[i]);
    while ((tmp > SS_MAXD1) || (tmp < SS_MIND1))
        if (tmp >= 0.0)
            tmp = tmp - SS_RD1;
        else
            tmp = tmp + SS_RD1;

    return (((tmp - SS_MIND1) / SS_RD1) * SS_RD2) + SS_MIND2;
}

void SS_UpdateMask(void)
{
    int i;
    double s, d, dd1, dd2;

    s = SS_MWIDTH / 2.0;

    for (i = 0; i < SS_RES; i++)
    {
        /* Calculate distance d to current behavior */
        if (SS_2D)
        {
            dd1 = ((SS_GetD1(i) - SS_GetD1(SS_OUTI)) / SS_RD1);
            dd2 = ((SS_GetD2(i) - SS_GetD2(SS_OUTI)) / SS_RD2);
            d = sqrt((dd1 * dd1) + (dd2 * dd2));
        }
        else
            d = fabs(SS_GetD1(i) - SS_GetD1(SS_OUTI)) / SS_RD1;

        /* Add new radial masks below */
        switch (SS_MTYPE)
        {
        /* Delta mask */
        case 0:
            if(i==SS_OUTI)
                SS_MASK[i] = 1;
            else
                SS_MASK[i] = 0;
            break;
        /* Delta mask eliminating coordinate (0,0) at all times */
        case 1:
            if((i==SS_OUTI) || ((SS_GetD1(i) == 0.0) && (SS_GetD2(i) == 0.0)))
                SS_MASK[i] = 1;
            else
                SS_MASK[i] = 0;
            break;
        /* Linear mask */
        case 2:
            if (d <= s)
                SS_MASK[i] = 1.0 - (d / s);
            else
                SS_MASK[i] = 0.0;
            break;

        /* Linear mask eliminating coordinate (0,0) at all times */
        case 3:
            if (d <= s)
                SS_MASK[i] = 1.0 - (d / s);
            else
                SS_MASK[i] = 0.0;
            if ((SS_GetD1(i) == 0.0) && (SS_GetD2(i) == 0.0))
                SS_MASK[i] = 1.0;
            break;
        }
    }
}

int SS_FindMin(double *V)
{
    double min;
    int i, j, minc, mini, minj;

    /*Find minimum value*/
    min = 1.0;
    mini = 0;
    for (i = 0; i < SS_RES; i++)
    {
        if (V[i] < min)
        {
            min = V[i];
            mini = i;
        }
    }

    /*Anchor estimate and count minima*/
    minc = 0;
    for (i = 0; i < SS_RES; i++)
    {
        V[i] = V[i] - min;
        if (V[i] == 0.0)
            minc++;
    }

    if (minc > 1) printf("More than 1 minimum found!\n");

/*
    if (minc > 1)
    {
        printf("More than 1 minimum found!\n");
        for (i = 0; i < SS_RES; i++)
            printf("%.4f\t",SS_ESTIMATE[i]);
        printf("\n");
    }
*/

    /*Obtain new index*/
    if (minc > 1)
    {
        minj = rand() % minc;
        i = -1;
        j = -1;
        while(j < minj)
        {
            i++;
            if (V[i] == 0.0)
                j++;
        }
        mini = i; /* Selected minimum */
    }

    return mini;

}

void SS_Init(void)
{

    int i;

    SS_DOMAIN = malloc(SS_RES * sizeof(double));
    SS_MASK = malloc(SS_RES * sizeof(double));
    SS_ESTIMATE = malloc(SS_RES * sizeof(double));
    ESTIMATE = malloc(SS_RES * sizeof(double));

    SS_RD1 = SS_MAXD1 - SS_MIND1;
    SS_RD2 = SS_MAXD2 - SS_MIND2;

    SS_LinSpace();

    /*Initialize Intention Estimate and Mask*/
    for (i = 0; i < SS_RES; i++)
    {
        SS_MASK[i] = 0.0;
        if ((SS_GetD1(i) == 0.0) && (SS_GetD2(i) == 0.0) && (SS_MTYPE == 2))
            SS_MASK[i] = 1.0;
        SS_ESTIMATE[i] = (double)rand() / (RAND_MAX + 1.0);
    }

}

void SS_ApplyDecay(double *V, double K)
{
    int i;

/*
    printf("%.4f\n",Dt);
*/

    /*Recursive (viscoelastic) Decay*/
    for (i = 0; i < SS_RES; i++)
        V[i] = K * V[i];
}

int SS_IfExecute(void)
{
    int i, OUTI;

    for (i = 0; i < SS_RES; i++)
        ESTIMATE[i] = SS_ESTIMATE[i];

    /*Apply decay*/
    SS_ApplyDecay(ESTIMATE, SS_K);

    /*Apply mask and obtain minimum*/
    for (i = 0; i < SS_RES; i++)
        ESTIMATE[i] = ESTIMATE[i] + (SS_MASK[i] * (1.0 - ESTIMATE[i]));

    OUTI = SS_FindMin(ESTIMATE);

    return OUTI;
}

int SS_Execute(void)
{
    int i;

        /* Debug: print estimate */

    /*for (i = 0; i < SS_RES; i++)
    {
        printf("%.4f",SS_ESTIMATE[i]);
        if (i == (SS_RES - 1))
            printf("\n");
        else
            printf("\t");
    }*/

    /*Apply decay*/
    SS_ApplyDecay(SS_ESTIMATE, SS_K);

        /* Debug: print estimate */

    /*for (i = 0; i < SS_RES; i++)
    {
        printf("%.4f",SS_ESTIMATE[i]);
        if (i == (SS_RES - 1))
            printf("\n");
        else
            printf("\t");
    }*/

    /*Apply mask and obtain minimum*/
    for (i = 0; i < SS_RES; i++)
        SS_ESTIMATE[i] = SS_ESTIMATE[i] + (SS_MASK[i] * (1.0 - SS_ESTIMATE[i]));

        /* Debug: print estimate */
    /*for (i = 0; i < SS_RES; i++)
    {
        printf("%.4f",SS_ESTIMATE[i]);
        if (i == (SS_RES - 1))
            printf("\n");
        else
            printf("\t");
    }*/
    SS_OUTI = SS_FindMin(SS_ESTIMATE);

        /* Debug: print estimate */
    /*for (i = 0; i < SS_RES; i++)
    {
        printf("%.4f",SS_ESTIMATE[i]);
        if (i == (SS_RES - 1))
            printf("\n");
        else
            printf("\t");
    }*/
        /*printf("\n");*/

    SS_UpdateMask();

/* Debug: print mask */
/*
    for (i = 0; i < SS_RES; i++)
    {
        printf("%f",SS_MASK[i]);
        if (i == (SS_RES - 1))
            printf("\n");
        else
            printf("\t");
    }
*/
/* Debug: print estimate */
/*
    for (i = 0; i < SS_RES; i++)
    {
        printf("%.4f",SS_ESTIMATE[i]);
        if (i == (SS_RES - 1))
            printf("\n");
        else
            printf("\t");
    }
*/
    return SS_OUTI;

}

void SS_Clear(void)
{
    free(SS_DOMAIN);
    free(SS_MASK);
    free(SS_ESTIMATE);
    free(ESTIMATE);
}

---