Rev 1117 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1085 | pj | 1 | /* |
| 2 | * Project: S.Ha.R.K. |
||
| 3 | * |
||
| 4 | * Coordinators: |
||
| 5 | * Giorgio Buttazzo <giorgio@sssup.it> |
||
| 6 | * Paolo Gai <pj@gandalf.sssup.it> |
||
| 7 | * |
||
| 8 | * Authors : |
||
| 9 | * Paolo Gai <pj@gandalf.sssup.it> |
||
| 10 | * (see the web pages for full authors list) |
||
| 11 | * |
||
| 12 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
| 13 | * |
||
| 14 | * http://www.sssup.it |
||
| 15 | * http://retis.sssup.it |
||
| 16 | * http://shark.sssup.it |
||
| 17 | */ |
||
| 18 | |||
| 19 | /** |
||
| 20 | ------------ |
||
| 1123 | pj | 21 | CVS : $Id: eli.c,v 1.3 2003-01-07 17:10:16 pj Exp $ |
| 1085 | pj | 22 | |
| 23 | File: $File$ |
||
| 1123 | pj | 24 | Revision: $Revision: 1.3 $ |
| 25 | Last update: $Date: 2003-01-07 17:10:16 $ |
||
| 1085 | pj | 26 | ------------ |
| 27 | |||
| 28 | This file is similar to the configuration of Hartik 3.3.1 |
||
| 29 | |||
| 30 | **/ |
||
| 31 | |||
| 32 | /* |
||
| 33 | * Copyright (C) 2000 Paolo Gai and ALLEN-DESTRO |
||
| 34 | * |
||
| 35 | * This program is free software; you can redistribute it and/or modify |
||
| 36 | * it under the terms of the GNU General Public License as published by |
||
| 37 | * the Free Software Foundation; either version 2 of the License, or |
||
| 38 | * (at your option) any later version. |
||
| 39 | * |
||
| 40 | * This program is distributed in the hope that it will be useful, |
||
| 41 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
| 42 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
| 43 | * GNU General Public License for more details. |
||
| 44 | * |
||
| 45 | * You should have received a copy of the GNU General Public License |
||
| 46 | * along with this program; if not, write to the Free Software |
||
| 47 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
| 48 | * |
||
| 49 | */ |
||
| 50 | |||
| 51 | #include "eli.h" |
||
| 52 | |||
| 1117 | pj | 53 | #include <time.h> |
| 1085 | pj | 54 | |
| 55 | //######################################## |
||
| 56 | // Definizione variabili globali |
||
| 57 | //######################################## |
||
| 58 | char tastiera; |
||
| 59 | long double Forza_x,Forza_y,Velocita_x,Velocita_y,Acc_x,Acc_y,Pos_x,Pos_y; |
||
| 60 | long double Pos_pe_y[5]; |
||
| 61 | long double Pos_pe_x[5]; |
||
| 62 | long double T1=0; |
||
| 63 | long double r; |
||
| 64 | int n_peso=0; |
||
| 65 | int peso_agganciato[5]={0,0,0,0,0}; |
||
| 66 | int pre_peso[5]={0,0,0,0,0}; |
||
| 67 | int libero_peso[5]={0,0,0,0,0}; |
||
| 68 | PID pid_peso[5]; |
||
| 69 | PID pid_ruspa[5]; |
||
| 70 | int x_r=200; |
||
| 71 | int n_ruspa=5; |
||
| 72 | int indietro; |
||
| 73 | int eli_occ=0; |
||
| 74 | int kill_p; |
||
| 75 | |||
| 76 | //PID pid_r0,pid_r1,pid_r2,pid_r3,pid_r4; |
||
| 77 | PID pid_kill_p,pid_pulisci_p,pid_kill_e,pid_pulisci_e; |
||
| 78 | |||
| 79 | |||
| 80 | // Definizione del semaforo per l'utilizzo della modalita' grafica |
||
| 81 | sem_t mutex; |
||
| 82 | sem_t pu; |
||
| 83 | |||
| 84 | |||
| 85 | |||
| 86 | |||
| 87 | //####################################################################### |
||
| 88 | //############### DEFINIZIONE TASK ###################################### |
||
| 89 | //####################################################################### |
||
| 90 | |||
| 91 | TASK kill_task(int i) |
||
| 92 | { int x,y,dim; |
||
| 93 | while (1) |
||
| 94 | { |
||
| 95 | |||
| 96 | if(i==1) |
||
| 97 | { |
||
| 98 | x=Pos_pe_x[kill_p]; |
||
| 99 | y=Pos_pe_y[kill_p]; |
||
| 100 | dim=25; |
||
| 101 | } |
||
| 102 | else |
||
| 103 | { |
||
| 104 | x=Pos_x; |
||
| 105 | y=Pos_y; |
||
| 106 | dim=35; |
||
| 107 | } |
||
| 108 | |||
| 109 | PULISCI(x,y,dim); |
||
| 110 | ESPLOSIONE(x,y); |
||
| 111 | |||
| 112 | sem_wait(&pu); |
||
| 113 | |||
| 114 | if(i==1) task_activate(pid_pulisci_p); |
||
| 115 | else task_activate(pid_pulisci_e); |
||
| 116 | |||
| 117 | task_endcycle(); |
||
| 118 | |||
| 119 | } |
||
| 120 | |||
| 121 | |||
| 122 | } |
||
| 123 | |||
| 124 | TASK pulisci(int i) |
||
| 1117 | pj | 125 | { |
| 126 | int x,y,dim; |
||
| 127 | struct timespec delay; |
||
| 128 | |||
| 129 | delay.tv_sec=0; |
||
| 130 | delay.tv_nsec=3000000; |
||
| 131 | |||
| 132 | while (1) |
||
| 1085 | pj | 133 | { |
| 134 | |||
| 135 | if(i==1) |
||
| 136 | { |
||
| 137 | x=Pos_pe_x[kill_p]; |
||
| 138 | y=Pos_pe_y[kill_p]; |
||
| 139 | dim=25; |
||
| 140 | } |
||
| 141 | else |
||
| 142 | { |
||
| 143 | x=Pos_x; |
||
| 144 | y=Pos_y; |
||
| 145 | dim=35; |
||
| 146 | } |
||
| 147 | |||
| 148 | |||
| 1117 | pj | 149 | nanosleep(&delay, NULL); |
| 1085 | pj | 150 | PULISCI(x,y,dim); |
| 151 | |||
| 152 | sem_post(&pu); |
||
| 153 | |||
| 154 | task_endcycle(); |
||
| 155 | } |
||
| 156 | } |
||
| 157 | |||
| 158 | |||
| 159 | |||
| 160 | |||
| 161 | TASK elicottero(int i) |
||
| 162 | { long double x_old,x_new,y_old,y_new; |
||
| 163 | TIME us_old,us_new; |
||
| 164 | long double delta_T; |
||
| 165 | long double Fx,Fy,Ax,Ay,Vx,Vy,m_el; |
||
| 166 | int Destra_old,Destra_new; |
||
| 167 | // int uccidi,colore; |
||
| 168 | // int prova; |
||
| 169 | |||
| 170 | m_el=1; |
||
| 171 | Vx=0;Vy=0; |
||
| 172 | Ax=0;Ay=0; |
||
| 173 | Fx=0;Fy=0-m_el*G; // Fy=0; |
||
| 174 | x_old=150; |
||
| 175 | y_old=150; |
||
| 176 | Destra_old=1; |
||
| 177 | Destra_new=1; |
||
| 178 | |||
| 179 | us_old=sys_gettime(NULL); |
||
| 180 | |||
| 181 | while (1) |
||
| 182 | { |
||
| 183 | |||
| 184 | // prova=2; |
||
| 185 | |||
| 186 | |||
| 187 | us_new=sys_gettime(NULL); |
||
| 188 | delta_T=(us_new-us_old)/100000; |
||
| 189 | // if(delta_T<0.9) prova=1; |
||
| 190 | // if(delta_T<0) prova=4; |
||
| 191 | // if(delta_T>1.1) prova=3; |
||
| 192 | |||
| 193 | |||
| 194 | delta_T=1; |
||
| 195 | Ax=(Fx-(r*Vx))/m_el; |
||
| 196 | Ay=(Fy-(r*Vy)+m_el*G+T1)/m_el; |
||
| 197 | x_new=x_old+Vx*delta_T+0.5*Ax*delta_T*delta_T; |
||
| 198 | y_new=y_old+Vy*delta_T+0.5*Ay*delta_T*delta_T; |
||
| 199 | |||
| 200 | // if(prova==1) {x_new=300; y_new=150;} |
||
| 201 | // if(prova==3) {x_new=150; y_new=300;} |
||
| 202 | // if(prova==2) {x_new=300; y_new=300;} |
||
| 203 | // if(prova==4) {x_new=400; y_new=400;} |
||
| 204 | |||
| 205 | Vx=Vx+Ax*delta_T; |
||
| 206 | Vy=Vy+Ay*delta_T; |
||
| 207 | if ((Vx>0)|| ((Vx==0) && (Destra_old==1))) Destra_new=1; |
||
| 208 | else Destra_new=0; |
||
| 209 | |||
| 210 | if ( ((y_new>=(Y0max-DIM_EL/2-2)) || ( (y_old<=400-DIM_EL/2-2) && (y_new>=400-DIM_EL/2-2) && (x_new>=X0min+151) && (x_new<=X0min+280) )) ) |
||
| 211 | { |
||
| 212 | if ((x_new>=X0min+151) && (x_new<=X0min+280)) y_new=400-DIM_EL/2-2; |
||
| 213 | else y_new=Y0max-DIM_EL/2-2; |
||
| 214 | Vy=0; |
||
| 215 | Vx=0; |
||
| 216 | x_new=x_old; |
||
| 217 | Ay=0; |
||
| 218 | Ax=0; |
||
| 219 | } |
||
| 220 | |||
| 221 | |||
| 222 | |||
| 223 | |||
| 224 | sem_wait(&mutex); |
||
| 225 | |||
| 226 | /* |
||
| 227 | uccidi=0; |
||
| 228 | |||
| 229 | colore=grx_getpixel(x_new-2*DIM_EL,y_new-DIM_EL); |
||
| 230 | if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1; |
||
| 231 | colore=grx_getpixel(x_new+2*DIM_EL,y_new-DIM_EL); |
||
| 232 | if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1; |
||
| 233 | colore=grx_getpixel(x_new-2*DIM_EL,y_new+DIM_EL/2); |
||
| 234 | if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1; |
||
| 235 | colore=grx_getpixel(x_new+2*DIM_EL,y_new+DIM_EL/2); |
||
| 236 | if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1; |
||
| 237 | colore=grx_getpixel(x_new,y_new); |
||
| 238 | if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1; |
||
| 239 | */ |
||
| 240 | |||
| 241 | if ((x_old>100) && (y_old>100) && (x_old<X0max) ) |
||
| 242 | { |
||
| 243 | if (Destra_old==0) {ELICOTTERO_S(x_old,y_old,DIM_EL,COL_SFONDO,COL_SFONDO);} |
||
| 244 | else {ELICOTTERO_D(x_old,y_old,DIM_EL,COL_SFONDO,COL_SFONDO);} |
||
| 245 | } |
||
| 246 | |||
| 247 | if ((x_new>100) && (y_new>100) && (x_new<X0max) ) |
||
| 248 | { |
||
| 249 | /* |
||
| 250 | if ( uccidi==1) |
||
| 251 | { |
||
| 252 | // grx_box(10,10,100,100,RED); |
||
| 253 | // task_activate(pid_kill_e); |
||
| 254 | // sem_post(&mutex); |
||
| 255 | // eli_occ=0; |
||
| 256 | sem_post(&mutex); |
||
| 257 | task_abort(); |
||
| 258 | } |
||
| 259 | */ |
||
| 260 | |||
| 261 | |||
| 262 | if (Destra_new==0) {ELICOTTERO_S(x_new,y_new,DIM_EL,COL_EL,COL_SFONDO);} |
||
| 263 | else {ELICOTTERO_D(x_new,y_new,DIM_EL,COL_EL,COL_SFONDO);} |
||
| 264 | } |
||
| 265 | sem_post(&mutex); |
||
| 266 | |||
| 267 | if (tastiera=='c') Fx=Fx+0.25; |
||
| 268 | if (tastiera=='z') Fx=Fx-0.25; |
||
| 269 | if (tastiera=='x') Fy=Fy+0.25; |
||
| 270 | if (tastiera=='s') Fy=Fy-0.25; |
||
| 271 | |||
| 272 | if (Fx>2) Fx=2; |
||
| 273 | if (Fx<-2) Fx=-2; |
||
| 274 | if (Fy>0) Fy=0; |
||
| 275 | if (Fy<-17.5) Fy=-17.5; |
||
| 276 | |||
| 277 | Forza_x=Fx; |
||
| 278 | Forza_y=Fy; |
||
| 279 | Acc_x=Ax; |
||
| 280 | Acc_y=Ay; |
||
| 281 | Velocita_x=Vx; |
||
| 282 | Velocita_y=Vy; |
||
| 283 | Pos_x=x_new; |
||
| 284 | Pos_y=y_new; |
||
| 285 | |||
| 286 | |||
| 287 | if ( (tastiera=='z') || (tastiera=='x') || (tastiera=='c') || (tastiera=='s') ) |
||
| 288 | tastiera='q'; |
||
| 289 | |||
| 290 | |||
| 291 | us_old=us_new; |
||
| 292 | x_old=x_new; |
||
| 293 | y_old=y_new; |
||
| 294 | Destra_old=Destra_new; |
||
| 295 | |||
| 296 | task_endcycle(); |
||
| 297 | } |
||
| 298 | } |
||
| 299 | |||
| 300 | |||
| 301 | |||
| 302 | TASK peso(int i) |
||
| 303 | { |
||
| 304 | long double delta_T; |
||
| 305 | long double Ay,Vy; |
||
| 306 | long double x_new,x_old,y_old,y_new; |
||
| 307 | int scelta=0; |
||
| 308 | int rit,colore,uccidi; |
||
| 309 | double m_peso[5]={0.5,0.4,0.3,0.1,0.6}; |
||
| 310 | int delta_y1,delta_y2; |
||
| 311 | |||
| 312 | |||
| 313 | // PID pid; |
||
| 314 | // MODEL m = BASE_MODEL; |
||
| 315 | |||
| 316 | Vy=0; |
||
| 317 | Ay=0; |
||
| 318 | |||
| 319 | x_old=700-DIM_PESO-1; |
||
| 320 | y_old=Y0max-DIM_PESO*2-1; |
||
| 321 | delta_T=1; |
||
| 322 | x_new=700-DIM_PESO-1; |
||
| 323 | |||
| 324 | libero_peso[i]=0; |
||
| 325 | |||
| 326 | while (1 ) |
||
| 327 | { |
||
| 328 | uccidi=0; |
||
| 329 | |||
| 330 | if (scelta==0 ) |
||
| 331 | { |
||
| 332 | |||
| 333 | y_new=y_old; |
||
| 334 | if (x_new+DIM_PESO==x_r-1) x_new=x_new-1; |
||
| 335 | else x_new=x_new; |
||
| 336 | if (indietro==1) {scelta=1; libero_peso[i]=1;} |
||
| 337 | } |
||
| 338 | |||
| 339 | if (scelta==1 ) |
||
| 340 | { |
||
| 341 | y_new=y_old; |
||
| 342 | x_new=x_old; |
||
| 343 | if (peso_agganciato[i]==1) scelta=2; |
||
| 344 | } |
||
| 345 | if (scelta==2) |
||
| 346 | { |
||
| 347 | eli_occ=1; |
||
| 348 | Ay=(m_peso[i]*G-(r*Vy)-T1)/m_peso[i]; |
||
| 349 | if(peso_agganciato[i]==1) x_new=Pos_x; |
||
| 350 | else x_new=x_old; |
||
| 351 | y_new=y_old+Vy*delta_T+0.5*Ay*delta_T*delta_T; |
||
| 352 | Vy=Vy+Ay*delta_T; |
||
| 353 | |||
| 354 | delta_y1=Y0max-y_new; |
||
| 355 | delta_y2=400-y_new; |
||
| 356 | |||
| 357 | if ( (y_new>=(Y0max-2*DIM_PESO-1)) || ( (y_old<400-2*DIM_PESO-1) && (y_new>=400-2*DIM_PESO-1) && (x_new>=X0min+151-DIM_PESO) && (x_new<=X0min+280+DIM_PESO) ) ) |
||
| 358 | { |
||
| 359 | if ((x_new>=X0min+151-DIM_PESO) && (x_new<=X0min+280+DIM_PESO)) |
||
| 360 | { |
||
| 361 | if (delta_y2<6) uccidi=1; |
||
| 362 | y_new=400-2*DIM_PESO-2; |
||
| 363 | } |
||
| 364 | else { |
||
| 365 | if (delta_y1<6) uccidi=1; |
||
| 366 | y_new=Y0max-2*DIM_PESO-2; |
||
| 367 | } |
||
| 368 | Vy=0; |
||
| 369 | if(peso_agganciato[i]==0) {scelta=3; rit=0;} |
||
| 370 | } |
||
| 371 | } |
||
| 372 | if (scelta==3) |
||
| 373 | { |
||
| 374 | x_new=x_new; |
||
| 375 | y_new=y_new; |
||
| 376 | eli_occ=0; |
||
| 377 | if (y_new==400-2*DIM_PESO-2) rit++; |
||
| 378 | |||
| 379 | |||
| 380 | |||
| 381 | if (rit==100) { |
||
| 382 | peso_agganciato[i]=0; |
||
| 383 | pre_peso[i]=0; |
||
| 384 | sem_wait(&mutex); |
||
| 385 | PESO(x_old,y_old,DIM_PESO,COL_SFONDO); |
||
| 386 | sem_post(&mutex); |
||
| 387 | |||
| 388 | return NULL; |
||
| 389 | } |
||
| 390 | |||
| 391 | if(peso_agganciato[i]==1) scelta=2; |
||
| 392 | } |
||
| 393 | |||
| 394 | sem_wait(&mutex); |
||
| 395 | |||
| 396 | |||
| 397 | colore=grx_getpixel(x_new-DIM_PESO,y_new); |
||
| 398 | if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1; |
||
| 399 | |||
| 400 | colore=grx_getpixel(x_new+DIM_PESO,y_new); |
||
| 401 | if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1; |
||
| 402 | |||
| 403 | colore=grx_getpixel(x_new-DIM_PESO,y_new+2*DIM_PESO); |
||
| 404 | if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1; |
||
| 405 | |||
| 406 | colore=grx_getpixel(x_new+DIM_PESO,y_new+2*DIM_PESO); |
||
| 407 | if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1; |
||
| 408 | |||
| 409 | colore=grx_getpixel(x_new,y_new+DIM_PESO); |
||
| 410 | if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1; |
||
| 411 | |||
| 412 | |||
| 413 | |||
| 414 | if ((x_old>100) && (y_old>100) && (x_old<X0max) ) |
||
| 415 | { |
||
| 416 | PESO(x_old,y_old,DIM_PESO,COL_SFONDO); |
||
| 417 | } |
||
| 418 | |||
| 419 | y_old=y_new; |
||
| 420 | x_old=x_new; |
||
| 421 | Pos_pe_y[i]=y_new; |
||
| 422 | Pos_pe_x[i]=x_new; |
||
| 423 | |||
| 424 | |||
| 425 | if ((x_new>100) && (y_new>100) && (x_new<X0max) ) |
||
| 426 | { |
||
| 427 | if ( uccidi==1) |
||
| 428 | { |
||
| 429 | kill_p=i; |
||
| 430 | task_activate(pid_kill_p); |
||
| 431 | sem_post(&mutex); |
||
| 432 | eli_occ=0; |
||
| 433 | pre_peso[i]=0; |
||
| 434 | peso_agganciato[i]=0; |
||
| 435 | return NULL; |
||
| 436 | } |
||
| 437 | |||
| 438 | PESO(x_new,y_new,DIM_PESO,DELTA_COL+i*2); |
||
| 439 | } |
||
| 440 | sem_post(&mutex); |
||
| 441 | |||
| 442 | |||
| 443 | task_endcycle(); |
||
| 444 | } |
||
| 445 | |||
| 446 | |||
| 447 | } |
||
| 448 | |||
| 449 | TASK ruspa(int i) |
||
| 450 | { |
||
| 451 | int x,y,ii; |
||
| 452 | int end_peso[5]={370,420,470,520,570}; |
||
| 453 | |||
| 454 | PID pid; |
||
| 455 | HARD_TASK_MODEL m_hard; |
||
| 456 | |||
| 457 | pre_peso[i]=1; |
||
| 458 | |||
| 459 | hard_task_default_model(m_hard); |
||
| 460 | hard_task_def_wcet(m_hard,1); // wcet ignored!!! |
||
| 461 | hard_task_def_mit(m_hard,PER_DISEGNA); |
||
| 462 | hard_task_def_usemath(m_hard); |
||
| 463 | hard_task_def_arg(m_hard,(void *)i); |
||
| 464 | pid=task_create("peso2",peso,&m_hard,NULL); |
||
| 465 | pid_peso[i]=pid; |
||
| 466 | task_activate(pid_peso[i]); |
||
| 467 | |||
| 468 | |||
| 469 | x=720; |
||
| 470 | y=Y0max; |
||
| 471 | |||
| 472 | indietro=0; |
||
| 473 | while (x!=end_peso[i]+DIM_PESO) |
||
| 474 | { |
||
| 475 | sem_wait(&mutex); |
||
| 476 | RUSPA_S(x+1,y,COL_SFONDO,COL_SFONDO); |
||
| 477 | RUSPA_S(x,y,YELLOW-1,LIGHTBLUE); |
||
| 478 | sem_post(&mutex); |
||
| 479 | x_r=x; |
||
| 480 | x--; |
||
| 481 | task_endcycle(); |
||
| 482 | } |
||
| 483 | sem_wait(&mutex); |
||
| 484 | RUSPA_S(x+1,y,COL_SFONDO,COL_SFONDO); |
||
| 485 | sem_post(&mutex); |
||
| 486 | indietro=1; |
||
| 487 | while (x!=730 ) |
||
| 488 | { |
||
| 489 | sem_wait(&mutex); |
||
| 490 | RUSPA_D(x-1+40,y,COL_SFONDO,COL_SFONDO); |
||
| 491 | RUSPA_D(x+40,y,YELLOW-1,LIGHTBLUE); |
||
| 492 | sem_post(&mutex); |
||
| 493 | x++; |
||
| 494 | task_endcycle(); |
||
| 495 | } |
||
| 496 | |||
| 497 | ii=i+1; |
||
| 498 | |||
| 499 | while(ii>4) |
||
| 500 | { |
||
| 501 | n_ruspa=5; |
||
| 502 | if(pre_peso[4]==0){ |
||
| 503 | ii=4; |
||
| 504 | if(pre_peso[3]==0){ |
||
| 505 | ii=3; |
||
| 506 | if(pre_peso[2]==0){ |
||
| 507 | ii=2; |
||
| 508 | if(pre_peso[1]==0){ |
||
| 509 | ii=1; |
||
| 510 | if(pre_peso[0]==0){ |
||
| 511 | ii=0; |
||
| 512 | } |
||
| 513 | } |
||
| 514 | } |
||
| 515 | } |
||
| 516 | } |
||
| 517 | task_endcycle(); |
||
| 518 | |||
| 519 | } |
||
| 520 | |||
| 521 | hard_task_def_arg(m_hard,(void *)ii); |
||
| 522 | pid_ruspa[ii]=task_create("ruspa",ruspa,&m_hard,NULL); |
||
| 523 | task_activate(pid_ruspa[ii]); |
||
| 524 | n_ruspa=ii; |
||
| 525 | |||
| 526 | return NULL; |
||
| 527 | } |
||
| 528 | |||
| 529 | |||
| 530 | |||
| 531 | |||
| 532 | TASK molla(int i) |
||
| 533 | { |
||
| 534 | double k=0.05; |
||
| 535 | double x,x_old,y1_old,y2_old; |
||
| 536 | double T1_old; //,T2_old; |
||
| 537 | |||
| 538 | x_old=Pos_x; |
||
| 539 | y1_old=Pos_y; |
||
| 540 | y2_old=Pos_pe_y[n_peso]; |
||
| 541 | |||
| 542 | while (1) |
||
| 543 | { |
||
| 544 | |||
| 545 | sem_wait(&mutex); |
||
| 546 | if ((x_old>100-DIM_EL) && (y1_old>100-DIM_EL) && (x_old<X0max) ) |
||
| 547 | { |
||
| 548 | MOLLA(x_old,y1_old+DIM_EL,y2_old,COL_SFONDO) |
||
| 549 | } |
||
| 550 | else if( (y2_old>100) && (x_old<X0max) && (x_old>100) ) |
||
| 551 | { |
||
| 552 | MOLLA(x_old,X0min+100,y2_old,COL_SFONDO) |
||
| 553 | } |
||
| 554 | |||
| 555 | x_old=Pos_x; |
||
| 556 | y1_old=Pos_y; |
||
| 557 | |||
| 558 | if(peso_agganciato[n_peso]==0) y2_old=y1_old+20; |
||
| 559 | else y2_old=Pos_pe_y[n_peso]; |
||
| 560 | |||
| 561 | if ((x_old>100) && (y1_old>100-DIM_EL) && (x_old<X0max) ) |
||
| 562 | { |
||
| 563 | MOLLA(x_old,y1_old+DIM_EL,y2_old,RED) |
||
| 564 | } |
||
| 565 | else if( (y2_old>100) && (x_old<X0max) && (x_old>100)) |
||
| 566 | { |
||
| 567 | MOLLA(x_old,X0min+100,y2_old,RED) |
||
| 568 | } |
||
| 569 | sem_post(&mutex); |
||
| 570 | |||
| 571 | x=y2_old-y1_old; |
||
| 572 | x=x-20; |
||
| 573 | T1=x*k; |
||
| 574 | T1_old=T1; |
||
| 575 | |||
| 576 | task_endcycle(); |
||
| 577 | |||
| 578 | } |
||
| 579 | } |
||
| 580 | |||
| 581 | TASK seleziona(int i) |
||
| 582 | { int w,deltax,deltay,disegna; |
||
| 583 | |||
| 584 | eli_occ=0; |
||
| 585 | disegna=0; |
||
| 586 | |||
| 587 | while (1) |
||
| 588 | { |
||
| 589 | disegna=0; |
||
| 590 | for (w=0;w<5;w++) |
||
| 591 | { |
||
| 592 | deltax=Pos_pe_x[w]-Pos_x; |
||
| 593 | deltay=Pos_pe_y[w]-Pos_y-DIM_PESO-20+DIM_EL; |
||
| 594 | if ( (pre_peso[w]==1) && (eli_occ==0) && (deltax<4) && (deltax>-4) && (deltay<3) && (deltay>-3) ) |
||
| 595 | { |
||
| 596 | disegna=1; |
||
| 597 | if ( (tastiera=='p') && (libero_peso[w]==1) ) |
||
| 598 | { |
||
| 599 | eli_occ=1; |
||
| 600 | peso_agganciato[w]=1; |
||
| 601 | n_peso=w; |
||
| 602 | |||
| 603 | } |
||
| 604 | } |
||
| 605 | } |
||
| 606 | |||
| 607 | if(disegna==1) |
||
| 608 | { |
||
| 609 | sem_wait(&mutex); |
||
| 610 | grx_box(X0max-90,Y0min+5,X0max-5,Y0min+95,RED); |
||
| 611 | // grx_text("Ok", X0max-40, Y0min+50, COL_TESTO, RED); |
||
| 612 | sem_post(&mutex); |
||
| 613 | } else |
||
| 614 | { |
||
| 615 | sem_wait(&mutex); |
||
| 616 | grx_box(X0max-95,Y0min+5,X0max-5,Y0min+95,COL_SFONDO); |
||
| 617 | sem_post(&mutex); |
||
| 618 | } |
||
| 619 | |||
| 620 | if(eli_occ==1) |
||
| 621 | { |
||
| 622 | sem_wait(&mutex); |
||
| 623 | grx_box(X0max-90-100,Y0min+5,X0max-5-100,Y0min+95,GREEN); |
||
| 624 | // grx_text("Go", X0max-35-100, Y0min+50, COL_TESTO, GREEN); |
||
| 625 | sem_post(&mutex); |
||
| 626 | } else |
||
| 627 | { |
||
| 628 | sem_wait(&mutex); |
||
| 629 | grx_box(X0max-90-100,Y0min+5,X0max-5-100,Y0min+95,COL_SFONDO); |
||
| 630 | sem_post(&mutex); |
||
| 631 | } |
||
| 632 | |||
| 633 | |||
| 634 | if (tastiera=='l') peso_agganciato[n_peso]=0; |
||
| 635 | |||
| 636 | task_endcycle(); |
||
| 637 | |||
| 638 | } |
||
| 639 | } |
||
| 640 | |||
| 641 | |||
| 642 | //############################################################### |
||
| 643 | // ## |
||
| 644 | // Funzione di uscita dal programma ## |
||
| 645 | // ## |
||
| 646 | //################################################################ |
||
| 647 | void my_end(KEY_EVT* e) |
||
| 648 | { |
||
| 649 | //#################################### |
||
| 650 | // Sezione esecutiva della funzione ## |
||
| 651 | //#################################### |
||
| 652 | |||
| 653 | grx_close(); |
||
| 654 | cprintf("Ctrl-brk pressed!\n"); |
||
| 655 | sys_end(); |
||
| 656 | } // Fine della funzionemy_end |
||
| 657 | |||
| 658 | //################################################################ |
||
| 659 | // ## |
||
| 660 | // Funzione di uscita dal programma ## |
||
| 661 | // ## |
||
| 662 | //################################################################ |
||
| 663 | void end() |
||
| 664 | { |
||
| 665 | //#################################### |
||
| 666 | // Sezione esecutiva della funzione ## |
||
| 667 | //#################################### |
||
| 668 | |||
| 669 | grx_close(); |
||
| 670 | } // Fine della funzione my_end |
||
| 671 | |||
| 672 | |||
| 673 | |||
| 674 | //############################################################## |
||
| 675 | // ## |
||
| 676 | // Main ## |
||
| 677 | // ## |
||
| 678 | //############################################################## |
||
| 679 | int main() |
||
| 680 | { |
||
| 681 | //######################################## |
||
| 682 | // Sezione dichiarativa delle variabili ## |
||
| 683 | //######################################## |
||
| 684 | // Identficativi di task generici |
||
| 685 | PID pid0, pid1, pid3, pid5; //, pid2,pid4,pid6; |
||
| 686 | |||
| 687 | HARD_TASK_MODEL m_hard; |
||
| 688 | NRT_TASK_MODEL m_nrt; |
||
| 689 | |||
| 690 | // Ascoltatore di eventi |
||
| 691 | KEY_EVT emerg; |
||
| 692 | // Contatori |
||
| 693 | int j; |
||
| 694 | |||
| 695 | |||
| 696 | //#################################### |
||
| 697 | // Sezione esecutiva del main ## |
||
| 698 | //#################################### |
||
| 699 | |||
| 700 | // Inizializzazione dei parametri hartik |
||
| 701 | sem_init(&mutex,0,1); |
||
| 702 | sem_init(&pu,0,1); |
||
| 703 | |||
| 704 | emerg.ascii = 'x'; |
||
| 705 | emerg.scan = KEY_X; |
||
| 706 | emerg.flag = ALTL_BIT; |
||
| 707 | keyb_hook(emerg, my_end); |
||
| 708 | sys_atrunlevel(end, NULL, RUNLEVEL_BEFORE_EXIT); |
||
| 709 | |||
| 710 | if (grx_open(800, 600, 8) < 0) |
||
| 711 | { |
||
| 712 | cprintf("GRX Err\n"); |
||
| 713 | sys_abort(300); |
||
| 714 | } |
||
| 715 | cprintf("scheda ok\n"); |
||
| 716 | |||
| 717 | r=0.1; |
||
| 718 | |||
| 719 | /* Disposizione degli elementi grafici statici */ |
||
| 720 | sem_wait(&mutex); |
||
| 721 | draw_static(); |
||
| 722 | sem_post(&mutex); |
||
| 723 | |||
| 724 | // Attivazione dei task |
||
| 725 | hard_task_default_model(m_hard); |
||
| 726 | hard_task_def_wcet(m_hard,1); // wcet ignored!!! |
||
| 727 | hard_task_def_usemath(m_hard); |
||
| 728 | |||
| 729 | hard_task_def_mit(m_hard,PER_DISEGNA); |
||
| 730 | pid0=task_create("elicottero",elicottero,&m_hard,NULL); |
||
| 731 | task_activate(pid0); |
||
| 732 | |||
| 733 | hard_task_def_mit(m_hard,PER_DISEGNA); |
||
| 734 | pid1=task_create("indicometro",disegna_stato,&m_hard,NULL); |
||
| 735 | task_activate(pid1); |
||
| 736 | |||
| 737 | hard_task_def_mit(m_hard,PER_MOLLA); |
||
| 738 | pid3=task_create("molla",molla,&m_hard,NULL); |
||
| 739 | task_activate(pid3); |
||
| 740 | |||
| 741 | hard_task_def_mit(m_hard,PER_DISEGNA); |
||
| 742 | pid5=task_create("seleziona",seleziona,&m_hard,NULL); |
||
| 743 | task_activate(pid5); |
||
| 744 | |||
| 745 | |||
| 746 | hard_task_def_mit(m_hard,PER_DISEGNA); |
||
| 747 | hard_task_def_arg(m_hard,0); |
||
| 748 | pid_ruspa[0]=task_create("ruspa",ruspa,&m_hard,NULL); |
||
| 749 | task_activate(pid_ruspa[0]); |
||
| 750 | n_ruspa=0; |
||
| 751 | |||
| 752 | nrt_task_default_model(m_nrt); |
||
| 753 | nrt_task_def_usemath(m_nrt); |
||
| 754 | nrt_task_def_arg(m_nrt,0); |
||
| 755 | pid_kill_e = task_create("kill_task",kill_task, &m_nrt, NULL); |
||
| 756 | pid_pulisci_e = task_create("pulisci",pulisci, &m_nrt, NULL); |
||
| 757 | |||
| 758 | nrt_task_def_arg(m_nrt,(void *)1); |
||
| 759 | pid_kill_p = task_create("kill_task",kill_task, &m_nrt, NULL); |
||
| 760 | pid_pulisci_p = task_create("pulisci",pulisci, &m_nrt, NULL); |
||
| 761 | |||
| 762 | |||
| 763 | |||
| 764 | |||
| 765 | do { |
||
| 766 | tastiera = keyb_getch(BLOCK); |
||
| 767 | |||
| 768 | } while(tastiera!=ESC); |
||
| 769 | |||
| 770 | task_kill(pid5); |
||
| 771 | task_kill(pid3); |
||
| 772 | task_kill(pid1); |
||
| 773 | task_kill(pid0); |
||
| 774 | |||
| 775 | task_kill(pid_kill_p); |
||
| 776 | task_kill(pid_kill_e); |
||
| 777 | |||
| 778 | if (n_ruspa!=5) task_kill(pid_ruspa[n_ruspa]); |
||
| 779 | for(j=0;j<5;j++) { if (pre_peso[j]==1) task_kill(pid_peso[j]); } |
||
| 780 | |||
| 781 | grx_close(); |
||
| 782 | sys_end(); |
||
| 783 | |||
| 784 | return 0; |
||
| 785 | } // Fine del main |
||
| 786 | |||
| 787 | /***************************<Fine del file>*******************************/ |