Subversion Repositories shark

Rev

Rev 1456 | 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
 ------------
1550 pj 21
 CVS :        $Id: orbit.c,v 1.9 2005-01-08 14:34:48 pj Exp $
1085 pj 22
 
23
 File:        $File$
1550 pj 24
 Revision:    $Revision: 1.9 $
25
 Last update: $Date: 2005-01-08 14:34:48 $
1085 pj 26
 ------------
27
*/
28
 
29
/*
30
 * Copyright (C) 2000 Giorgio Buttazzo and Paolo Gai
31
 *
32
 * This program is free software; you can redistribute it and/or modify
33
 * it under the terms of the GNU General Public License as published by
34
 * the Free Software Foundation; either version 2 of the License, or
35
 * (at your option) any later version.
36
 *
37
 * This program is distributed in the hope that it will be useful,
38
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
39
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
40
 * GNU General Public License for more details.
41
 *
42
 * You should have received a copy of the GNU General Public License
43
 * along with this program; if not, write to the Free Software
44
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
45
 *
46
 */
47
 
48
/****************************************************************/
49
/*              SIMULAZIONE DI MASSE CHE ORBITANO               */
50
/****************************************************************/
51
 
52
#include <kernel/kern.h>
53
#include <semaphore.h>
54
#include <stdlib.h>
55
#include <math.h>
1453 giacomo 56
#include <string.h>
1085 pj 57
 
1453 giacomo 58
#include <drivers/shark_fb26.h>
59
#include <drivers/shark_keyb26.h>
60
 
1456 giacomo 61
#define MAX     11              /* numero massimo pianeti       */
1085 pj 62
#define XMAX    639             /* valore massimo coordinata X  */
63
#define YMAX    479             /* valore massimo coordinata Y  */
64
 
1453 giacomo 65
extern int vga16color[16];
66
 
1085 pj 67
struct  coord {
68
  double        x;
69
  double        y;
70
};
71
 
72
int     flen;                   // file length
73
 
1456 giacomo 74
double  mass[MAX];              /* vettore masse pianeti        */
1085 pj 75
struct  coord pos[MAX];         /* vettore posizioni attuali    */
76
struct  coord vel[MAX];         /* vettore velocita' iniziali   */
77
 
78
int     XGS, YGS;               /* Coordinate centro spazio     */
79
int     RP, RT;                 /* raggio pianeta, raggio Terra */
80
int     np;                     /* numero attuale di pianeti    */
1456 giacomo 81
double  G;                      /* Gravitazione Universale      */
82
double  tick;                   /* tick di sistema              */
83
double  delta;                  /* incremento temporale         */
84
double  scala;                  /* fattore grafico di scala     */
1085 pj 85
 
1453 giacomo 86
char    fbuf[1000] ="\
87
----------------------------------------------------\n\
1456 giacomo 88
period: 10000   wcet: 500\n\
1453 giacomo 89
delta: 1.       scala: 150.\n\
90
G: 6.6e-15\n\
91
r_pianeta: 4    r_Terra: 8\n\
92
X_centro: 320   Y_centro: 240\n\
93
------------------- pianeti ------------------------\n\
94
        massa   pos.x   pos.y   vel.x   vel.y\n\
1456 giacomo 95
0:      \t 6.0e21  \t 0.      \t 0.      \t 0.      \t 0. \n\
96
1:      \t 1.0e21  \t 10000.  \t 8000.   \t -60.    \t 0. \n\
97
2:      \t 1.0e8   \t 5000.   \t 0.      \t 0.      \t 80. \n\
98
3:      \t 5.0e18  \t 10000.  \t 8000.   \t -50.    \t 0. \n\
99
4:      \t 1.0e9   \t 10000.  \t 8000.   \t -40.    \t 20. \n\
100
5:      \t 1.0e15  \t 1000.   \t 5000.   \t -80.    \t 0. \n\
101
6:      \t 1.0e5   \t 1000.   \t 5000.   \t -80.    \t 0. \n\
102
7:      \t 1.0e17  \t 1000.   \t 5000.   \t -80.    \t 0. \n\
103
8:      \t 1.0e5   \t 1000.   \t 5000.   \t -80.    \t 0. \n\
104
9:      \t 1.0e5   \t 1000.   \t 5000.   \t -80.    \t 0. \n\
105
10:     \t 1.0e5   \t 1000.   \t 5000.   \t -80.    \t 0. \n\
1453 giacomo 106
----------------------------------------------------\n";
107
 
1085 pj 108
// -------------------------------------------------------
109
// NOTA: %f o %lf significa double e %nf significa float
110
// -------------------------------------------------------
111
 
112
PID     pid;
113
int     period;                 /* task period                  */
114
int     wcet;                   /* task wcet                    */
115
sem_t   mutex;                  /* semaforo di mutua esclusione */
116
 
117
 
118
void    get_par(void);
119
 
120
/*--------------------------------------------------------------*/
121
 
122
int     inside(int x, int y)
123
{
124
  return ((x > RP) && (x < XMAX-RP) &&
125
          (y > RP) && (y < YMAX-RP));
126
}
127
 
128
/*--------------------------------------------------------------*/
129
 
130
TASK    massa(void *xxx)
131
{
132
  int   i = (int)xxx;           /* parametro del task */
133
  int   gx, gy;                 /* coordinate grafiche pallina  */
134
  int   ox, oy;                 /* vecchia posizione pallina    */
135
  int   j;
136
  int   r, col;                 /* raggio e colore pianeta      */
137
  double        dt;                     /* incremento temporale         */
138
  double        dist=0.0, dx, dy;       /* distanze pianeta-pianeta     */
139
  double        dist0=0.0;              /* distanze pianeta-Terra       */
140
  double        x, y;                   /* posizione del pianeta        */
141
  double        vx, vy;                 /* velocita' del pianeta        */
142
  double        ax, ay;                 /* accelerazione del pianeta    */
143
  double        k;                      /* variabile ausiliaria         */
144
  double        arg;                    /* variabile di appoggio        */
145
 
146
  x = pos[i].x; y = pos[i].y;
147
  vx = vel[i].x;        vy = vel[i].y;
148
  ox = XGS + x / scala;
149
  oy = YGS + y / scala;
150
  dt = delta;
151
 
152
  do {
153
    x = pos[i].x;
154
    y = pos[i].y;
155
    ax = ay = 0.0;
156
    for (j=0; j<np; j++) {
157
      if (j != i) {
158
        dx = pos[j].x - x;
159
        dy = pos[j].y - y;
160
        arg = dx*dx + dy*dy;
161
        dist = sqrt(arg);
162
        if (dist < RP*scala) dist = RP*scala;
163
        k = G * mass[j] / (dist*dist*dist);
164
        ax += k * dx;
165
        ay += k * dy;
166
      }
167
      if (j == 0) dist0 = dist - (RP+RT)*scala;
168
    }
169
    x += vx*dt + 0.5*ax*dt*dt;
170
    y += vy*dt + 0.5*ay*dt*dt;
171
    vx += ax * dt;
172
    vy += ay * dt;
173
 
174
    gx = XGS + x / scala;
175
    gy = YGS + y / scala;
176
 
177
    r = RP;
178
    col = i + 1;
179
 
180
    sem_wait(&mutex);
1453 giacomo 181
    grx_disc(ox,oy,r,vga16color[0]);
182
    grx_disc(gx,gy,r,vga16color[col]);
1085 pj 183
    sem_post(&mutex);
184
 
185
    pos[i].x = x;       pos[i].y = y;
186
    ox = gx;    oy = gy;
187
 
188
    task_endcycle();
189
 
190
  } while ((dist0 > 0) && inside(gx,gy));
191
 
192
  sem_wait(&mutex);
1453 giacomo 193
  grx_disc(ox,oy,r,vga16color[0]);
194
  grx_disc(XGS,YGS,RT,vga16color[12]);
195
  grx_circle(XGS,YGS,RT,vga16color[14]);
1085 pj 196
  sem_post(&mutex);
197
 
198
  return NULL;
199
}
200
 
201
/*--------------------------------------------------------------*/
202
/*                      MAIN                                    */
203
/*--------------------------------------------------------------*/
204
 
1111 pj 205
int main()
1085 pj 206
{
207
  HARD_TASK_MODEL m;
208
  char  c;                      /* carattere letto da tastiera  */
209
 
210
  sem_init(&mutex,0,1);
211
 
212
  get_par();
213
 
1453 giacomo 214
  grx_disc(XGS,YGS,RT,vga16color[12]);
215
  grx_circle(XGS,YGS,RT,vga16color[14]);
1085 pj 216
 
217
  np = 0;
218
 
219
  do {
220
    if (np < MAX-1) {
221
      np++;
222
      hard_task_default_model(m);
223
      hard_task_def_arg      (m, (void *)np);
224
      hard_task_def_wcet     (m, wcet);
225
      hard_task_def_mit      (m, period);
226
      hard_task_def_usemath  (m);
227
      pid = task_create("massa", massa, &m, NULL);
228
      if (pid == NIL) {
1159 pj 229
        sys_shutdown_message("Could not create task");
1550 pj 230
        exit(1);
1085 pj 231
      }
232
      task_activate(pid);
233
    }
234
    c = keyb_getch(BLOCK);
235
 
236
  } while (c != ESC);
237
 
1550 pj 238
  exit(1);
1085 pj 239
 
240
  return 0;
241
}
242
 
243
/*------------------------------------------------------*/
244
/*              file reading                            */
245
/*------------------------------------------------------*/
246
 
247
void    read_file(void)
248
{
249
  int   err;
250
  DOS_FILE *fp;
251
 
252
  fp = DOS_fopen("orbit.dat","r");
253
 
254
  if (!fp) {
255
    err = DOS_error();
256
    cprintf("Error %d opening myfile.txt...\n", err);
257
    flen = 0;
258
    return;
259
  }
260
 
261
  flen = DOS_fread(&fbuf, 1, 1000, fp);
262
  cprintf("Read %d bytes from orbit.dat\n", flen);
263
 
264
  DOS_fclose(fp);
265
}
266
 
267
/*------------------------------------------------------*/
268
/*              get data from buffer                    */
269
/*------------------------------------------------------*/
270
 
271
void    get_par(void)
272
{
273
  int   x = 0;
274
  int   i;
1456 giacomo 275
  double vx, vy, px, py;
1454 giacomo 276
 
1453 giacomo 277
  flen = strlen(fbuf);
1085 pj 278
 
279
  while ((fbuf[x] != ':') && (x < flen)) x++;
280
  x++;
281
  sscanf(&fbuf[x], "%d", &period);
282
 
283
  while ((fbuf[x] != ':') && (x < flen)) x++;
284
  x++;
285
  sscanf(&fbuf[x], "%d", &wcet);
286
 
287
  while ((fbuf[x] != ':') && (x < flen)) x++;
288
  x++;
1456 giacomo 289
  sscanf(&fbuf[x], "%lf", &delta);
1085 pj 290
 
291
  while ((fbuf[x] != ':') && (x < flen)) x++;
292
  x++;
1456 giacomo 293
  sscanf(&fbuf[x], "%lf", &scala);
1085 pj 294
 
295
  while ((fbuf[x] != ':') && (x < flen)) x++;
296
  x++;
1456 giacomo 297
  sscanf(&fbuf[x], "%lf", &G);
1085 pj 298
 
299
  while ((fbuf[x] != ':') && (x < flen)) x++;
300
  x++;
301
  sscanf(&fbuf[x], "%d", &RP);
302
 
303
  while ((fbuf[x] != ':') && (x < flen)) x++;
304
  x++;
305
  sscanf(&fbuf[x], "%d", &RT);
306
 
307
  while ((fbuf[x] != ':') && (x < flen)) x++;
308
  x++;
309
  sscanf(&fbuf[x], "%d", &XGS);
310
 
311
  while ((fbuf[x] != ':') && (x < flen)) x++;
312
  x++;
313
  sscanf(&fbuf[x], "%d", &YGS);
314
 
315
  for (i=0; i<MAX; i++) {
1455 giacomo 316
 
317
    mass[i] = 0.;
318
    px = 0.;
319
    py = 0.;
320
    vx = 0.;
321
    vy = 0.;
322
 
1454 giacomo 323
    while ((fbuf[x] != '\t') && (x < flen)) x++;
324
    x++;
1456 giacomo 325
    sscanf(&fbuf[x], "%lf", &mass[i]);
1085 pj 326
 
327
    while ((fbuf[x] != '\t') && (x < flen)) x++;
328
    x++;
1456 giacomo 329
    sscanf(&fbuf[x], "%lf", &px);
1085 pj 330
 
331
    while ((fbuf[x] != '\t') && (x < flen)) x++;
332
    x++;
1456 giacomo 333
    sscanf(&fbuf[x], "%lf", &py);
1085 pj 334
 
335
    while ((fbuf[x] != '\t') && (x < flen)) x++;
336
    x++;
1456 giacomo 337
    sscanf(&fbuf[x], "%lf", &vx);
1085 pj 338
 
339
    while ((fbuf[x] != '\t') && (x < flen)) x++;
340
    x++;
1456 giacomo 341
    sscanf(&fbuf[x], "%lf", &vy);
1454 giacomo 342
 
343
    pos[i].x = px; pos[i].y = py;
344
 
1085 pj 345
    vel[i].x = vx; vel[i].y = vy;
1454 giacomo 346
 
1085 pj 347
  }
348
}
349
 
350
/*--------------------------------------------------------------*/
351