0,0 → 1,62 |
#include "kernel/kern.h" |
#include "modules/cabs.h" |
|
#include "tmwtypes.h" |
|
#include "rtw.h" |
|
extern CAB output_cid[NOUTPUTCAB]; |
|
TASK ACTUATOR_body(void *output_port) { |
|
real_T *p, value; |
int out = (int)(output_port); |
|
if (out >= NOUTPUTCAB) return NULL; |
|
while(1) { |
|
/* Get CAB message */ |
p = (real_T *)cab_getmes(output_cid[out]); |
|
/* Set value */ |
value = *p; |
|
/* Release CAB message */ |
cab_unget(output_cid[out], p); |
|
cprintf("Value = %lf\n",value); |
|
task_endcycle(); |
|
} |
|
return NULL; |
|
} |
|
void activate_actuators() { |
|
HARD_TASK_MODEL ACTUATOR_task; |
PID ACTUATOR_pid; |
|
hard_task_default_model(ACTUATOR_task); |
hard_task_def_mit(ACTUATOR_task,20000); |
hard_task_def_wcet(ACTUATOR_task,4000); |
/* Set actuator port number */ |
hard_task_def_arg(ACTUATOR_task,(void *)(0)); |
hard_task_def_usemath(ACTUATOR_task); |
hard_task_def_ctrl_jet(ACTUATOR_task); |
|
ACTUATOR_pid = task_create("ACTUATOR0",ACTUATOR_body,&ACTUATOR_task,NULL); |
if (ACTUATOR_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [ACTUATOR0] Task\n"); |
sys_end(); |
} |
|
task_activate(ACTUATOR_pid); |
|
} |
|
|
|