Rev 1655 | Details | Compare with Previous | 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 */ |