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 |