Subversion Repositories shark

Rev

Rev 1123 | Rev 1453 | 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
 ------------
1159 pj 21
 CVS :        $Id: orbit.c,v 1.4 2003-05-01 19:44:07 pj Exp $
1085 pj 22
 
23
 File:        $File$
1159 pj 24
 Revision:    $Revision: 1.4 $
25
 Last update: $Date: 2003-05-01 19:44:07 $
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 <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
 
1111 pj 188
int main()
1085 pj 189
{
190
  HARD_TASK_MODEL m;
191
  char  c;                      /* carattere letto da tastiera  */
192
 
193
  sys_atrunlevel(my_fine, NULL, RUNLEVEL_BEFORE_EXIT);
194
 
195
  sem_init(&mutex,0,1);
196
 
197
  get_par();
198
  keyb_getch(BLOCK);
199
 
200
  grx_init();
201
  grx_open(640, 480, 8);
202
 
203
  grx_disc(XGS,YGS,RT,12);
204
  grx_circle(XGS,YGS,RT,14);
205
 
206
  np = 0;
207
 
208
  do {
209
    if (np < MAX-1) {
210
      np++;
211
      hard_task_default_model(m);
212
      hard_task_def_arg      (m, (void *)np);
213
      hard_task_def_wcet     (m, wcet);
214
      hard_task_def_mit      (m, period);
215
      hard_task_def_usemath  (m);
216
      pid = task_create("massa", massa, &m, NULL);
217
      if (pid == NIL) {
1159 pj 218
        sys_shutdown_message("Could not create task");
219
        sys_end();
1085 pj 220
      }
221
      task_activate(pid);
222
    }
223
    c = keyb_getch(BLOCK);
224
 
225
  } while (c != ESC);
226
 
227
  sys_end();
228
 
229
  return 0;
230
}
231
 
232
/*------------------------------------------------------*/
233
/*              file reading                            */
234
/*------------------------------------------------------*/
235
 
236
void    read_file(void)
237
{
238
  int   err;
239
  DOS_FILE *fp;
240
 
241
  fp = DOS_fopen("orbit.dat","r");
242
 
243
  if (!fp) {
244
    err = DOS_error();
245
    cprintf("Error %d opening myfile.txt...\n", err);
246
    flen = 0;
247
    return;
248
  }
249
 
250
  flen = DOS_fread(&fbuf, 1, 1000, fp);
251
  cprintf("Read %d bytes from orbit.dat\n", flen);
252
 
253
  DOS_fclose(fp);
254
}
255
 
256
/*------------------------------------------------------*/
257
/*              get data from buffer                    */
258
/*------------------------------------------------------*/
259
 
260
void    get_par(void)
261
{
262
  int   x = 0;
263
  int   i;
264
  double        vx, vy;
265
 
266
  while ((fbuf[x] != ':') && (x < flen)) x++;
267
  x++;
268
  sscanf(&fbuf[x], "%d", &period);
269
  cprintf("period = %d\n", period);
270
 
271
  while ((fbuf[x] != ':') && (x < flen)) x++;
272
  x++;
273
  sscanf(&fbuf[x], "%d", &wcet);
274
  cprintf("wcet = %d\n", wcet);
275
 
276
  while ((fbuf[x] != ':') && (x < flen)) x++;
277
  x++;
278
  sscanf(&fbuf[x], "%f", &delta);
279
  cprintf("delta = %f\n", delta);
280
 
281
  while ((fbuf[x] != ':') && (x < flen)) x++;
282
  x++;
283
  sscanf(&fbuf[x], "%f", &scala);
284
  cprintf("scala = %f\n", scala);
285
 
286
  while ((fbuf[x] != ':') && (x < flen)) x++;
287
  x++;
288
  sscanf(&fbuf[x], "%f", &G);
289
  cprintf("G = %20.15f\n", G);
290
 
291
  while ((fbuf[x] != ':') && (x < flen)) x++;
292
  x++;
293
  sscanf(&fbuf[x], "%d", &RP);
294
  cprintf("RP = %d\n", RP);
295
 
296
  while ((fbuf[x] != ':') && (x < flen)) x++;
297
  x++;
298
  sscanf(&fbuf[x], "%d", &RT);
299
  cprintf("RT = %d\n", RT);
300
 
301
  while ((fbuf[x] != ':') && (x < flen)) x++;
302
  x++;
303
  sscanf(&fbuf[x], "%d", &XGS);
304
  cprintf("XGS = %d\n", XGS);
305
 
306
  while ((fbuf[x] != ':') && (x < flen)) x++;
307
  x++;
308
  sscanf(&fbuf[x], "%d", &YGS);
309
  cprintf("YGS = %d\n", YGS);
310
 
311
  for (i=0; i<MAX; i++) {
312
    while ((fbuf[x] != ':') && (x < flen)) x++;
313
    x++; x++;
314
    sscanf(&fbuf[x], "%f", &mass[i]);
315
 
316
    while ((fbuf[x] != '\t') && (x < flen)) x++;
317
    x++;
318
    sscanf(&fbuf[x], "%f", &vx);
319
 
320
    while ((fbuf[x] != '\t') && (x < flen)) x++;
321
    x++;
322
    sscanf(&fbuf[x], "%f", &vy);
323
    pos[i].x = vx; pos[i].y = vy;
324
 
325
    while ((fbuf[x] != '\t') && (x < flen)) x++;
326
    x++;
327
    sscanf(&fbuf[x], "%f", &vx);
328
 
329
    while ((fbuf[x] != '\t') && (x < flen)) x++;
330
    x++;
331
    sscanf(&fbuf[x], "%f", &vy);
332
    vel[i].x = vx; vel[i].y = vy;
333
 
334
    cprintf("mass[%d] = %f\t", i, mass[i]);
335
    cprintf("pos: %f, %f\t", pos[i].x, pos[i].y);
336
    cprintf("vel: %f, %f\n", vel[i].x, vel[i].y);
337
  }
338
}
339
 
340
/*--------------------------------------------------------------*/
341