Subversion Repositories shark

#include <math.h>

#include <modules/hartport.h>
#include <kernel/kern.h>
#include <kernel/func.h>
#include <ll/i386/x-dos.h>

#include "drivers/pclab.h"

#include "const.h"

float vmax = 0.0, vmin = 0.0, vmaxth = 0.0, vminth = 0.0;

int da_motor(float v)
{

  da_conv( 2.5 , 2 );
  da_conv( v + 2.5 , 1 );

  return(0);

}

float v2x(float v)
{
  float x;

  x= LUNGH * (v-(vmax+vmin)/2 ) / (vmax-vmin);

  return x;

}

float v2theta(float v)
{
  float theta;
 
  theta=2.0*(THETAMAX/FCA) * (v-(vmaxth+vminth)/2 ) / (vmaxth-vminth);
 
  return theta;

}

float bass1(float u)
{
  float y;
  static float oldy=0;

  y=(oldy + prm.WCUT * DEADSECX(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECX(PERIOD_CARRELLO));
  oldy=y;

  return y;

}

float bass2(float u)
{
  float y;
  static float oldy=0;

  y=(oldy + prm.WCUT1 * DEADSECX(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECX(PERIOD_CARRELLO));
  oldy=y;

  return y;

}

float bass3(float u)
{
  float y;
  static float oldy=0;

  y=(oldy + prm.WCUT * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECTH(PERIOD_CARRELLO));
  oldy=y;

  return y;

}

float bass4(float u)
{
  float y;
  static float oldy=0;

  y=(oldy + prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO));
  oldy=y;

  return y;

}

float  dx(float u)
{
  static float oldu=0;
  float y;

  y=(u-oldu)/DEADSECX(PERIOD_CARRELLO);
  oldu=u;

  return y;

}

float  dth(float u)
{
  static float oldu=0;
  float y;

  y=(u-oldu)/DEADSECTH(PERIOD_CARRELLO);
  oldu=u;

  return y;

}

TASK carrello(void)
{
  float   y[2]={0,0}, yp[2]={0,0};
  PORT    px, pth;                        
 
  float th_input = 0.0,arr_th_input[AVR],th_eff,av_th_input;
  float x_input = 0.0,arr_x_input[AVR],x_eff,av_x_input;

  int flag_th=1,flag_x=1,index_th=0,index_x=0;
 
  int i;
  float offset,vout,vout_total;
 
  px = port_create("porta1",sizeof(float),1,STICK,WRITE);
  pth = port_create("porta2",sizeof(float),1,STICK,WRITE);

  while (1) {

    task_nopreempt();
    th_input=ad_conv(10);
    task_preempt();
     
    if(flag_th==1) {
      for(i=0; i<AVR;++i) arr_th_input[i] = th_input;
      flag_th=0;
    }
    av_th_input=0;
    for(i=0;i<AVR;++i) av_th_input += arr_th_input[i];
    av_th_input /= AVR ;

    if(fabs(th_input-av_th_input)>=prm.NOISE) th_input=av_th_input;

    arr_th_input[index_th]=th_input;
    index_th = (index_th+1) % AVR ;

    th_eff = v2theta(th_input);
    y[1] = bass3(th_eff);
    yp[1] = bass4(dth(y[1]));

    task_nopreempt();
    x_input=ad_conv(11);
    task_preempt();

    if(flag_x==1) {
      for(i=0; i<AVR;++i) arr_x_input[i] = x_input;
      flag_x=0;
    }
    av_x_input=0;
    for(i=0;i<AVR;++i) av_x_input += arr_x_input[i];
    av_x_input /= AVR;
    if(fabs(x_input-av_x_input)>=prm.NOISE) x_input = av_x_input;

    arr_x_input[index_x] = x_input;
    index_x = (index_x+1) % AVR ;
       
    x_eff = v2x(x_input);      
    y[0] = bass1(x_eff);
    yp[0] = bass2(dx(y[0]));
       
    vout = prm.COST * (prm.GUAD[0] * (y[0]-v2x((vmax+vmin)/2)) + prm.GUAD[1] * th_eff\
                    + prm.GUAD[2] * yp[0] + prm.GUAD[3] * yp[1]);

    if(vout >= 0) offset=prm.OFFSVAL;
        else offset=-prm.OFFSVAL;

    vout_total = vout + offset;

    if(vout_total >= VDANG ) vout_total = VDANG ;
        if(vout_total <= -VDANG ) vout_total = -VDANG ;
       
    da_motor(vout_total);

    port_send(px, &y[0], NON_BLOCK);
    port_send(pth, &y[1], NON_BLOCK);

    task_endcycle();
 
  }
 
  port_delete(px);
  port_delete(pth);

}   /* FINE DEL TASK CARRELLO */