Subversion Repositories shark

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1655 giacomo 1
#include <math.h>
2
 
3
#include <modules/hartport.h>
4
#include <kernel/kern.h>
5
#include <kernel/func.h>
6
#include <ll/i386/x-dos.h>
7
 
8
#include "drivers/pclab.h"
9
 
10
#include "const.h"
11
 
12
float vmax = 0.0, vmin = 0.0, vmaxth = 0.0, vminth = 0.0;
13
 
14
int da_motor(float v)
15
{
16
 
17
  da_conv( 2.5 , 2 );
18
  da_conv( v + 2.5 , 1 );
19
 
20
  return(0);
21
 
22
}
23
 
24
float v2x(float v)
25
{
26
  float x;
27
 
28
  x= LUNGH * (v-(vmax+vmin)/2 ) / (vmax-vmin);
29
 
30
  return x;
31
 
32
}
33
 
34
float v2theta(float v)
35
{
36
  float theta;
37
 
38
  theta=2.0*(THETAMAX/FCA) * (v-(vmaxth+vminth)/2 ) / (vmaxth-vminth);
39
 
40
  return theta;
41
 
42
}
43
 
44
float bass1(float u)
45
{
46
  float y;
47
  static float oldy=0;
48
 
49
  y=(oldy + prm.WCUT * DEADSECX(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECX(PERIOD_CARRELLO));
50
  oldy=y;
51
 
52
  return y;
53
 
54
}
55
 
56
float bass2(float u)
57
{
58
  float y;
59
  static float oldy=0;
60
 
61
  y=(oldy + prm.WCUT1 * DEADSECX(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECX(PERIOD_CARRELLO));
62
  oldy=y;
63
 
64
  return y;
65
 
66
}
67
 
68
float bass3(float u)
69
{
70
  float y;
71
  static float oldy=0;
72
 
73
  y=(oldy + prm.WCUT * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECTH(PERIOD_CARRELLO));
74
  oldy=y;
75
 
76
  return y;
77
 
78
}
79
 
80
float bass4(float u)
81
{
82
  float y;
83
  static float oldy=0;
84
 
85
  y=(oldy + prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO));
86
  oldy=y;
87
 
88
  return y;
89
 
90
}
91
 
92
float  dx(float u)
93
{
94
  static float oldu=0;
95
  float y;
96
 
97
  y=(u-oldu)/DEADSECX(PERIOD_CARRELLO);
98
  oldu=u;
99
 
100
  return y;
101
 
102
}
103
 
104
float  dth(float u)
105
{
106
  static float oldu=0;
107
  float y;
108
 
109
  y=(u-oldu)/DEADSECTH(PERIOD_CARRELLO);
110
  oldu=u;
111
 
112
  return y;
113
 
114
}
115
 
116
TASK carrello(void)
117
{
118
  float   y[2]={0,0}, yp[2]={0,0};
119
  PORT    px, pth;                        
120
 
121
  float th_input = 0.0,arr_th_input[AVR],th_eff,av_th_input;
122
  float x_input = 0.0,arr_x_input[AVR],x_eff,av_x_input;
123
 
124
  int flag_th=1,flag_x=1,index_th=0,index_x=0;
125
 
126
  int i;
127
  float offset,vout,vout_total;
128
 
129
  px = port_create("porta1",sizeof(float),1,STICK,WRITE);
130
  pth = port_create("porta2",sizeof(float),1,STICK,WRITE);
131
 
132
  while (1) {
133
 
134
    task_nopreempt();
135
    th_input=ad_conv(10);
136
    task_preempt();
137
 
138
    if(flag_th==1) {
139
      for(i=0; i<AVR;++i) arr_th_input[i] = th_input;
140
      flag_th=0;
141
    }
142
    av_th_input=0;
143
    for(i=0;i<AVR;++i) av_th_input += arr_th_input[i];
144
    av_th_input /= AVR ;
145
 
146
    if(fabs(th_input-av_th_input)>=prm.NOISE) th_input=av_th_input;
147
 
148
    arr_th_input[index_th]=th_input;
149
    index_th = (index_th+1) % AVR ;
150
 
151
    th_eff = v2theta(th_input);
152
    y[1] = bass3(th_eff);
153
    yp[1] = bass4(dth(y[1]));
154
 
155
    task_nopreempt();
156
    x_input=ad_conv(11);
157
    task_preempt();
158
 
159
    if(flag_x==1) {
160
      for(i=0; i<AVR;++i) arr_x_input[i] = x_input;
161
      flag_x=0;
162
    }
163
    av_x_input=0;
164
    for(i=0;i<AVR;++i) av_x_input += arr_x_input[i];
165
    av_x_input /= AVR;
166
    if(fabs(x_input-av_x_input)>=prm.NOISE) x_input = av_x_input;
167
 
168
    arr_x_input[index_x] = x_input;
169
    index_x = (index_x+1) % AVR ;
170
 
171
    x_eff = v2x(x_input);      
172
    y[0] = bass1(x_eff);
173
    yp[0] = bass2(dx(y[0]));
174
 
175
    vout = prm.COST * (prm.GUAD[0] * (y[0]-v2x((vmax+vmin)/2)) + prm.GUAD[1] * th_eff\
176
                    + prm.GUAD[2] * yp[0] + prm.GUAD[3] * yp[1]);
177
 
178
    if(vout >= 0) offset=prm.OFFSVAL;
179
        else offset=-prm.OFFSVAL;
180
 
181
    vout_total = vout + offset;
182
 
183
    if(vout_total >= VDANG ) vout_total = VDANG ;
184
        if(vout_total <= -VDANG ) vout_total = -VDANG ;
185
 
186
    da_motor(vout_total);
187
 
188
    port_send(px, &y[0], NON_BLOCK);
189
    port_send(pth, &y[1], NON_BLOCK);
190
 
191
    task_endcycle();
192
 
193
  }
194
 
195
  port_delete(px);
196
  port_delete(pth);
197
 
198
}   /* FINE DEL TASK CARRELLO */