Subversion Repositories shark

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1655 giacomo 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
 *              Bera Marco              mbera@libero.it
10
 *              Varasio Gabriele        varasio@odino.unipv.it
11
 *
12
 * Universita' degli studi di Pavia
13
 *
14
 * http://www.sssup.it
15
 * http://retis.sssup.it
16
 * http://shark.sssup.it
17
 */
18
 
19
/**
20
 ------------
21
 CVS :        $Id: tel.c,v 1.1.1.1 2004-05-24 18:03:39 giacomo Exp $
22
 
23
 File:        $File$
24
 Revision:    $Revision: 1.1.1.1 $
25
 Last update: $Date: 2004-05-24 18:03:39 $
26
 ------------
27
**/
28
 
29
/*
30
 * Copyright (C) 2000 Paolo Gai and Giorgio Buttazzo
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
/*      PERIODIC PROCESS TEST                                   */
50
/****************************************************************/
51
 
52
#include <ll/i386/x-dos.h>
53
#include <kernel/kern.h>
54
#include <modules/cabs.h>
55
#include <drivers/glib.h>
56
#include <drivers/keyb.h>
57
#include <math.h>
58
#include <semaphore.h>
59
#include <stdlib.h>
60
#include "moon.h"
61
 
62
 
63
#define N_MAX_TELESCOPI 5
64
#define X0      10
65
#define pi      3.14
66
#define ex       2.71
67
#define NCAB    N_MAX_TELESCOPI+1
68
#define dim_msg 8
69
#define dim_x 49
70
#define dim_y 49
71
#define radius 9
72
#define Y_TELESCOPI 300
73
/* task periods */
74
#define PERIOD_T1  80000
75
#define PERIOD_T4 100000
76
#define PERIOD_T5 100000
77
#define WCET_T 5000
78
int period_t1=0;
79
#define SOGLIA 8
80
 
81
char    *cname[NCAB] = {"cab1", "cab2", "cab3", "cab4","cab5","cab6"};
82
 
83
// posizioni dei telescopi e rispettivi poli
84
 
85
int     x[5] = {100, 200, 300, 400,500};
86
//double  poli[5]={-50,-30,-1, -20, -5};
87
double  poli[5];
88
// guadagno proporzionale
89
double kp=0.0;
90
// flag per rilevare la pressione del tasto spazio
91
int numero_telescopi=0;
92
int flag=0;
93
KEY_EVT         move_tasto;
94
// Immagini ausiliarie per il disegno della luna
95
BYTE lunabkg[900];
96
BYTE lunadest[900];
97
 
98
 
99
/* A semaphore used to access Video Cards in mutual exclusion */
100
sem_t   mutex;
101
sem_t   mutex_tasto;
102
 
103
/***************************************************************/
104
 
105
// cab necessari allo scambio delle coordinate dell'oggetto e delle
106
// immagini dei rispettivi telescopi
107
 
108
CAB   cid[NCAB];
109
 
110
// vettori per memorizzare le immagini
111
BYTE videobuf[dim_x*dim_y];
112
BYTE videobuf1[dim_x*dim_y];
113
BYTE videobuf2[dim_x*dim_y];
114
BYTE videobuf3[dim_x*dim_y];
115
BYTE videobuf4[dim_x*dim_y];
116
BYTE videobuf5[dim_x*dim_y];
117
 
118
 // Variabili per il disegno delle stelle sullo sfondo
119
 int num_p;
120
 int y_p[7];
121
 int x_p[7];
122
 int dim_p[7];
123
 int col_p[7];
124
 
125
 
126
 
127
TASK    media(void *arg)
128
{
129
char *p;
130
 
131
int i=0;
132
int j=0;
133
while(1)
134
{
135
        p = cab_getmes(cid[1]);
136
        memcpy(videobuf,p,dim_x*dim_y*sizeof(BYTE));
137
        cab_unget(cid[1], p);
138
        p = cab_getmes(cid[2]);
139
        memcpy(videobuf1,p,dim_x*dim_y*sizeof(BYTE));
140
        cab_unget(cid[2], p);
141
        p = cab_getmes(cid[3]);
142
        memcpy(videobuf2,p,dim_x*dim_y*sizeof(BYTE));
143
        cab_unget(cid[3], p);
144
        p = cab_getmes(cid[4]);
145
        memcpy(videobuf4,p,dim_x*dim_y*sizeof(BYTE));
146
        cab_unget(cid[4], p);
147
 
148
        p = cab_getmes(cid[5]);
149
        memcpy(videobuf5,p,dim_x*dim_y*sizeof(BYTE));
150
        cab_unget(cid[5], p);
151
 
152
        // calcolo media
153
 
154
        for (i=0;i<dim_x;i++)
155
 
156
        {
157
                for(j=0;j<dim_y;j++)
158
                {
159
                videobuf3[i*dim_x+j]=(double)(videobuf[i*dim_x+j]+videobuf1[i*dim_x+j]+videobuf2[i*dim_x+j]+videobuf4[i*dim_x+j]+videobuf5[i*dim_x+j])/numero_telescopi;
160
                }
161
        }
162
 
163
        // visualizza immagine mediata
164
        sem_wait(&mutex);
165
        grx_putimage(570,10,570+dim_x-1,10+dim_y-1,videobuf3);
166
        sem_post(&mutex);      
167
        task_endcycle();
168
     }
169
}
170
 
171
TASK    move(void *arg)
172
{
173
//int     i = (int)arg;
174
char *p;
175
int x_object=300;  // posizione dell'oggetto da rilevare
176
int y_object=200;
177
// variabile contatore
178
int z=0;
179
char coord_x[dim_msg];
180
p = cab_reserve(cid[0]);
181
sprintf(coord_x,"%d %d",x_object,y_object);
182
memcpy(p,coord_x,dim_msg*sizeof(char));
183
cab_putmes(cid[0], p);
184
 
185
      //Normalizzazione
186
      for(z=0;z<900;z++)
187
        {
188
        if (luna[z]>0)
189
                {
190
                luna[z]=((luna[z])*16.0/255.0)+16;
191
                }
192
 
193
        }
194
 
195
 
196
        task_endcycle();
197
        while (1)
198
        {
199
 
200
        // Cancellazione Luna
201
        sem_wait(&mutex);      
202
        grx_box(x_object-15,y_object-15,x_object+14,y_object+14,0);
203
        sem_post(&mutex);
204
        if (keyb_getcode(&move_tasto,NON_BLOCK))
205
                {
206
                       if (move_tasto.ascii=='4')    
207
                       x_object=x_object-5;      
208
                       if (move_tasto.ascii=='6')            
209
                       x_object=x_object+5;
210
                       if (move_tasto.ascii=='2')
211
                       y_object=y_object+5;
212
                       if (move_tasto.ascii=='8')
213
                       y_object=y_object-5;
214
                       if (move_tasto.ascii=='9'){    
215
                       y_object=y_object-5;                
216
                       x_object=x_object+5; }      
217
                       if (move_tasto.ascii=='1'){    
218
                       y_object=y_object+5;                
219
                       x_object=x_object-5; }
220
                       if (move_tasto.ascii=='7'){    
221
                       y_object=y_object-5;                
222
                       x_object=x_object-5; }
223
                       if (move_tasto.ascii=='3'){    
224
                       y_object=y_object+5;
225
                    x_object=x_object+5;}
226
                    if (move_tasto.ascii==' '){
227
                    sem_wait(&mutex_tasto);
228
                    flag=1;
229
                            if(numero_telescopi<N_MAX_TELESCOPI)
230
                            {
231
                            numero_telescopi++;
232
                            }
233
                    sem_post(&mutex_tasto);
234
                    }
235
              //CONTROLLI SULLA POSIZIONE
236
                       if(y_object<(dim_y/2+70)) y_object=dim_y/2+70;
237
                       if(y_object>(240-dim_y/2)) y_object=(240-dim_y/2);
238
                       if(x_object<(dim_x/2+20 )) x_object=dim_x/2+20;
239
                       if(x_object>(630-dim_x/2-10)) x_object=(630-dim_x/2-10);
240
                }
241
                p = cab_reserve(cid[0]);
242
                sprintf(coord_x,"%d %d",x_object,y_object);
243
                memcpy(p,coord_x,dim_msg*sizeof(char));
244
                cab_putmes(cid[0], p);
245
 
246
        for(z=0;z<7;z++)
247
        {
248
        if (sqrt(pow((x_object-x_p[z]),2)+pow((y_object-y_p[z]),2))<20+dim_p[z]+radius)
249
                        grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]);        
250
        }
251
 
252
 
253
        sem_wait(&mutex);
254
        // Gestione sfondo
255
        grx_getimage(x_object-15,y_object-15,x_object+14, y_object+14,lunabkg);
256
        for(z=0;z<900;z++)
257
        {
258
         if( luna[z]==0 && lunabkg!=0 )
259
          {
260
                lunadest[z]=lunabkg[z];
261
          }
262
         else
263
          {
264
         lunadest[z]=luna[z];
265
          }
266
        }
267
        grx_putimage(x_object-15,y_object-15,x_object+14, y_object+14,lunadest);
268
        sem_post(&mutex);
269
        task_endcycle();
270
        }
271
}
272
 
273
 
274
TASK    tele(void *arg)
275
{
276
int     i = (int)arg;
277
int     passi;
278
char    s[100];
279
int x_object=300;  // posizione dell'oggetto da rilevare
280
int y_object=200;
281
int x_start=500;   // posizione iniziale dei telescopi
282
int y_start=190;
283
int x_current=x_start; // posizione corrente dei telescopi
284
int y_current=y_start;
285
int x_current_d[9];
286
int y_current_d[9];
287
int y= Y_TELESCOPI;  // coordinata y dei telescopi
288
double alpha_new=atan((y-(double)y_current)/((x[i]-(double)x_current)));
289
double alpha=alpha_new;
290
double distance=0.0;
291
double alpha_target=0.0;
292
double tc=0.1;
293
double u=0.0;
294
double u_old=0.0;
295
double errore=0.0;
296
double delta_x=0;
297
double delta_y=0;
298
 
299
char *p;
300
//int j=0;
301
// indice matrice per aggiunta di rumore
302
int k=0;
303
int q=0;
304
double polo=poli[i];
305
int val=0;
306
 
307
 
308
 
309
BYTE videobuf[dim_x*dim_y];
310
 
311
        passi = 0;
312
        //srand(i);
313
        //srand(i+sys_gettime(NULL));
314
        alpha_target=atan((y-(double)y_object)/((double)x_object-x[i]));      
315
 
316
        sem_wait(&mutex);
317
        grx_text("targ",4,340,12,0);
318
        sprintf(s,"%f",poli[i]);
319
        grx_text(s,x[i],y+60,12,0);
320
        grx_text("polo",4,y+60,12,0);
321
        grx_text("new",4,y+50,12,0);
322
        sprintf(s,"%1.6f",alpha_target);
323
        grx_text(s, x[i]-25,y+50,12,0);
324
        grx_rect(x[i]-40,y+25,x[i]+40,y+85,2);
325
        grx_rect(x[i]-40,y+90,x[i]+40,y+150,2);
326
        sem_post(&mutex);
327
 
328
        // Disegno telescopio
329
        grx_disc(x[i],y,19,i+1);
330
        grx_box( x[i]-19,y,x[i]+19,y+20,i+1);
331
 
332
 
333
        task_endcycle();
334
 
335
        while (1) {
336
                // legge di controllo
337
                passi++;
338
                //sprintf(s,"%d",passi);
339
                //grx_text(s, 50,110,12,0);
340
                p = cab_getmes(cid[0]);
341
                sscanf(p,"%d %d",&val,&y_object);
342
                cab_unget(cid[0], p);
343
                x_object=val;      
344
                alpha_target=atan((y-(double)y_object)/((double)x_object-x[i]));
345
                if (alpha_target <0)
346
                {
347
                alpha_target=3.14+alpha_target;
348
                }
349
 
350
                errore=alpha_target-alpha_new;
351
                u=u_old+kp*tc*errore;
352
                //alpha_new=0.13*alpha+0.87*u;
353
                alpha_new=(exp(polo*tc))*alpha+(1-exp(polo*tc))*u;
354
                u_old=u;
355
                alpha=alpha_new;
356
 
357
                // implementazione dei limiti degli attuattori
358
                if (alpha_new > 3.14)
359
                        alpha_new=3.14;
360
                if (alpha_new <0)
361
                        alpha_new=0;
362
 
363
                distance=sqrt(((y-y_object)*(y-y_object))+((x_object-x[i])*(x_object-x[i])));
364
 
365
                sem_wait(&mutex);
366
              // Cancello braccio telescopio
367
              if (passi>1)
368
              {
369
                      for (k=0; k<9 ; k++)
370
                        {
371
                        grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],0);
372
                        }
373
             }
374
                x_current=x[i]+distance*cos(alpha_new);
375
                y_current=300-distance*sin(alpha_new);
376
 
377
                // lettura immagine
378
 
379
                grx_getimage(x_current-(dim_x/2),y_current-(dim_y/2),x_current+(dim_x/2),y_current+(dim_y/2),videobuf);
380
 
381
                //aggiunta rumore
382
 
383
                sem_post(&mutex);
384
                for (k=0;k<dim_x;k++)
385
                {
386
                        for(q=0;q<dim_y;q++)
387
                        {
388
                        int num=0;
389
                                num=rand();
390
                                num=(num%10)+1;
391
                                if (num>SOGLIA)
392
                                {
393
                                videobuf[k*dim_x+q]=videobuf[k*dim_x+q]+1;
394
                                }
395
                        }
396
                }
397
                sem_wait(&mutex);
398
 
399
                grx_putimage(x[i]-25,y+92,x[i]-25+dim_x-1,y+92+dim_y-1,videobuf);
400
 
401
                sprintf(s,"%3.4f",(180*(alpha_target/pi)));
402
                grx_text(s, x[i]-25,y+40,12,0);
403
                sprintf(s,"%3.4f",180*(alpha_new/pi));
404
                grx_text(s, x[i]-25,y+50,12,0);
405
 
406
                delta_x=x[i]+20*cos(alpha_new);
407
                delta_y=y-20*sin(alpha_new);  
408
 
409
                for ( k=0; k<9 ; k++) {
410
                x_current_d[k]=x[i]+50*cos(alpha_new+(k-4)*0.01);
411
                y_current_d[k]=300-50*sin(alpha_new+(k-4)*0.01);  }
412
 
413
             // Disegno braccio telescopio
414
                for(k=0 ; k<9 ; k++)
415
                 grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],i+1);
416
 
417
                sem_post(&mutex);
418
 
419
                // scrive immagine nel cab
420
                p = cab_reserve(cid[i+1]);
421
                memcpy(p,videobuf,dim_x*dim_y*sizeof(BYTE));
422
                cab_putmes(cid[i+1], p);
423
 
424
              task_endcycle();
425
 
426
        }
427
 
428
}
429
 
430
 
431
/****************************************************************/
432
 
433
/* This is the exception handler. It is called when an exception
434
   is raised.
435
   It exits from the graphical mode, then it prints a message and
436
   shutdown the kernel using sys_abort()
437
*/
438
 
439
void demo_exc_handler(int signo, siginfo_t *info, void *extra)
440
{
441
  struct timespec t;
442
 
443
  grx_close();
444
 
445
  /* Default action for an kern exception is  */
446
  kern_cli();
447
  ll_gettime(TIME_EXACT, &t),
448
  kern_printf("\nS.Ha.R.K. Exception raised!!!"
449
              "\nTime (s:ns)     :%ld:%ld"
450
              "\nException number:%d (numbers in include/bits/errno.h)"
451
              "\nPID             :%d\n",
452
              t.tv_sec, t.tv_nsec, info->si_value.sival_int,
453
              info->si_task);
454
  sys_abort(1);
455
}
456
 
457
/******************************************************************/
458
 
459
/* This function is called when Alt-X is pressed.
460
   It simply shutdown the system using sys_end.
461
   Note that the byebye() function is called only if we exit from
462
   the system using sys_end()!!!!
463
*/
464
void my_end(KEY_EVT* e)
465
{
466
        sys_end();
467
}
468
 
469
/******************************************************************/
470
 
471
/* This function is called when the system exit correctly after Alt-X.
472
   It exits from the graphic mode and then it prints a small greeting.
473
   Note that:
474
   - The function calls grx_exit, so it must be registered using
475
     RUNLEVEL_BEFORE_EXIT (RUNLEVEL_AFTER_EXIT does not work because
476
     at that point the kernel is already returned in real mode!!!)
477
   - When an exception is raised, the exception handler is called.
478
     Since the exception handler already exits from the graphic mode,
479
     this funcion has not to be called. For this reason:
480
     . we registered byebye using the flag NO_AT_ABORT
481
     . the exception handler exits using sys_abort; in that way byebye is
482
       NOT called
483
*/
484
 
485
void byebye(void *arg)
486
{
487
  grx_close();
488
  kern_printf("Bye Bye!\n");
489
}
490
 
491
/****************************** MAIN ******************************/
492
 
493
int main(int argc, char **argv)
494
{
495
  PID             pid1, pid4,pid5;
496
  KEY_EVT         emerg;
497
  int i=0;  
498
  int z=0; // contatore per disegno dei pianeti
499
  HARD_TASK_MODEL m1, m4,m5;
500
  //FILE *fp;
501
 
502
  struct sigaction action;
503
 
504
  cid[0] = cab_create(cname[0], dim_msg, 5);
505
  cid[1] = cab_create(cname[1], dim_x*dim_y, 3);
506
  cid[2] = cab_create(cname[2], dim_x*dim_y, 3);
507
  cid[3] = cab_create(cname[3], dim_x*dim_y, 3);
508
  cid[4] = cab_create(cname[4], dim_x*dim_y, 4);
509
  cid[5] = cab_create(cname[5], dim_x*dim_y, 3);
510
  cid[6] = cab_create(cname[6], dim_x*dim_y, 3);
511
 
512
  /*fp=fopen("file.txt","r");
513
  fscanf(fp,"%d",a);
514
  fclose(fp);
515
  */
516
 
517
        /* Init the standard S.Ha.R.K. exception handler */
518
        action.sa_flags = SA_SIGINFO;            /* Set the signal action */
519
        action.sa_sigaction = demo_exc_handler;
520
        action.sa_handler = 0;
521
        sigfillset(&action.sa_mask); /* we block all the other signals... */
522
 
523
        if (sigaction(SIGHEXC, &action, NULL) == -1) {  /* set the signal */
524
          perror("Error initializing signals...");
525
          sys_end();
526
        }
527
 
528
        /* Set the closing function */
529
        sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT|NO_AT_ABORT);
530
 
531
        /* Initializes the semaphore */
532
        sem_init(&mutex,0,1);
533
 
534
        /* graphic card Initialization */
535
        if (grx_init() < 1) {
536
           sys_abort(1);
537
        }
538
 
539
        if (grx_open(640, 480, 8) < 0) {
540
            kern_printf("GRX Err\n");
541
            sys_abort(1);
542
        }
543
        kern_printf("Video card ok!\n");
544
 
545
 
546
 
547
        /* set the keyboard handler to exit correctly */
548
        emerg.ascii = 'x';
549
        emerg.scan = KEY_X;
550
        emerg.flag = ALTL_BIT;
551
        keyb_hook(emerg,my_end);
552
 
553
        srand(i+sys_gettime(NULL));
554
 
555
        emerg.ascii = 'x';
556
        emerg.scan = KEY_X;
557
        emerg.flag = ALTR_BIT;
558
        keyb_hook(emerg,my_end);
559
        sem_wait(&mutex);
560
        /* a small banner */
561
        grx_clear(0);
562
 
563
        grx_text("REALTIME TELESCOPES di Marco Bera e Gabriele Varasio",8,8,WHITE,0);
564
        grx_text("Premere Spazio per creare un telescopio",8,22,WHITE,0);
565
        grx_text("Premere Alt-X per uscire",8,32,WHITE,0);
566
        grx_text("Usare il tastierino numerico per muovere l'oggetto",8,42,WHITE,0);
567
 
568
        // DISEGNARE BORDI
569
        grx_rect(1,1,638,478,2);
570
        grx_rect(558,1,638,61,2);
571
        grx_rect(1,1,638,61,2);
572
 
573
 
574
       // Disegno sfondo
575
        for(z=0;z<7;z++)
576
        {
577
        num_p=rand();
578
        y_p[z]=100+((num_p%130)+1);
579
        x_p[z]=50+((num_p%540)+1);
580
        dim_p[z]=2+((num_p%5)+1);
581
        col_p[z]=((num_p%10)+1);
582
        grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]);
583
        }    
584
        sem_post(&mutex);
585
 
586
 
587
        hard_task_default_model(m4);
588
        hard_task_def_ctrl_jet (m4);
589
        hard_task_def_arg      (m4, (void *)3);
590
        hard_task_def_wcet     (m4, 19000);
591
        hard_task_def_mit      (m4, PERIOD_T4);
592
        hard_task_def_group    (m4,1);
593
        pid4 = task_create("move", move, &m4, NULL);
594
        if (pid4 == NIL) {
595
          grx_close();
596
          perror("Could not create task <move>");
597
          sys_abort(1);
598
        }
599
        task_activate(pid4);
600
        hard_task_default_model(m5);
601
        hard_task_def_ctrl_jet (m5);
602
        hard_task_def_arg      (m5, (void *)3);
603
        hard_task_def_wcet     (m5, 19000);
604
        hard_task_def_mit      (m5, PERIOD_T5);
605
        hard_task_def_group    (m5,1);
606
        pid5 = task_create("media", media, &m5, NULL);
607
 
608
        if (pid5 == NIL) {
609
          grx_close();
610
          perror("Could not create task <move>");
611
          sys_abort(1);
612
        }
613
        task_activate(pid5);
614
        /* and finally we activate the three threads... */
615
 
616
 
617
 
618
        do {
619
        int val_flag=0;
620
        sem_wait(&mutex_tasto);
621
        val_flag=flag;
622
        sem_post(&mutex_tasto);
623
 
624
        if ((val_flag==1) && (i < N_MAX_TELESCOPI))
625
        {
626
        flag=0;
627
        hard_task_default_model(m1);
628
        hard_task_def_ctrl_jet (m1);
629
        hard_task_def_arg      (m1, (void *)i);
630
        hard_task_def_wcet     (m1, WCET_T);
631
//        hard_task_def_mit      (m1, PERIOD_T1);
632
        hard_task_def_mit      (m1, period_t1);
633
 
634
        hard_task_def_group    (m1,1);
635
        pid1 = task_create("tele1", tele, &m1, NULL);
636
        if (pid1 == NIL)
637
        {
638
          grx_close();
639
          perror("Could not create task <tele1>");
640
          sys_abort(1);
641
        }
642
            task_activate(pid1);
643
            i++;
644
        }
645
    } while (1);
646
 
647
        /*
648
           now the task main ends, but the system does not shutdown because
649
           there are others.
650
 
651
           The demo will finish if a Alt-X key is pressed.
652
        */
653
 
654
        return 0;
655
}
656
/*********** lettura da file ********************/
657
void read_cfg_file(int argc, char **argv)
658
{
659
  int err;
660
  DOS_FILE      *fp;
661
  char myfilebuf[1000];
662
  int myfilebuf_length;
663
 
664
 
665
 
666
  if (2)
667
  {
668
    fp = DOS_fopen("dati.cnf","r");
669
 
670
    if (fp)
671
    {
672
      /* read up to 1000 chars */
673
      myfilebuf_length = DOS_fread(&myfilebuf,1,1000,fp);
674
 
675
      /* check for errors */
676
      err = DOS_error();
677
 
678
      cprintf("Read %d bytes...\n", myfilebuf_length);
679
 
680
              if (err)
681
              {
682
                cprintf("Error %d reading file...Using default values\n", err);
683
              }
684
              else
685
              {
686
                //geti(myfilebuf, &pos, &NMouses);          // Number of Mouses
687
              sscanf(myfilebuf,"%lf %lf %lf %lf %lf %lf %d",&poli[0],&poli[1],&poli[2],&poli[3],&poli[4],&kp,&period_t1);
688
              }
689
 
690
      DOS_fclose(fp);
691
      return;
692
 
693
    }
694
    else {
695
      /* error!! */
696
      err = DOS_error();
697
      /* note that if you call DOS_error() here, it return 0!!! */
698
      cprintf("Error %d opening myfile.txt...Using default values\n", err);
699
    }
700
 
701
  }
702
  else {
703
    cprintf("Wrong number of arguments...\n");
704
    l1_exit(0);
705
  }
706
}
707
 
708
 
709
/****************************************************************/