Subversion Repositories shark

Rev

Rev 1419 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1418 giacomo 1
#include "kernel/kern.h"
2
#include "modules/cabs.h"
3
 
4
#include "tmwtypes.h"
5
 
6
#include "rtw.h"
7
 
1419 giacomo 8
extern CAB output_cid[NOUTPUTCAB];
1418 giacomo 9
 
1419 giacomo 10
TASK SENSOR_body(void *output_port) {
1418 giacomo 11
 
1419 giacomo 12
    real_T *p, value;
13
    int out = (int)(output_port);
1418 giacomo 14
 
1419 giacomo 15
    if (out >= NOUTPUTCAB) return NULL;
1418 giacomo 16
 
17
    while(1) {
1419 giacomo 18
 
19
        /* Get CAB message */
20
        p = (real_T *)cab_getmes(output_cid[out]);
1418 giacomo 21
 
1419 giacomo 22
        /* Set value */
23
        value = *p;
1418 giacomo 24
 
1419 giacomo 25
        /* Release CAB message */
26
        cab_unget(output_cid[out], p);
27
 
28
        cprintf("Value %d = %lf\n",out,value);
29
 
1418 giacomo 30
        task_endcycle();
31
 
32
    }
33
 
34
    return NULL;
35
 
36
}
37
 
38
void activate_sensors() {
39
 
40
    HARD_TASK_MODEL     SENSOR_task;
1420 giacomo 41
    PID                 DISTANCE_pid,ANGLE_pid,CURVE_pid,WR_pid,WL_pid;
1418 giacomo 42
 
43
    hard_task_default_model(SENSOR_task);
1419 giacomo 44
    hard_task_def_mit(SENSOR_task,60000);
45
    hard_task_def_wcet(SENSOR_task,4000);
1418 giacomo 46
    hard_task_def_usemath(SENSOR_task);
47
    hard_task_def_ctrl_jet(SENSOR_task);
1419 giacomo 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");
1418 giacomo 53
        sys_end();
54
    }
55
 
1419 giacomo 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");
60
        sys_end();
61
    }
1418 giacomo 62
 
1419 giacomo 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");
67
        sys_end();
68
    }
1420 giacomo 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");
74
        sys_end();
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");
81
        sys_end();
82
    }
1419 giacomo 83
 
84
    task_activate(DISTANCE_pid);
85
    task_activate(ANGLE_pid);
86
    task_activate(CURVE_pid);
1420 giacomo 87
    task_activate(WR_pid);
88
    task_activate(WL_pid);
1419 giacomo 89
 
1418 giacomo 90
}
91
 
92
 
93