Subversion Repositories shark

Rev

Rev 1625 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1624 giacomo 1
#include "kernel/kern.h"
2
#include "modules/cabs.h"
3
 
4
#include "tmwtypes.h"
5
 
6
#include "rtw.h"
7
 
8
extern CAB output_cid[NOUTPUTCAB];
9
 
10
TASK SENSOR_body(void *output_port) {
11
 
12
    real_T *p, value;
13
    int out = (int)(output_port);
14
 
15
    if (out >= NOUTPUTCAB) return NULL;
16
 
17
    while(1) {
18
 
19
        /* Get CAB message */
20
        p = (real_T *)cab_getmes(output_cid[out]);
21
 
22
        /* Set value */
23
        value = *p;
24
 
25
        /* Release CAB message */
26
        cab_unget(output_cid[out], p);
27
 
28
        cprintf("Value %d = %lf\n",out,value);
29
 
30
        task_endcycle();
31
 
32
    }
33
 
34
    return NULL;
35
 
36
}
37
 
38
void activate_sensors() {
39
 
40
    HARD_TASK_MODEL     SENSOR_task;
41
    PID                 DISTANCE_pid,ANGLE_pid,CURVE_pid,WR_pid,WL_pid;
42
 
43
    hard_task_default_model(SENSOR_task);
44
    hard_task_def_mit(SENSOR_task,60000);
45
    hard_task_def_wcet(SENSOR_task,4000);
46
    hard_task_def_usemath(SENSOR_task);
47
    hard_task_def_ctrl_jet(SENSOR_task);
48
 
49
    hard_task_def_arg(SENSOR_task,(void *)(0));
50
    DISTANCE_pid = task_create("DISTANCE",SENSOR_body,&SENSOR_task,NULL);
51
    if (DISTANCE_pid == NIL) {
52
        cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n");
1650 pj 53
        exit(1);
1624 giacomo 54
    }
55
 
56
    hard_task_def_arg(SENSOR_task,(void *)(1));
57
    ANGLE_pid = task_create("ANGLE",SENSOR_body,&SENSOR_task,NULL);
58
    if (DISTANCE_pid == NIL) {
59
        cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n");
1650 pj 60
        exit(1);
1624 giacomo 61
    }
62
 
63
    hard_task_def_arg(SENSOR_task,(void *)(2));
64
    CURVE_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL);
65
    if (CURVE_pid == NIL) {
66
        cprintf("Error: Cannot create RealTime Workshop [CURVE] Task\n");
1650 pj 67
        exit(1);
1624 giacomo 68
    }
69
 
70
    hard_task_def_arg(SENSOR_task,(void *)(3));
71
    WR_pid = task_create("WR",SENSOR_body,&SENSOR_task,NULL);
72
    if (WR_pid == NIL) {
73
        cprintf("Error: Cannot create RealTime Workshop [WR] Task\n");
1650 pj 74
        exit(1);
1624 giacomo 75
    }
76
 
77
    hard_task_def_arg(SENSOR_task,(void *)(4));
78
    WL_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL);
79
    if (WR_pid == NIL) {
80
        cprintf("Error: Cannot create RealTime Workshop [WL] Task\n");
1650 pj 81
        exit(1);
1624 giacomo 82
    }
83
 
84
    task_activate(DISTANCE_pid);
85
    task_activate(ANGLE_pid);
86
    task_activate(CURVE_pid);
87
    task_activate(WR_pid);
88
    task_activate(WL_pid);
89
 
90
}
91
 
92
 
93