| 5,30 → 5,28 |
|---|
| |
| #include "rtw.h" |
| |
| extern CAB input_cid[NINPUTCAB]; |
| extern CAB output_cid[NOUTPUTCAB]; |
| |
| TASK SENSOR_body(void *input_port) { |
| TASK SENSOR_body(void *output_port) { |
| |
| real_T *p; |
| int in = (int)(input_port); |
| char cname[20]; |
| real_T *p, value; |
| int out = (int)(output_port); |
| |
| if (in >= NINPUTCAB) return NULL; |
| if (out >= NOUTPUTCAB) return NULL; |
| |
| sprintf(cname,"INPUT%d",in); |
| input_cid[in] = cab_create(cname, sizeof(real_T), 2); |
| |
| while(1) { |
| |
| /* Get CAB message */ |
| p = (real_T *)cab_getmes(output_cid[out]); |
| |
| /* Reserve a message */ |
| p = (real_T *)cab_reserve(input_cid[in]); |
| /* Set value */ |
| value = *p; |
| |
| /* Save data */ |
| *p = 1000.0; |
| |
| /* Put CAB message */ |
| cab_putmes(input_cid[in], p); |
| |
| /* Release CAB message */ |
| cab_unget(output_cid[out], p); |
| |
| cprintf("Value %d = %lf\n",out,value); |
| |
| task_endcycle(); |
| |
| } |
| 40,24 → 38,39 |
|---|
| void activate_sensors() { |
| |
| HARD_TASK_MODEL SENSOR_task; |
| PID SENSOR_pid; |
| PID DISTANCE_pid,ANGLE_pid,CURVE_pid; |
| |
| hard_task_default_model(SENSOR_task); |
| hard_task_def_mit(SENSOR_task,10000); |
| hard_task_def_wcet(SENSOR_task,1000); |
| /* Set sensor port number */ |
| hard_task_def_arg(SENSOR_task,(void *)(0)); |
| hard_task_def_mit(SENSOR_task,60000); |
| hard_task_def_wcet(SENSOR_task,4000); |
| hard_task_def_usemath(SENSOR_task); |
| hard_task_def_ctrl_jet(SENSOR_task); |
| |
| SENSOR_pid = task_create("SENSOR0",SENSOR_body,&SENSOR_task,NULL); |
| if (SENSOR_pid == NIL) { |
| cprintf("Error: Cannot create RealTime Workshop [SENSOR0] Task\n"); |
| |
| hard_task_def_arg(SENSOR_task,(void *)(0)); |
| DISTANCE_pid = task_create("DISTANCE",SENSOR_body,&SENSOR_task,NULL); |
| if (DISTANCE_pid == NIL) { |
| cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n"); |
| sys_end(); |
| } |
| |
| task_activate(SENSOR_pid); |
| hard_task_def_arg(SENSOR_task,(void *)(1)); |
| ANGLE_pid = task_create("ANGLE",SENSOR_body,&SENSOR_task,NULL); |
| if (DISTANCE_pid == NIL) { |
| cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n"); |
| sys_end(); |
| } |
| |
| hard_task_def_arg(SENSOR_task,(void *)(2)); |
| CURVE_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL); |
| if (CURVE_pid == NIL) { |
| cprintf("Error: Cannot create RealTime Workshop [CURVE] Task\n"); |
| sys_end(); |
| } |
| |
| task_activate(DISTANCE_pid); |
| task_activate(ANGLE_pid); |
| task_activate(CURVE_pid); |
| |
| } |
| |
| |