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 |