Subversion Repositories shark

Rev

Details | 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: orbit.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
 
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 <drivers/glib.h>
54
#include <drivers/keyb.h>
55
#include <semaphore.h>
56
#include <stdlib.h>
57
#include <math.h>
58
 
59
#define MAX     11              /* numero massimo pianeti       */
60
#define ESC     27              /* codice ASCII tasto ESCAPE    */
61
#define XMAX    639             /* valore massimo coordinata X  */
62
#define YMAX    479             /* valore massimo coordinata Y  */
63
 
64
struct  coord {
65
  double        x;
66
  double        y;
67
};
68
 
69
char    fbuf[1000];             // buffer for reading a file
70
int     flen;                   // file length
71
 
72
double  mass[MAX];              /* vettore masse pianeti        */
73
struct  coord pos[MAX];         /* vettore posizioni attuali    */
74
struct  coord vel[MAX];         /* vettore velocita' iniziali   */
75
 
76
int     XGS, YGS;               /* Coordinate centro spazio     */
77
int     RP, RT;                 /* raggio pianeta, raggio Terra */
78
int     np;                     /* numero attuale di pianeti    */
79
double  G;                      /* Gravitazione Universale      */
80
double  tick;                   /* tick di sistema              */
81
double  delta;                  /* incremento temporale         */
82
double  scala;                  /* fattore grafico di scala     */
83
 
84
// -------------------------------------------------------
85
// NOTA: %f o %lf significa double e %nf significa float
86
// -------------------------------------------------------
87
 
88
PID     pid;
89
int     period;                 /* task period                  */
90
int     wcet;                   /* task wcet                    */
91
sem_t   mutex;                  /* semaforo di mutua esclusione */
92
 
93
 
94
void    get_par(void);
95
 
96
/*--------------------------------------------------------------*/
97
 
98
void    my_fine(void *arg)
99
{
100
  grx_close();
101
}
102
 
103
/*--------------------------------------------------------------*/
104
 
105
int     inside(int x, int y)
106
{
107
  return ((x > RP) && (x < XMAX-RP) &&
108
          (y > RP) && (y < YMAX-RP));
109
}
110
 
111
/*--------------------------------------------------------------*/
112
 
113
TASK    massa(void *xxx)
114
{
115
  int   i = (int)xxx;           /* parametro del task */
116
  int   gx, gy;                 /* coordinate grafiche pallina  */
117
  int   ox, oy;                 /* vecchia posizione pallina    */
118
  int   j;
119
  int   r, col;                 /* raggio e colore pianeta      */
120
  double        dt;                     /* incremento temporale         */
121
  double        dist=0.0, dx, dy;       /* distanze pianeta-pianeta     */
122
  double        dist0=0.0;              /* distanze pianeta-Terra       */
123
  double        x, y;                   /* posizione del pianeta        */
124
  double        vx, vy;                 /* velocita' del pianeta        */
125
  double        ax, ay;                 /* accelerazione del pianeta    */
126
  double        k;                      /* variabile ausiliaria         */
127
  double        arg;                    /* variabile di appoggio        */
128
 
129
  x = pos[i].x; y = pos[i].y;
130
  vx = vel[i].x;        vy = vel[i].y;
131
  ox = XGS + x / scala;
132
  oy = YGS + y / scala;
133
  dt = delta;
134
 
135
  do {
136
    x = pos[i].x;
137
    y = pos[i].y;
138
    ax = ay = 0.0;
139
    for (j=0; j<np; j++) {
140
      if (j != i) {
141
        dx = pos[j].x - x;
142
        dy = pos[j].y - y;
143
        arg = dx*dx + dy*dy;
144
        dist = sqrt(arg);
145
        if (dist < RP*scala) dist = RP*scala;
146
        k = G * mass[j] / (dist*dist*dist);
147
        ax += k * dx;
148
        ay += k * dy;
149
      }
150
      if (j == 0) dist0 = dist - (RP+RT)*scala;
151
    }
152
    x += vx*dt + 0.5*ax*dt*dt;
153
    y += vy*dt + 0.5*ay*dt*dt;
154
    vx += ax * dt;
155
    vy += ay * dt;
156
 
157
    gx = XGS + x / scala;
158
    gy = YGS + y / scala;
159
 
160
    r = RP;
161
    col = i + 1;
162
 
163
    sem_wait(&mutex);
164
    grx_disc(ox,oy,r,0);
165
    grx_disc(gx,gy,r,col);
166
    sem_post(&mutex);
167
 
168
    pos[i].x = x;       pos[i].y = y;
169
    ox = gx;    oy = gy;
170
 
171
    task_endcycle();
172
 
173
  } while ((dist0 > 0) && inside(gx,gy));
174
 
175
  sem_wait(&mutex);
176
  grx_disc(ox,oy,r,0);
177
  grx_disc(XGS,YGS,RT,12);
178
  grx_circle(XGS,YGS,RT,14);
179
  sem_post(&mutex);
180
 
181
  return NULL;
182
}
183
 
184
/*--------------------------------------------------------------*/
185
/*                      MAIN                                    */
186
/*--------------------------------------------------------------*/
187
 
188
TASK    main()
189
{
190
  HARD_TASK_MODEL m;
191
  char  c;                      /* carattere letto da tastiera  */
192
 
193
  set_exchandler_grx();
194
  sys_atrunlevel(my_fine, NULL, RUNLEVEL_BEFORE_EXIT);
195
 
196
  sem_init(&mutex,0,1);
197
 
198
  get_par();
199
  keyb_getch(BLOCK);
200
 
201
  grx_init();
202
  grx_open(640, 480, 8);
203
 
204
  grx_disc(XGS,YGS,RT,12);
205
  grx_circle(XGS,YGS,RT,14);
206
 
207
  np = 0;
208
 
209
  do {
210
    if (np < MAX-1) {
211
      np++;
212
      hard_task_default_model(m);
213
      hard_task_def_arg      (m, (void *)np);
214
      hard_task_def_wcet     (m, wcet);
215
      hard_task_def_mit      (m, period);
216
      hard_task_def_usemath  (m);
217
      pid = task_create("massa", massa, &m, NULL);
218
      if (pid == NIL) {
219
        grx_close();
220
        perror("Could not create task");
221
        c = keyb_getch(BLOCK);
222
        sys_abort(1);
223
      }
224
      task_activate(pid);
225
    }
226
    c = keyb_getch(BLOCK);
227
 
228
  } while (c != ESC);
229
 
230
  sys_end();
231
 
232
  return 0;
233
}
234
 
235
/*------------------------------------------------------*/
236
/*              file reading                            */
237
/*------------------------------------------------------*/
238
 
239
void    read_file(void)
240
{
241
  int   err;
242
  DOS_FILE *fp;
243
 
244
  fp = DOS_fopen("orbit.dat","r");
245
 
246
  if (!fp) {
247
    err = DOS_error();
248
    cprintf("Error %d opening myfile.txt...\n", err);
249
    flen = 0;
250
    return;
251
  }
252
 
253
  flen = DOS_fread(&fbuf, 1, 1000, fp);
254
  cprintf("Read %d bytes from orbit.dat\n", flen);
255
 
256
  DOS_fclose(fp);
257
}
258
 
259
/*------------------------------------------------------*/
260
/*              get data from buffer                    */
261
/*------------------------------------------------------*/
262
 
263
void    get_par(void)
264
{
265
  int   x = 0;
266
  int   i;
267
  double        vx, vy;
268
 
269
  while ((fbuf[x] != ':') && (x < flen)) x++;
270
  x++;
271
  sscanf(&fbuf[x], "%d", &period);
272
  cprintf("period = %d\n", period);
273
 
274
  while ((fbuf[x] != ':') && (x < flen)) x++;
275
  x++;
276
  sscanf(&fbuf[x], "%d", &wcet);
277
  cprintf("wcet = %d\n", wcet);
278
 
279
  while ((fbuf[x] != ':') && (x < flen)) x++;
280
  x++;
281
  sscanf(&fbuf[x], "%f", &delta);
282
  cprintf("delta = %f\n", delta);
283
 
284
  while ((fbuf[x] != ':') && (x < flen)) x++;
285
  x++;
286
  sscanf(&fbuf[x], "%f", &scala);
287
  cprintf("scala = %f\n", scala);
288
 
289
  while ((fbuf[x] != ':') && (x < flen)) x++;
290
  x++;
291
  sscanf(&fbuf[x], "%f", &G);
292
  cprintf("G = %20.15f\n", G);
293
 
294
  while ((fbuf[x] != ':') && (x < flen)) x++;
295
  x++;
296
  sscanf(&fbuf[x], "%d", &RP);
297
  cprintf("RP = %d\n", RP);
298
 
299
  while ((fbuf[x] != ':') && (x < flen)) x++;
300
  x++;
301
  sscanf(&fbuf[x], "%d", &RT);
302
  cprintf("RT = %d\n", RT);
303
 
304
  while ((fbuf[x] != ':') && (x < flen)) x++;
305
  x++;
306
  sscanf(&fbuf[x], "%d", &XGS);
307
  cprintf("XGS = %d\n", XGS);
308
 
309
  while ((fbuf[x] != ':') && (x < flen)) x++;
310
  x++;
311
  sscanf(&fbuf[x], "%d", &YGS);
312
  cprintf("YGS = %d\n", YGS);
313
 
314
  for (i=0; i<MAX; i++) {
315
    while ((fbuf[x] != ':') && (x < flen)) x++;
316
    x++; x++;
317
    sscanf(&fbuf[x], "%f", &mass[i]);
318
 
319
    while ((fbuf[x] != '\t') && (x < flen)) x++;
320
    x++;
321
    sscanf(&fbuf[x], "%f", &vx);
322
 
323
    while ((fbuf[x] != '\t') && (x < flen)) x++;
324
    x++;
325
    sscanf(&fbuf[x], "%f", &vy);
326
    pos[i].x = vx; pos[i].y = vy;
327
 
328
    while ((fbuf[x] != '\t') && (x < flen)) x++;
329
    x++;
330
    sscanf(&fbuf[x], "%f", &vx);
331
 
332
    while ((fbuf[x] != '\t') && (x < flen)) x++;
333
    x++;
334
    sscanf(&fbuf[x], "%f", &vy);
335
    vel[i].x = vx; vel[i].y = vy;
336
 
337
    cprintf("mass[%d] = %f\t", i, mass[i]);
338
    cprintf("pos: %f, %f\t", pos[i].x, pos[i].y);
339
    cprintf("vel: %f, %f\n", vel[i].x, vel[i].y);
340
  }
341
}
342
 
343
/*--------------------------------------------------------------*/
344