Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1659 | giacomo | 1 | /************************************************************* |
| 2 | * * |
||
| 3 | * Stabilizzazione di un pendolo inverso * |
||
| 4 | * * |
||
| 5 | * * |
||
| 6 | *Written by: Gabriele Bolognini - Scuola S.Anna -- Pisa * |
||
| 7 | * * |
||
| 8 | *************************************************************/ |
||
| 9 | #include "demo.h" |
||
| 10 | #include "pclab.h" |
||
| 11 | #include "cost.h" /* Contiene le costanti del task DISEGNA */ |
||
| 12 | |||
| 13 | |||
| 14 | |||
| 15 | #ifdef SOFTPROG |
||
| 16 | long int PERIOD_CARRELLO={1200}, PERIOD_TRACKING={12000}; |
||
| 17 | long int WCET_CARRELLO={240}, WCET_TRACKING={7200}; |
||
| 18 | #else |
||
| 19 | long int PERIOD_CARRELLO={1800}, PERIOD_TRACKING={22200}; |
||
| 20 | #endif |
||
| 21 | |||
| 22 | #define LUNGH 35.0 |
||
| 23 | #define THETAMAX 45.0 |
||
| 24 | #define VDANG 2.4 |
||
| 25 | #define NMAX 300 |
||
| 26 | #define SEC_ID 7.0 |
||
| 27 | #define MAXRUN_ID 9 //16 |
||
| 28 | int start_id=0; |
||
| 29 | // dati da scrivere nei files |
||
| 30 | |||
| 31 | static long int contat_id=-1, num_id=0; |
||
| 32 | #ifdef DATAPLANT |
||
| 33 | struct DATA_FILE { |
||
| 34 | TIME time; |
||
| 35 | float xist ; |
||
| 36 | float thist ; |
||
| 37 | float par1 ; |
||
| 38 | float par2 ; |
||
| 39 | float par3 ; |
||
| 40 | float par4 ; |
||
| 41 | } dati[NMAX]; |
||
| 42 | long int numdat=0; // numero dati scritti su DATI[NMAX] |
||
| 43 | #endif |
||
| 44 | |||
| 45 | #define NSAMPLES 500 |
||
| 46 | #define NTASKS 10 |
||
| 47 | static float timetab[NSAMPLES][NTASKS]; |
||
| 48 | static int index[NTASKS]; |
||
| 49 | static float somma[MAXRUN_ID][2]; // valore dello indice di prestazione |
||
| 50 | mutex_t mutex,mutexadc; |
||
| 51 | |||
| 52 | #define MAX_TEMPI 15 |
||
| 53 | static char stringa[MAX_TEMPI][250]; |
||
| 54 | // **- ACHTUNG!!!: param. iniz. del guadagno, deadline, ecc.-----******* / |
||
| 55 | #define INIPARAMS {{ 2.3 , 300 , 0.5 , 15 },1.,1.0,20.,20., 0.1, 1 , 1, 0.} |
||
| 56 | #define MODPARAMS {{ 1. , 300 , 0.9 , 34 },0.74 ,1.,20.,20., 0.2, 1 , 1, 0.} |
||
| 57 | #define PARAM2 {{ 1. , 300 , 0.79 , 23 },0.70 ,2.5 ,20.,20., 0.1, 1 , 1, 0.} |
||
| 58 | /* |
||
| 59 | struct Parametri{ |
||
| 60 | float GUAD[4]; |
||
| 61 | float COST; |
||
| 62 | float NOISE; |
||
| 63 | float WCUT; |
||
| 64 | float WCUT1; |
||
| 65 | float OFFSVAL; |
||
| 66 | int DEADLINE; |
||
| 67 | int SCAN; |
||
| 68 | float XTRASL; |
||
| 69 | }; |
||
| 70 | struct Data_cab1 { |
||
| 71 | float rif; |
||
| 72 | int imp; |
||
| 73 | }; |
||
| 74 | struct Data_cab2 { |
||
| 75 | float x; |
||
| 76 | float y; |
||
| 77 | } ; |
||
| 78 | */ |
||
| 79 | struct Parametri prm=PARAM2, prmbase=INIPARAMS, prmmod=MODPARAMS, prm2=PARAM2; |
||
| 80 | |||
| 81 | |||
| 82 | |||
| 83 | #define DEADSECX(x) ( x /*PERIOD_TRACKING*/ * 1.0e-6 ) // mit task TRACK in sec |
||
| 84 | #define DEADSECTH(x) (x /*PERIOD_CARRELLO*/ * 1.0e-6 ) // mit task CARRELLO in sec |
||
| 85 | |||
| 86 | |||
| 87 | CAB cab1, cab2; |
||
| 88 | |||
| 89 | float vmax, vmin, vmaxth, vminth; |
||
| 90 | |||
| 91 | float vin=0, vout, yout, thout; |
||
| 92 | |||
| 93 | |||
| 94 | |||
| 95 | void graphframe(void); |
||
| 96 | void my_end(void); |
||
| 97 | void scritt(void); |
||
| 98 | void scritt1(void); |
||
| 99 | void scritt2(void); |
||
| 100 | void scritt3(void); |
||
| 101 | void nuovi_tempi(long int*); |
||
| 102 | void initial(void); |
||
| 103 | |||
| 104 | void scenario_jetcontrol(void); |
||
| 105 | void init_jetcontrol(void); |
||
| 106 | TASK jetslide_task(void *); |
||
| 107 | TASK jetctrl_task(void *); |
||
| 108 | TASK jetdummy_task(void *); |
||
| 109 | |||
| 110 | |||
| 111 | void framegrabber_close(void *); |
||
| 112 | TASK camera_task(void *); |
||
| 113 | TASK tracking_task(void *); |
||
| 114 | |||
| 115 | extern int white, black, red, gray, green, blue; |
||
| 116 | float integrale(float y[], float u, int flag) |
||
| 117 | { |
||
| 118 | static float sum=0, oldcoeff=0; |
||
| 119 | float coeff=0 ; |
||
| 120 | static float R=1 , Q[2][2]={1,0,0,1.5} ; |
||
| 121 | |||
| 122 | if(flag) sum=0; //reset |
||
| 123 | coeff=Q[0][0]*y[0]*y[0]+Q[0][1]*y[0]*y[1]+Q[1][0]*y[1]*y[0]+Q[1][1]*y[1]*y[1]+R*u*u; |
||
| 124 | sum += (coeff+oldcoeff)*DEADSECTH(PERIOD_CARRELLO) / 2.0 ; |
||
| 125 | oldcoeff=coeff; |
||
| 126 | return sum; |
||
| 127 | } |
||
| 128 | void zerofun(KEY_EVT *k) |
||
| 129 | { |
||
| 130 | int i; |
||
| 131 | for (i=0; i<MAX_PROC; i++) jet_delstat(i); |
||
| 132 | } |
||
| 133 | |||
| 134 | |||
| 135 | int da_motor(float v) |
||
| 136 | { |
||
| 137 | da_conv( 2.5 , 2 ); |
||
| 138 | da_conv( v + 2.5 , 1 ); |
||
| 139 | return(0); |
||
| 140 | } |
||
| 141 | float v2x(float v) |
||
| 142 | { /* trasforma i volt in x */ |
||
| 143 | float x; |
||
| 144 | x= LUNGH * (v-(vmax+vmin)/2 ) / (vmax-vmin); |
||
| 145 | return x; |
||
| 146 | } |
||
| 147 | float v2theta(float v) |
||
| 148 | { /* trasforma i volt in angoli */ |
||
| 149 | float theta; |
||
| 150 | theta=2.0*(THETAMAX/FCA) * (v-(vmaxth+vminth)/2 ) / (vmaxth-vminth); |
||
| 151 | return theta; |
||
| 152 | } |
||
| 153 | float bass1(float u) |
||
| 154 | { |
||
| 155 | float y; |
||
| 156 | static float oldy=0; |
||
| 157 | y=(oldy + prm.WCUT * DEADSECX(PERIOD_TRACKING) *u)/(1+ prm.WCUT * DEADSECX(PERIOD_TRACKING)); |
||
| 158 | oldy=y; |
||
| 159 | return y; |
||
| 160 | } |
||
| 161 | float bass2(float u) |
||
| 162 | { |
||
| 163 | float y; |
||
| 164 | static float oldy=0; |
||
| 165 | y=(oldy + prm.WCUT1 * DEADSECX(PERIOD_TRACKING) *u)/(1+ prm.WCUT1 * DEADSECX(PERIOD_TRACKING)); |
||
| 166 | oldy=y; |
||
| 167 | return y; |
||
| 168 | } |
||
| 169 | float bass3(float u) |
||
| 170 | { |
||
| 171 | float y; |
||
| 172 | static float oldy=0; |
||
| 173 | y=(oldy + prm.WCUT * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECTH(PERIOD_CARRELLO)); |
||
| 174 | oldy=y; |
||
| 175 | return y; |
||
| 176 | } |
||
| 177 | float bass4(float u) |
||
| 178 | { |
||
| 179 | float y; |
||
| 180 | static float oldy=0; |
||
| 181 | y=(oldy + prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO)); |
||
| 182 | oldy=y; |
||
| 183 | return y; |
||
| 184 | } |
||
| 185 | float dx(float u) |
||
| 186 | { |
||
| 187 | static float oldu=0; |
||
| 188 | float y; |
||
| 189 | y=(u-oldu)/DEADSECX(PERIOD_TRACKING); |
||
| 190 | oldu=u; |
||
| 191 | return y; |
||
| 192 | } |
||
| 193 | float dth(float u) |
||
| 194 | { |
||
| 195 | static float oldu=0; |
||
| 196 | float y; |
||
| 197 | y=(u-oldu)/DEADSECTH(PERIOD_CARRELLO); |
||
| 198 | oldu=u; |
||
| 199 | return y; |
||
| 200 | } |
||
| 201 | |||
| 202 | TASK eval(void) |
||
| 203 | { |
||
| 204 | int x,y; |
||
| 205 | char str[100]; |
||
| 206 | mutex_lock(&mutex); |
||
| 207 | |||
| 208 | grx_rect(10,325,XTI - 20,479,blue); |
||
| 209 | x = 20; |
||
| 210 | y = 350; |
||
| 211 | grx_text("PARAMETRI DI CONTROLLO ( R , T ) ", x+60, y-18 , red, 0); |
||
| 212 | grx_text("Guad=", x, y,green, 0);y+=18; |
||
| 213 | grx_text("Cost=", x, y,green, 0); y+=18; |
||
| 214 | |||
| 215 | grx_text("WCUT=", x, y,green, 0); y+=18; |
||
| 216 | grx_text("WCUT1=", x, y,green, 0); y+=18; |
||
| 217 | grx_text("NOISE=", x, y,green, 0); y+=18; |
||
| 218 | grx_text("DEADL=", x, y,green, 0);y+=18; |
||
| 219 | grx_text("OFFS=", x, y,green, 0);y+=18; |
||
| 220 | |||
| 221 | { /* legenda */ |
||
| 222 | int x=280, y=350; |
||
| 223 | grx_text("Guad 1,2 3,4 5,6 7,8", x, y,green, 0);y+=18; |
||
| 224 | grx_text("Cost [ , ]", x, y,green, 0); y+=18; |
||
| 225 | grx_text("WCUT o , p", x , y,green, 0); y+=18; |
||
| 226 | grx_text("WCUT1 k , l", x, y,green, 0); y+=18; |
||
| 227 | grx_text("NOISE n , m", x, y,green, 0); y+=18; |
||
| 228 | grx_text("DEADL - , +", x, y,green, 0);y+=18; |
||
| 229 | grx_text("OFFS / , *", x, y,green, 0);y+=18; |
||
| 230 | } |
||
| 231 | { /* frame dei valori rilevati dai sensori */ |
||
| 232 | int x=15, y=YTI+10; |
||
| 233 | grx_rect(10,YTI,XTI - 20,YTI+70,red); |
||
| 234 | grx_text("V_com = ", x, y,green, 0);y+=14; |
||
| 235 | grx_text("V+offs = ", x, y,green, 0);y+=14; |
||
| 236 | grx_text("V_sens_x = ", x, y,green, 0);y+=14; |
||
| 237 | grx_text("SCAN = ", x, y,green, 0);y+=14; |
||
| 238 | |||
| 239 | } |
||
| 240 | |||
| 241 | mutex_unlock(&mutex); |
||
| 242 | while(1){ |
||
| 243 | x = 70; |
||
| 244 | y = 350; |
||
| 245 | |||
| 246 | mutex_lock(&mutex); |
||
| 247 | |||
| 248 | grx_text(" ", x, y, 12, 0); |
||
| 249 | sprintf(str, "%6.2f %6.2f %6.2f %6.2f ", prm.GUAD[0],prm.GUAD[1],prm.GUAD[2],prm.GUAD[3]); |
||
| 250 | grx_text(str, x, y, blue, 0);y+=18; |
||
| 251 | |||
| 252 | grx_text(" ", x, y, 12, 0); |
||
| 253 | sprintf(str, "%6.2f ", prm.COST); |
||
| 254 | grx_text(str, x, y, blue, 0); y += 18; |
||
| 255 | |||
| 256 | grx_text(" ", x, y, 12, 0); |
||
| 257 | sprintf(str, "%6.2f Hz", prm.WCUT); |
||
| 258 | grx_text(str, x, y, blue, 0); y += 18; |
||
| 259 | |||
| 260 | grx_text(" ", x, y, 12, 0); |
||
| 261 | sprintf(str, "%6.2f Hz ", prm.WCUT1); |
||
| 262 | grx_text(str, x, y, blue, 0); y += 18; |
||
| 263 | |||
| 264 | grx_text(" ", x, y, 12, 0); |
||
| 265 | sprintf(str, "%6.2f Volt ", prm.NOISE); |
||
| 266 | grx_text(str, x, y, blue, 0); y += 18; |
||
| 267 | |||
| 268 | grx_text(" ", x, y, 12, 0); |
||
| 269 | sprintf(str, "%d ticks ", prm.DEADLINE); |
||
| 270 | grx_text(str, x, y, blue, 0); y += 18; |
||
| 271 | |||
| 272 | grx_text(" ", x, y, 12, 0); |
||
| 273 | sprintf(str, "%6.2f Volt ", prm.OFFSVAL); |
||
| 274 | grx_text(str, x, y, blue, 0); y += 18; |
||
| 275 | |||
| 276 | mutex_unlock(&mutex); |
||
| 277 | |||
| 278 | |||
| 279 | |||
| 280 | /* scrivo vin , vout (vin+offs) , SCAN e PARAMETRIBASE*/ |
||
| 281 | { int x=95, y= YTI + 10; |
||
| 282 | |||
| 283 | mutex_lock(&mutex); |
||
| 284 | |||
| 285 | grx_text(" ", x, y, 0, 0); |
||
| 286 | sprintf(str, "%7.4f ", vin); |
||
| 287 | grx_text(str, x, y, blue, 0);y += 14; |
||
| 288 | |||
| 289 | grx_text(" ", x, y, 0, 0); |
||
| 290 | sprintf(str, "%7.4f", vout); |
||
| 291 | grx_text(str, x, y, blue, 0);y += 14; |
||
| 292 | |||
| 293 | grx_text(" ", x, y, 0, 0); |
||
| 294 | sprintf(str, "%7.3f", yout); |
||
| 295 | grx_text(str, x, y , blue, 0); y += 14; |
||
| 296 | |||
| 297 | grx_text(" ", x, y, 0, 0); |
||
| 298 | sprintf(str, "%3.2d c , v", prm.SCAN); |
||
| 299 | grx_text(str, x, y , blue, 0); y += 14; |
||
| 300 | |||
| 301 | mutex_unlock(&mutex); |
||
| 302 | } |
||
| 303 | |||
| 304 | |||
| 305 | |||
| 306 | task_endcycle(); |
||
| 307 | } |
||
| 308 | } |
||
| 309 | /*------------------------------------------------------------------------ |
||
| 310 | Il task CARRELLO controlla la posizione del carrello |
||
| 311 | |||
| 312 | y = posizione angolare dell'asta (non usata) |
||
| 313 | x = posizione lineare del carrello |
||
| 314 | |||
| 315 | -----------------------------------------------------------------------*/ |
||
| 316 | float xrif=0, orif=1; |
||
| 317 | TASK carrello(void) |
||
| 318 | { |
||
| 319 | float thist=0, y[2]={0,0}, yp[2]={0,0}; /* valori attuali */ |
||
| 320 | PORT pa, pb; /* communication ports */ |
||
| 321 | struct Data_cab1 data; |
||
| 322 | struct Data_cab2 tmp; |
||
| 323 | |||
| 324 | char *pun, *pun2; |
||
| 325 | int i; |
||
| 326 | float offset; |
||
| 327 | |||
| 328 | long int k=0; |
||
| 329 | long int stime =0 ; |
||
| 330 | /* Si creano 2 per mandare al task DISEGNA le var.di stato x1 e x3 */ |
||
| 331 | |||
| 332 | pa = port_create("porta1",sizeof(float),1,STICK,WRITE); |
||
| 333 | pb = port_create("porta2",sizeof(float),1,STICK,WRITE); |
||
| 334 | |||
| 335 | while (1) { |
||
| 336 | |||
| 337 | /* prendo i dati sul riferimento in volt per la x */ |
||
| 338 | pun = cab_getmes(cab1); |
||
| 339 | memcpy(&data, pun, sizeof(struct Data_cab1)); |
||
| 340 | cab_unget(cab1, pun); |
||
| 341 | |||
| 342 | /* prendo i dati sulle posizioni dal trasduttore ADC */ |
||
| 343 | |||
| 344 | |||
| 345 | task_nopreempt(); |
||
| 346 | thout=ad_conv(10); |
||
| 347 | task_preempt(); |
||
| 348 | |||
| 349 | /* taglio brusco dei livelli di rumore sopra il NOISE */ |
||
| 350 | { |
||
| 351 | #define AVR 10 |
||
| 352 | static float othout[AVR]; |
||
| 353 | float avth; |
||
| 354 | static int index=0, flag=1; |
||
| 355 | |||
| 356 | if(flag==1) { |
||
| 357 | for(i=0; i<AVR;++i) { othout[i] = thout;} |
||
| 358 | flag=0; |
||
| 359 | } |
||
| 360 | avth=0; |
||
| 361 | for(i=0;i<AVR;++i) { avth += othout[i]; } |
||
| 362 | avth /= AVR ; |
||
| 363 | |||
| 364 | |||
| 365 | if(fabs(thout-avth)>=prm.NOISE) { thout=avth;} |
||
| 366 | |||
| 367 | othout[index]=thout; |
||
| 368 | index = (index+1) % AVR ; |
||
| 369 | } |
||
| 370 | |||
| 371 | /* converto i valori dei sensori in posizioni */ |
||
| 372 | /* calcolo le velocita' lineari e angolari opportunamente filtrate */ |
||
| 373 | |||
| 374 | |||
| 375 | thist=v2theta(thout); |
||
| 376 | y[1]=bass3(thist); |
||
| 377 | yp[1] = bass4(dth(y[1])); |
||
| 378 | |||
| 379 | |||
| 380 | /* a questo punto dovrei prendere i valori dall'altro task CARR2 */ |
||
| 381 | |||
| 382 | |||
| 383 | pun2 = cab_getmes(cab2); |
||
| 384 | memcpy(&tmp , pun2, sizeof(struct Data_cab2)); |
||
| 385 | cab_unget(cab2, pun2); |
||
| 386 | |||
| 387 | y[0]=yout=tmp.x; |
||
| 388 | yp[0]=tmp.y; |
||
| 389 | /* PARAMETRI del controllore :1 */ |
||
| 390 | vin = prm.COST * (prm.GUAD[0] * (y[0]-v2x(data.rif+prm.XTRASL))+ prm.GUAD[1] * thist + prm.GUAD[2] * yp[0] + prm.GUAD[3] * yp[1]) ; |
||
| 391 | |||
| 392 | |||
| 393 | /* metto un offset che mi elimina l'attrito */ |
||
| 394 | if( vin >= 0) offset=prm.OFFSVAL; |
||
| 395 | else offset=-prm.OFFSVAL; |
||
| 396 | |||
| 397 | vout=vin+offset; |
||
| 398 | |||
| 399 | |||
| 400 | if(vout >= VDANG ) vout = VDANG ; |
||
| 401 | if(vout <= -VDANG ) vout = -VDANG ; |
||
| 402 | /* la applico */ |
||
| 403 | da_motor(vout); |
||
| 404 | |||
| 405 | |||
| 406 | /* mando i dati sulle posizioni al task DISEGNA */ |
||
| 407 | port_send(pa, &y[1], NON_BLOCK); |
||
| 408 | port_send(pb, &y[0], NON_BLOCK); |
||
| 409 | |||
| 410 | { |
||
| 411 | char str[5]; |
||
| 412 | static long int contatwait=-1; |
||
| 413 | // extern float xrif; |
||
| 414 | if(start_id == 1) { |
||
| 415 | start_id=0; |
||
| 416 | contat_id=(int) (SEC_ID / DEADSECTH(PERIOD_CARRELLO)); |
||
| 417 | num_id++; |
||
| 418 | somma[num_id-1][1]=contat_id; |
||
| 419 | |||
| 420 | integrale(y,vout,1); |
||
| 421 | contatwait=(long int) (1.5 * SEC_ID / DEADSECTH(PERIOD_CARRELLO)); |
||
| 422 | |||
| 423 | } |
||
| 424 | if(contat_id > 0) { |
||
| 425 | xrif=0.0; //pone la xrif a zero |
||
| 426 | somma[num_id-1][0]=integrale(y,vout,0); |
||
| 427 | somma[num_id-1][1]= --contat_id; |
||
| 428 | } |
||
| 429 | if(contat_id == 0) { |
||
| 430 | if(num_id>= (MAXRUN_ID-1)) nuovi_tempi(&contatwait); //sys_end(); |
||
| 431 | xrif=-10.0; |
||
| 432 | contatwait--; if(contatwait==0) start_id=1;//attiva la racc. dati |
||
| 433 | } |
||
| 434 | if(contat_id == -1 ) ; // vero solo allo inizio del programma |
||
| 435 | } |
||
| 436 | /* aggiorno i riferimenti utilizzati in questo task, piu' in alto*/ |
||
| 437 | data.rif=(vmax+vmin)/2. + xrif * (vmax-vmin)/LUNGH; |
||
| 438 | data.imp=0; |
||
| 439 | |||
| 440 | pun = cab_reserve(cab1); |
||
| 441 | memcpy(pun, &data, sizeof(struct Data_cab1)); |
||
| 442 | cab_putmes(cab1, pun); |
||
| 443 | /* scrivo i dati istantanei (fino a NMAX) nella struttura 'dati' */ |
||
| 444 | #ifdef DATAPLANT |
||
| 445 | if((contat_id>0)&&(k<(NMAX-1))) { |
||
| 446 | dati[k].time=sys_gettime(NULL)/1000; |
||
| 447 | dati[k].xist=y[0]; |
||
| 448 | dati[k].thist=thist; |
||
| 449 | dati[k].par1=vout; |
||
| 450 | dati[k].par2=yp[0]; |
||
| 451 | dati[k].par3=yp[1]; |
||
| 452 | dati[k].par4=thist*FCA; |
||
| 453 | numdat=k++; |
||
| 454 | } |
||
| 455 | #endif |
||
| 456 | |||
| 457 | task_endcycle(); |
||
| 458 | } |
||
| 459 | /* Eliminazione porte di comunicazione */ |
||
| 460 | port_delete(pa); |
||
| 461 | port_delete(pb); |
||
| 462 | |||
| 463 | } /* FINE DEL TASK CARRELLO */ |
||
| 464 | |||
| 465 | |||
| 466 | /*--------------------------------------------------------------*/ |
||
| 467 | /* Il task DISEGNA riceve dal task CONTROLLO i valori delle */ |
||
| 468 | /* variabili x1 e x3 e disegna il pendolo corrispondente */ |
||
| 469 | /*--------------------------------------------------------------*/ |
||
| 470 | |||
| 471 | TASK diseg(void) |
||
| 472 | { |
||
| 473 | WORD xp, yp, oxp, oyp; /* coordinate carrello */ |
||
| 474 | WORD xa, ya, oxa, oya; /* coordinate asta */ |
||
| 475 | float x1, x3; /* variabili di stato */ |
||
| 476 | char str[100]; |
||
| 477 | PORT pra, prb, prc; /* communication ports */ |
||
| 478 | int delta; /* dimensione finestre */ |
||
| 479 | |||
| 480 | |||
| 481 | /* Connessione alle porte create dal task CONTROLLO */ |
||
| 482 | pra = port_connect("porta1",sizeof(float),STICK,READ); |
||
| 483 | prb = port_connect("porta2",sizeof(float),STICK,READ); |
||
| 484 | |||
| 485 | |||
| 486 | delta = 120; |
||
| 487 | |||
| 488 | /* Inizializzazione variabili carrello e asta */ |
||
| 489 | xp = oxp = 100; |
||
| 490 | xa = oxa = 100; |
||
| 491 | yp = oyp = 105 + delta - H; |
||
| 492 | ya = oya = 105 + delta - H; |
||
| 493 | |||
| 494 | |||
| 495 | graphframe(); |
||
| 496 | while (1) { |
||
| 497 | |||
| 498 | /*if(contat_id==0)*/ { |
||
| 499 | char str1[15]; |
||
| 500 | mutex_lock(&mutex); |
||
| 501 | grx_text(" ", XTI, YTI+174, 0, 0); |
||
| 502 | sprintf(str1, "%d ", contat_id); |
||
| 503 | grx_text(str1, XTI, YTI+174, red, 0); |
||
| 504 | grx_text(" ", XTI, YTI+188, 0, 0); |
||
| 505 | sprintf(str1, "%d ", num_id); |
||
| 506 | grx_text(str1, XTI, YTI+188,green , 0); |
||
| 507 | grx_text(" ", XTI, YTI+202, 0, 0); |
||
| 508 | sprintf(str1, "%f ", somma[num_id-1][0]); |
||
| 509 | grx_text(str1, XTI, YTI+202,green , 0); |
||
| 510 | |||
| 511 | grx_text(" ", XTI, YTI+216, 0, 0); |
||
| 512 | sprintf(str1, "carr= %d %d %f", PERIOD_CARRELLO, WCET_CARRELLO, ((float) WCET_CARRELLO) / PERIOD_CARRELLO ); |
||
| 513 | grx_text(str1, XTI, YTI+216,green , 0); |
||
| 514 | |||
| 515 | grx_text(" ", XTI, YTI+230, 0, 0); |
||
| 516 | sprintf(str1, "track= %d %d %f", PERIOD_TRACKING, WCET_TRACKING, ((float) WCET_TRACKING) / PERIOD_TRACKING ); |
||
| 517 | grx_text(str1, XTI, YTI+230,green , 0); |
||
| 518 | |||
| 519 | mutex_unlock(&mutex); |
||
| 520 | } |
||
| 521 | |||
| 522 | /* disegna il pendolo */ |
||
| 523 | |||
| 524 | port_receive(pra, &x1, BLOCK); |
||
| 525 | port_receive(prb, &x3, BLOCK); |
||
| 526 | |||
| 527 | /* scrivo posizione del carrello e angolo dell'asta */ |
||
| 528 | mutex_lock(&mutex); |
||
| 529 | |||
| 530 | grx_text(" ", XTI+45, YTI+28, 0, 0); |
||
| 531 | sprintf(str, "%5.2f", x3); |
||
| 532 | grx_text(str, XTI+45, YTI+28, blue, 0); |
||
| 533 | |||
| 534 | grx_text(" ", XTI+50, YTI+42, 0, 0); |
||
| 535 | sprintf(str, "%4.3f", x1 * FCA ); |
||
| 536 | grx_text(str, XTI+50, YTI+42, blue, 0); |
||
| 537 | |||
| 538 | mutex_unlock(&mutex); |
||
| 539 | |||
| 540 | /* calcolo ascissa del punto P del carrello */ |
||
| 541 | xp = XMED + x3 * SGX; |
||
| 542 | |||
| 543 | /* calcolo ascissa e ordinata dell'estremo dell'asta */ |
||
| 544 | xa = xp + LA * sin((double)(x1 * SGA)); |
||
| 545 | ya = yp - LA * cos((double)(x1 * SGA)); |
||
| 546 | |||
| 547 | mutex_lock(&mutex); |
||
| 548 | |||
| 549 | /* cancella e disegna il carrello */ |
||
| 550 | grx_box(oxp-LC,oyp,oxp+LC,oyp+H,0); |
||
| 551 | grx_box(xp-LC,yp,xp+LC,yp+H,blue); |
||
| 552 | |||
| 553 | /* cancella e disegna la ruota sinistra del carrello */ |
||
| 554 | grx_circle(oxp-LC+8,oyp+H+4,RAGGIO+2,0); |
||
| 555 | grx_circle(xp-LC+8,yp+H+4,RAGGIO+2,green); |
||
| 556 | |||
| 557 | /* cancella e disegna la ruota destra del carrello */ |
||
| 558 | grx_circle(oxp+LC-8,oyp+H+4,RAGGIO+2,0); |
||
| 559 | grx_circle(xp+LC-8,yp+H+4,RAGGIO+2,green); |
||
| 560 | |||
| 561 | /* cancella e disegna l'asta */ |
||
| 562 | grx_line(oxp,oyp,oxa,oya,0); |
||
| 563 | grx_line(xp,yp,xa,ya,red); |
||
| 564 | |||
| 565 | /* cancella e disegna la pallina sull'asta */ |
||
| 566 | grx_circle(oxa,oya,RAGGIO,0); |
||
| 567 | grx_circle(xa,ya,RAGGIO,blue); |
||
| 568 | |||
| 569 | mutex_unlock(&mutex); |
||
| 570 | |||
| 571 | /* aggiornamento variabili */ |
||
| 572 | oxp = xp; |
||
| 573 | oyp = yp; |
||
| 574 | oxa = xa; |
||
| 575 | oya = ya; |
||
| 576 | |||
| 577 | task_endcycle(); |
||
| 578 | |||
| 579 | } |
||
| 580 | |||
| 581 | port_disconnect(pra); |
||
| 582 | port_disconnect(prb); |
||
| 583 | port_disconnect(prc); |
||
| 584 | |||
| 585 | |||
| 586 | } /* FINE TASK DISEGNA */ |
||
| 587 | |||
| 588 | |||
| 589 | TASK query(void) |
||
| 590 | { |
||
| 591 | char c, str[100]; |
||
| 592 | int count = 0; |
||
| 593 | char *pun; |
||
| 594 | struct Data_cab1 data={0,0}; |
||
| 595 | |||
| 596 | while(1) |
||
| 597 | { |
||
| 598 | |||
| 599 | do { |
||
| 600 | c = keyb_getch(BLOCK); |
||
| 601 | |||
| 602 | |||
| 603 | switch (c) { |
||
| 604 | case 'd': xrif += 1; break; |
||
| 605 | case 's': xrif -= 1; break; |
||
| 606 | case 'D': xrif=7; break; |
||
| 607 | case 'S': xrif=-7; break; |
||
| 608 | case '0': xrif =0 ; break; |
||
| 609 | |||
| 610 | |||
| 611 | |||
| 612 | case '1': prm.GUAD[0] -= 0.1 ; break; |
||
| 613 | case '2': prm.GUAD[0] += 0.1; break; |
||
| 614 | case '3': prm.GUAD[1] -= 10 ; break; |
||
| 615 | case '4': prm.GUAD[1] += 10 ; break; |
||
| 616 | case '5': prm.GUAD[2] -= 0.05; break; |
||
| 617 | case '6': prm.GUAD[2] += 0.05; break; |
||
| 618 | case '7': prm.GUAD[3] -= 0.5; break; |
||
| 619 | case '8': prm.GUAD[3] += 0.5; break; |
||
| 620 | |||
| 621 | case 'o': prm.WCUT -= 2. ; break; |
||
| 622 | case 'p': prm.WCUT += 2. ; break; |
||
| 623 | case 'k': prm.WCUT1 -= 2. ; break; |
||
| 624 | case 'l': prm.WCUT1 += 2. ; break; |
||
| 625 | case 'n': prm.NOISE -= 0.1; break; |
||
| 626 | case 'm': prm.NOISE += 0.1; break; |
||
| 627 | |||
| 628 | case '/': prm.OFFSVAL -= 0.1; break; |
||
| 629 | case '*': prm.OFFSVAL += 0.1; break; |
||
| 630 | |||
| 631 | case '[': prm.COST -= 0.02; break; |
||
| 632 | case ']': prm.COST += 0.02; break; |
||
| 633 | |||
| 634 | case 'c': prm.SCAN -= 1; break; |
||
| 635 | case 'v': prm.SCAN += 1; break; |
||
| 636 | |||
| 637 | case 'R': prm = prmbase; break; |
||
| 638 | case 'T': prm = prmmod; break; |
||
| 639 | case 'U': prm = prm2; break; |
||
| 640 | |||
| 641 | case '<': prm.XTRASL -= .05; break; |
||
| 642 | case '>': prm.XTRASL += .05; break; |
||
| 643 | |||
| 644 | case '!': start_id=1; break; |
||
| 645 | case '@': if(num_id>0) num_id--; break; |
||
| 646 | default: break; |
||
| 647 | } |
||
| 648 | |||
| 649 | data.rif=(vmax+vmin)/2. + xrif * (vmax-vmin)/LUNGH; |
||
| 650 | |||
| 651 | /* controllo che il riferim non vada fuori scala massima */ |
||
| 652 | if (data.rif > vmax) data.rif = vmax; |
||
| 653 | if (data.rif < vmin) data.rif = vmin; |
||
| 654 | |||
| 655 | /* controllo l'impulso */ |
||
| 656 | if (count>0) count--; |
||
| 657 | if (data.imp != 0 && count==0) data.imp = 0; |
||
| 658 | |||
| 659 | /* scrivo sul cab1 i riferim ( rif + imp) per il carrello */ |
||
| 660 | pun = cab_reserve(cab1); |
||
| 661 | memcpy(pun, &data, sizeof(struct Data_cab1)); |
||
| 662 | cab_putmes(cab1, pun); |
||
| 663 | |||
| 664 | |||
| 665 | |||
| 666 | /* si visualizza il riferimento del carrello. */ |
||
| 667 | |||
| 668 | if (xrif != orif) { |
||
| 669 | orif = xrif; |
||
| 670 | mutex_lock(&mutex); |
||
| 671 | grx_text(" ", XTI+45, YTI+14, 0, 0); |
||
| 672 | sprintf(str, "%2.1f cm.", xrif); |
||
| 673 | grx_text(str, XTI+55, YTI+14, green, 0); |
||
| 674 | mutex_unlock(&mutex); |
||
| 675 | } |
||
| 676 | |||
| 677 | } while (c != 27); |
||
| 678 | |||
| 679 | sys_end(); |
||
| 680 | |||
| 681 | task_endcycle(); |
||
| 682 | } |
||
| 683 | } |
||
| 684 | |||
| 685 | int printglob; |
||
| 686 | TASK timejet(void *arg) |
||
| 687 | { |
||
| 688 | TIME table[JET_TABLE_DIM]; |
||
| 689 | int k, nistan; |
||
| 690 | PID i; |
||
| 691 | |||
| 692 | int printed; |
||
| 693 | |||
| 694 | while(1) { |
||
| 695 | for (i=2, printed=0; i<NTASKS ; i++) { |
||
| 696 | if ( (nistan=jet_gettable(i,(TIME *) table, -1)) != -1) { |
||
| 697 | for(k=0;k<nistan;k++){ |
||
| 698 | if(index[i] >= NSAMPLES) break; |
||
| 699 | if( i>=NTASKS ) {grx_close();perror("Ntask too high for jet");sys_end();} |
||
| 700 | timetab[ index[i] ][i] = (float) table[k]; |
||
| 701 | index[i]++; |
||
| 702 | } |
||
| 703 | printed++; |
||
| 704 | } |
||
| 705 | } |
||
| 706 | printglob=printed; |
||
| 707 | task_endcycle(); |
||
| 708 | } |
||
| 709 | } |
||
| 710 | |||
| 711 | |||
| 712 | /****************** * MAIN * ******************************/ |
||
| 713 | |||
| 714 | |||
| 715 | PID pc,pc2, pd, pe, pq, pt; |
||
| 716 | PID camera_PID; |
||
| 717 | PID tracking_PID; |
||
| 718 | void main(int argc, char **argv) |
||
| 719 | { |
||
| 720 | |||
| 721 | int modenum; |
||
| 722 | |||
| 723 | |||
| 724 | HARD_TASK_MODEL m; |
||
| 725 | SOFT_TASK_MODEL ms, md, mt; |
||
| 726 | |||
| 727 | SOFT_TASK_MODEL m1, m2; |
||
| 728 | HARD_TASK_MODEL m3; |
||
| 729 | |||
| 730 | |||
| 731 | |||
| 732 | white = rgb16(255,255,255); |
||
| 733 | black = rgb16(0,0,0); |
||
| 734 | red = rgb16(255,0,0); |
||
| 735 | gray = rgb16(128,128,128); |
||
| 736 | green = rgb16(0,255,0); |
||
| 737 | blue = rgb16(0,0,255); |
||
| 738 | |||
| 739 | #ifdef SOFTPROG |
||
| 740 | if(argc!=4) {perror("numero di parametri errato. Rilancia");keyb_getchar();sys_end();} |
||
| 741 | PERIOD_TRACKING=atol(argv[1]); |
||
| 742 | PERIOD_CARRELLO=atol(argv[2]); |
||
| 743 | WCET_TRACKING =(long int) PERIOD_TRACKING *( 1.0 - (atof(argv[3])) - 0.2) ; |
||
| 744 | WCET_CARRELLO =(long int) PERIOD_CARRELLO * ( atof(argv[3])); |
||
| 745 | #else |
||
| 746 | if(argc!=3) {perror("numero di parametri errato. Rilancia");keyb_getchar();sys_end();} |
||
| 747 | PERIOD_TRACKING=atol(argv[1]); |
||
| 748 | PERIOD_CARRELLO=atol(argv[2]); |
||
| 749 | #endif |
||
| 750 | |||
| 751 | |||
| 752 | |||
| 753 | |||
| 754 | sys_atrunlevel((void *) my_end,NULL, RUNLEVEL_BEFORE_EXIT); |
||
| 755 | #ifdef TIMEJET |
||
| 756 | sys_atrunlevel((void *) scritt,NULL, RUNLEVEL_AFTER_EXIT); |
||
| 757 | #endif |
||
| 758 | #ifdef DATAPLANT |
||
| 759 | sys_atrunlevel((void *) scritt1,NULL, RUNLEVEL_AFTER_EXIT); |
||
| 760 | #endif |
||
| 761 | |||
| 762 | sys_atrunlevel((void *) scritt2,NULL, RUNLEVEL_AFTER_EXIT); |
||
| 763 | sys_atrunlevel((void *) scritt3,NULL, RUNLEVEL_AFTER_EXIT); |
||
| 764 | |||
| 765 | modenum = grx_getmode(800, 600, 16); |
||
| 766 | if (modenum == -1) { |
||
| 767 | cprintf("Errore in grx_mode"); |
||
| 768 | sys_end(); |
||
| 769 | exit(2); |
||
| 770 | } |
||
| 771 | if (grx_setmode(modenum) == -1) { |
||
| 772 | cprintf("Non posso andare in modo grafico"); |
||
| 773 | sys_end(); |
||
| 774 | exit(3); |
||
| 775 | } |
||
| 776 | |||
| 777 | init_framegrabber(); |
||
| 778 | |||
| 779 | { char str[50]; |
||
| 780 | |||
| 781 | sprintf(str, "CARRELLO = %d TRACK = %d ", PERIOD_CARRELLO, PERIOD_TRACKING); |
||
| 782 | grx_text(str, 20 , 20 , green, 0); |
||
| 783 | grx_text("Premi un tasto per continuare", 20 , 30 , green, 0); |
||
| 784 | |||
| 785 | keyb_getchar(); |
||
| 786 | grx_text(" ", 20 , 20 , black, 0); |
||
| 787 | grx_text(" ", 20 , 30 , black, 0); |
||
| 788 | } |
||
| 789 | |||
| 790 | initial(); |
||
| 791 | |||
| 792 | app_mutex_init(&mutex); |
||
| 793 | |||
| 794 | scenario_jetcontrol(); |
||
| 795 | |||
| 796 | { |
||
| 797 | KEY_EVT k; |
||
| 798 | k.flag = ALTL_BIT; |
||
| 799 | k.scan = KEY_C; |
||
| 800 | k.ascii = 'c'; |
||
| 801 | keyb_hook(k,zerofun); |
||
| 802 | } |
||
| 803 | |||
| 804 | |||
| 805 | /* Creazione dei CAB */ |
||
| 806 | cab1 = cab_create("cab1", sizeof(struct Data_cab1), 3); |
||
| 807 | cab2 = cab_create("cab2", sizeof(struct Data_cab2), 3); |
||
| 808 | /* Creazione dei task */ |
||
| 809 | |||
| 810 | |||
| 811 | |||
| 812 | |||
| 813 | |||
| 814 | |||
| 815 | #ifdef SOFTPROG |
||
| 816 | soft_task_default_model(m1); |
||
| 817 | soft_task_def_level(m1,1); |
||
| 818 | soft_task_def_met(m1,WCET_TRACKING); |
||
| 819 | soft_task_def_usemath(m1); |
||
| 820 | |||
| 821 | soft_task_def_period(m1,(PERIOD_TRACKING)); |
||
| 822 | soft_task_def_group(m1,1); |
||
| 823 | soft_task_def_ctrl_jet(m1); |
||
| 824 | // soft_task_def_skip_arrivals(m1); |
||
| 825 | tracking_PID = task_create("track", tracking_task, &m1, NULL); |
||
| 826 | if (tracking_PID == -1) { |
||
| 827 | sys_end(); |
||
| 828 | exit(4); |
||
| 829 | } |
||
| 830 | #else |
||
| 831 | hard_task_default_model(m3); |
||
| 832 | //hard_task_def_level(m3,1); |
||
| 833 | hard_task_def_wcet(m3,WCET_TRACKING); |
||
| 834 | hard_task_def_mit(m3,(PERIOD_TRACKING)); |
||
| 835 | hard_task_def_usemath(m3); |
||
| 836 | |||
| 837 | hard_task_def_group(m3,1); |
||
| 838 | hard_task_def_ctrl_jet(m3); |
||
| 839 | tracking_PID = task_create("track", tracking_task, &m3, NULL); |
||
| 840 | if (tracking_PID == -1) { |
||
| 841 | sys_end(); |
||
| 842 | exit(4); |
||
| 843 | } |
||
| 844 | #endif |
||
| 845 | |||
| 846 | |||
| 847 | soft_task_default_model(m2); |
||
| 848 | soft_task_def_level(m2,1); |
||
| 849 | soft_task_def_met(m2,WCET_CAMERA); |
||
| 850 | soft_task_def_usemath(m2); |
||
| 851 | |||
| 852 | soft_task_def_period(m2,PERIOD_CAMERA); |
||
| 853 | soft_task_def_group(m2,1); |
||
| 854 | soft_task_def_ctrl_jet(m2); |
||
| 855 | |||
| 856 | camera_PID = task_create("cam", camera_task, &m2, NULL); |
||
| 857 | if (camera_PID == -1) { |
||
| 858 | sys_end(); |
||
| 859 | exit(4); |
||
| 860 | } |
||
| 861 | |||
| 862 | |||
| 863 | |||
| 864 | |||
| 865 | |||
| 866 | hard_task_default_model(m); |
||
| 867 | hard_task_def_mit(m, PERIOD_CARRELLO ); |
||
| 868 | hard_task_def_wcet(m,WCET_CARRELLO ); |
||
| 869 | hard_task_def_group(m,2); |
||
| 870 | hard_task_def_ctrl_jet(m); |
||
| 871 | hard_task_def_usemath(m); |
||
| 872 | |||
| 873 | soft_task_default_model(ms); |
||
| 874 | soft_task_def_period(ms, PERIOD_CARRELLO ); |
||
| 875 | soft_task_def_met(ms,WCET_CARRELLO ); |
||
| 876 | soft_task_def_group(ms,2); |
||
| 877 | soft_task_def_ctrl_jet(ms); |
||
| 878 | soft_task_def_usemath(ms); |
||
| 879 | |||
| 880 | soft_task_default_model(md); |
||
| 881 | soft_task_def_level(md,1); |
||
| 882 | soft_task_def_period(md,PERIOD_DESIGN); |
||
| 883 | soft_task_def_met(md, WCET_DESIGN); |
||
| 884 | soft_task_def_group(md, 2); |
||
| 885 | soft_task_def_ctrl_jet(md); |
||
| 886 | soft_task_def_usemath(md); |
||
| 887 | |||
| 888 | #ifdef TIMEJET |
||
| 889 | soft_task_default_model(mt); |
||
| 890 | soft_task_def_level(mt,1); |
||
| 891 | soft_task_def_period(mt,PERIOD_TIMEJET); |
||
| 892 | soft_task_def_met(mt, WCET_TIMEJET); |
||
| 893 | soft_task_def_group(mt, 2); |
||
| 894 | soft_task_def_ctrl_jet(mt); |
||
| 895 | soft_task_def_usemath(mt); |
||
| 896 | #endif |
||
| 897 | |||
| 898 | |||
| 899 | |||
| 900 | |||
| 901 | |||
| 902 | #ifndef TIMEJET |
||
| 903 | init_jetcontrol(); |
||
| 904 | #else |
||
| 905 | pt = task_create("timejet",(TASK) timejet,&md,NULL); |
||
| 906 | if (pt == -1) { |
||
| 907 | sys_end(); |
||
| 908 | exit(4); |
||
| 909 | } |
||
| 910 | #endif |
||
| 911 | #ifdef SOFTPROG |
||
| 912 | pc = task_create("carrello",(TASK) carrello, &ms, NULL); |
||
| 913 | if (pc == -1) { |
||
| 914 | sys_end(); |
||
| 915 | exit(4); |
||
| 916 | } |
||
| 917 | #else |
||
| 918 | pc = task_create("carrello",(TASK) carrello, &m, NULL); |
||
| 919 | if (pc == -1) { |
||
| 920 | sys_end(); |
||
| 921 | exit(4); |
||
| 922 | } |
||
| 923 | #endif |
||
| 924 | |||
| 925 | |||
| 926 | |||
| 927 | pe = task_create("eval",(TASK) eval,&md, NULL); |
||
| 928 | if (pe == -1) { |
||
| 929 | sys_end(); |
||
| 930 | exit(4); |
||
| 931 | } |
||
| 932 | |||
| 933 | |||
| 934 | pd = task_create("diseg",(TASK) diseg,&md,NULL); |
||
| 935 | if (pd == -1) { |
||
| 936 | sys_end(); |
||
| 937 | exit(4); |
||
| 938 | } |
||
| 939 | |||
| 940 | pq = task_create("query",(TASK) query,&md,NULL); |
||
| 941 | if (pq == -1) { |
||
| 942 | sys_end(); |
||
| 943 | exit(4); |
||
| 944 | } |
||
| 945 | |||
| 946 | |||
| 947 | |||
| 948 | /* Attivo i task per controllare e disegnare il carrello */ |
||
| 949 | |||
| 950 | #ifdef TIMEJET |
||
| 951 | task_activate(pt); |
||
| 952 | #endif |
||
| 953 | // task_activate(pc2); |
||
| 954 | |||
| 955 | task_activate(pd); |
||
| 956 | task_activate(pc); |
||
| 957 | task_activate(pe); |
||
| 958 | |||
| 959 | task_activate(pq); |
||
| 960 | |||
| 961 | |||
| 962 | group_activate(1); |
||
| 963 | |||
| 964 | |||
| 965 | |||
| 966 | } |
||
| 967 | |||
| 968 | /****************************************************************/ |
||
| 969 | /* initial */ |
||
| 970 | /****************************************************************/ |
||
| 971 | |||
| 972 | void initial(void) |
||
| 973 | { |
||
| 974 | int x, y; |
||
| 975 | |||
| 976 | |||
| 977 | grx_rect(1, 1, 639, 243, blue); /* dialog box pendolo */ |
||
| 978 | da_motor(0.); |
||
| 979 | |||
| 980 | x = 10; |
||
| 981 | y = YTI; |
||
| 982 | grx_text("INIZIALIZZ.", x, y, blue, 0); y += 42; |
||
| 983 | grx_text("Carr a sx e premi un tasto", x, y, green, 0);y += 28; |
||
| 984 | keyb_getchar(); |
||
| 985 | vmin=ad_conv(11); |
||
| 986 | |||
| 987 | grx_text("Carr a dx e premi un tasto",x,y,green,0); y += 28; |
||
| 988 | keyb_getchar(); |
||
| 989 | vmax=ad_conv(11); |
||
| 990 | |||
| 991 | grx_text("Asta a sx e premi un tasto", x, y, green, 0);y += 28; |
||
| 992 | keyb_getchar(); |
||
| 993 | vminth=ad_conv(10); |
||
| 994 | |||
| 995 | grx_text("Asta a dx e premi un tasto", x, y, green, 0);y += 28; |
||
| 996 | keyb_getchar(); |
||
| 997 | vmaxth=ad_conv(10); |
||
| 998 | |||
| 999 | grx_text("Premi 'y' per l' identificazione ", x, y, green, 0);y += 28; |
||
| 1000 | keyb_getchar(); |
||
| 1001 | |||
| 1002 | grx_clear(0); |
||
| 1003 | |||
| 1004 | |||
| 1005 | } |
||
| 1006 | |||
| 1007 | /****************************************************************/ |
||
| 1008 | /* graphframe */ |
||
| 1009 | /****************************************************************/ |
||
| 1010 | void graphframe(void) |
||
| 1011 | { |
||
| 1012 | int x, y; |
||
| 1013 | |||
| 1014 | mutex_lock(&mutex); |
||
| 1015 | grx_rect(XTI-15,245,639,479,blue); /* finestra di stato */ |
||
| 1016 | grx_rect(1, 1, 450 /*639*/, 243, blue); /* dialog box pendolo */ |
||
| 1017 | |||
| 1018 | |||
| 1019 | x = XTI; |
||
| 1020 | y = YTI; |
||
| 1021 | grx_text("PENDOLO", x, y, red, 0); y += 14; |
||
| 1022 | grx_text("Rif.:", x, y, green, 0); y += 14; |
||
| 1023 | |||
| 1024 | grx_text("X =", x, y, green, 0); y += 14; |
||
| 1025 | grx_text("Ang =", x, y, green, 0); y += 14; |
||
| 1026 | |||
| 1027 | |||
| 1028 | grx_text("s sx, d dx", x, y, green, 0); y += 14; |
||
| 1029 | grx_text("S: xrif-> -7 ", x, y, green, 0); y += 14; |
||
| 1030 | grx_text("D: xrif-> +7 ", x, y, green, 0); y += 14; |
||
| 1031 | grx_text("0: xrif-> 0 ", x, y, green, 0); y += 14; |
||
| 1032 | |||
| 1033 | grx_text("S imp-,D imp+", x, y, green, 0); y += 14; |
||
| 1034 | grx_text("CALIBRAZ: < , >", x, y, green, 0); y += 14; |
||
| 1035 | grx_text("Dati Start: !", x, y, green, 0); y += 14; |
||
| 1036 | grx_text("Cancel Last: @", x, y, green, 0); y += 14; |
||
| 1037 | |||
| 1038 | mutex_unlock(&mutex); |
||
| 1039 | } |
||
| 1040 | |||
| 1041 | /*--------------------------------------------------------------*/ |
||
| 1042 | /* This function is called at system termination */ |
||
| 1043 | /*--------------------------------------------------------------*/ |
||
| 1044 | void my_end(void) |
||
| 1045 | { |
||
| 1046 | da_motor(0.0); |
||
| 1047 | grx_close(); |
||
| 1048 | |||
| 1049 | // sys_status(CLOCK_STATUS|SCHED_STATUS); |
||
| 1050 | |||
| 1051 | sys_end(); |
||
| 1052 | } |
||
| 1053 | /*--------------------------------------------------------------*/ |
||
| 1054 | /* This function writes the data in the file DATA */ |
||
| 1055 | /*--------------------------------------------------------------*/ |
||
| 1056 | |||
| 1057 | |||
| 1058 | void scritt(void) |
||
| 1059 | { |
||
| 1060 | DOS_FILE *file; |
||
| 1061 | long int i , k; |
||
| 1062 | char str[500]; |
||
| 1063 | |||
| 1064 | file = DOS_fopen("data","w"); |
||
| 1065 | |||
| 1066 | #if 1 |
||
| 1067 | for(i=2;i<NTASKS;i++) { |
||
| 1068 | sprintf(str,"%s ",proc_table[i].name); |
||
| 1069 | cprintf("%s", str); |
||
| 1070 | DOS_fwrite(str,1,strlen(str) ,file); |
||
| 1071 | } |
||
| 1072 | |||
| 1073 | DOS_fwrite("\n",1,1,file); |
||
| 1074 | cprintf("%s", "\n"); |
||
| 1075 | #endif |
||
| 1076 | |||
| 1077 | for(k=0;k<NSAMPLES;k++){ |
||
| 1078 | |||
| 1079 | for(i=2;i<NTASKS;i++) { |
||
| 1080 | sprintf(str,"%f ",timetab[k][i]); |
||
| 1081 | cprintf("%s", str); |
||
| 1082 | // keyb_getchar(); |
||
| 1083 | DOS_fwrite(str,1,strlen(str) ,file); |
||
| 1084 | } |
||
| 1085 | |||
| 1086 | DOS_fwrite("\n",1,1,file); |
||
| 1087 | cprintf("%s", "\n"); |
||
| 1088 | |||
| 1089 | } |
||
| 1090 | DOS_fclose(file); |
||
| 1091 | } |
||
| 1092 | |||
| 1093 | #ifdef DATAPLANT |
||
| 1094 | void scritt1(void) |
||
| 1095 | { |
||
| 1096 | DOS_FILE *file; |
||
| 1097 | char str[300]; |
||
| 1098 | long int i; |
||
| 1099 | float pippy=3.14; |
||
| 1100 | |||
| 1101 | file = DOS_fopen("dataplant","w"); |
||
| 1102 | for(i=0;i<numdat;i++) { |
||
| 1103 | sprintf(str,"%lu %f %f %f %f %f %f \n",dati[i].time , dati[i].xist,dati[i].thist,dati[i].par1, dati[i].par2, dati[i].par3, dati[i].par4 ); |
||
| 1104 | cprintf("%lu %f %f %f %f %f %f \n",dati[i].time , dati[i].xist,dati[i].thist,dati[i].par1, dati[i].par2, dati[i].par3, dati[i].par4 ); |
||
| 1105 | if(DOS_fwrite(str,1,strlen(str),file)==0) perror("Errore in write"); |
||
| 1106 | } |
||
| 1107 | DOS_fclose(file); |
||
| 1108 | } |
||
| 1109 | #endif |
||
| 1110 | |||
| 1111 | void scritt2(void) |
||
| 1112 | { |
||
| 1113 | DOS_FILE *file; |
||
| 1114 | char tmp[15]={0}, str[250]; |
||
| 1115 | int i; |
||
| 1116 | float parz=0.0 , average=0.0, sumsq=0.0, confid_int=0.0; |
||
| 1117 | float t_stud[20]={0.0, 6.314,2.920,2.353,2.132,2.015,1.943,1.895,1.860,\ |
||
| 1118 | 1.833,1.812,1.796,1.782,1.771,1.761,1.753,1.746,1.740,1.734,1.729}; |
||
| 1119 | |||
| 1120 | file = DOS_fopen("integrale","wa"); |
||
| 1121 | for(i=0;i<num_id;i++) { |
||
| 1122 | sprintf(tmp,"%f %f \n",somma[i][0],somma[i][1]); |
||
| 1123 | strcat(str,tmp); |
||
| 1124 | } |
||
| 1125 | |||
| 1126 | for(i=0;i<num_id;i++) parz += somma[i][0]; |
||
| 1127 | average=parz /num_id; |
||
| 1128 | for(i=0;i<num_id;i++) sumsq +=(average-somma[i][0])*(average-somma[i][0]); |
||
| 1129 | confid_int= t_stud[num_id-1]*sqrt((double) sumsq/((float) num_id*(num_id-1))); |
||
| 1130 | sprintf(tmp,"n.run=%d aver=%f confid_int=%f \n",num_id,average,confid_int); |
||
| 1131 | strcat(str,tmp); |
||
| 1132 | if(DOS_fwrite(str,1,strlen(str),file)==0) perror("Errore in write"); |
||
| 1133 | DOS_fclose(file); |
||
| 1134 | |||
| 1135 | file = DOS_fopen("intdata","wa"); |
||
| 1136 | |||
| 1137 | sprintf(str,"%d %d %f %f %d %d %d \n",PERIOD_CARRELLO, PERIOD_TRACKING,average,confid_int, num_id,WCET_CARRELLO, WCET_TRACKING ); |
||
| 1138 | // strcat(str,tmp); |
||
| 1139 | if(DOS_fwrite(str,1,strlen(str),file)==0) perror("Errore in write"); |
||
| 1140 | |||
| 1141 | DOS_fclose(file); |
||
| 1142 | } |
||
| 1143 | |||
| 1144 | |||
| 1145 | |||
| 1146 | void nuovi_tempi(long int * contatwait) |
||
| 1147 | { |
||
| 1148 | |||
| 1149 | int i; |
||
| 1150 | float parz=0.0 , average=0.0, sumsq=0.0, confid_int=0.0; |
||
| 1151 | float t_stud[20]={0.0, 6.314,2.920,2.353,2.132,2.015,1.943,1.895,1.860,\ |
||
| 1152 | 1.833,1.812,1.796,1.782,1.771,1.761,1.753,1.746,1.740,1.734,1.729}; |
||
| 1153 | |||
| 1154 | static int num_tempi=0; |
||
| 1155 | // calcolo la media e lo interv di confidenza |
||
| 1156 | for(i=0;i<num_id;i++) parz += somma[i][0]; |
||
| 1157 | average=parz /num_id; |
||
| 1158 | for(i=0;i<num_id;i++) sumsq +=(average-somma[i][0])*(average-somma[i][0]); |
||
| 1159 | confid_int= t_stud[num_id-1]*sqrt((double) sumsq/((float) num_id*(num_id-1))); |
||
| 1160 | |||
| 1161 | |||
| 1162 | // scrivo i dati relativi alrun in una matrice globale |
||
| 1163 | sprintf(stringa[num_tempi],"%d %d %f %f %d %d %d \n",PERIOD_CARRELLO, PERIOD_TRACKING,average,confid_int, num_id,WCET_CARRELLO, WCET_TRACKING ); |
||
| 1164 | num_tempi++; |
||
| 1165 | if(num_tempi >= MAX_TEMPI) sys_end(); |
||
| 1166 | |||
| 1167 | |||
| 1168 | // cambio i periodi-banda dei tasks |
||
| 1169 | { |
||
| 1170 | double band; |
||
| 1171 | band=((float) WCET_CARRELLO)/PERIOD_CARRELLO; |
||
| 1172 | |||
| 1173 | PERIOD_CARRELLO = PERIOD_CARRELLO; // + 500; |
||
| 1174 | PERIOD_TRACKING = PERIOD_TRACKING; // +2000; |
||
| 1175 | band+=0.1; |
||
| 1176 | WCET_CARRELLO = band * PERIOD_CARRELLO; |
||
| 1177 | WCET_TRACKING = ( 0.8 - band) * PERIOD_TRACKING; |
||
| 1178 | |||
| 1179 | } |
||
| 1180 | |||
| 1181 | CBS_modify_bandwidth(pc,(TIME) PERIOD_CARRELLO,(int) WCET_CARRELLO); |
||
| 1182 | CBS_modify_bandwidth(tracking_PID,(TIME) PERIOD_TRACKING,(int) WCET_TRACKING); |
||
| 1183 | |||
| 1184 | // resetto le variabili per il run successivo |
||
| 1185 | num_id=0; |
||
| 1186 | *contatwait=(long int) (1.5 * SEC_ID / DEADSECTH(PERIOD_CARRELLO)); |
||
| 1187 | } |
||
| 1188 | |||
| 1189 | void scritt3(void) |
||
| 1190 | { |
||
| 1191 | DOS_FILE *file; |
||
| 1192 | char tmp[1000]={0}; |
||
| 1193 | int i; |
||
| 1194 | |||
| 1195 | file = DOS_fopen("totaldata","wa"); |
||
| 1196 | |||
| 1197 | for(i=0;i<MAX_TEMPI;i++) { |
||
| 1198 | |||
| 1199 | if(DOS_fwrite(stringa[i],1,strlen(stringa[i]),file)==0) perror("Errore in write"); |
||
| 1200 | |||
| 1201 | |||
| 1202 | // strcat(tmp, stringa[i][0]); |
||
| 1203 | } |
||
| 1204 | // if(DOS_fwrite(tmp,1,strlen(tmp),file)==0) perror("Errore in write"); |
||
| 1205 | DOS_fclose(file); |
||
| 1206 | } |
||
| 1207 |