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