9,7 → 9,6 |
|
#include "kernel/kern.h" |
#include "modules/cabs.h" |
#include "percorso.h" |
|
#include "rtw.h" |
|
369,9 → 368,6 |
|
TASK RTW_body(void *arg) { |
|
real_T *input, *p; |
|
input = (real_T *)rtmGetU(S); |
simulation_run = 1; |
|
while (!GBLbuf.stopExecutionFlag && |
382,21 → 378,6 |
(boolean_T *)&rtmGetStopRequested(S)); |
|
if (rtmGetStopRequested(S)) break; |
|
/* Get PAR0 */ |
if (input_cid[0] != -1) { |
p = (real_T *)cab_getmes(input_cid[0]); |
input[0] = *p; |
cab_unget(input_cid[0], p); |
} |
|
/* Get PAR1 */ |
if (input_cid[1] != -1) { |
p = (real_T *)cab_getmes(input_cid[1]); |
input[1] = *p; |
cab_unget(input_cid[1], p); |
} |
|
rt_OneStep(S); |
|
task_endcycle(); |
414,60 → 395,70 |
|
} |
|
TASK OUTPUT_body(void *arg) { |
TASK INPUT_body(void *input_port) { |
|
real_T *input, *p; |
int in = (int)(input_port); |
|
if (in >= NINPUTCAB) return NULL; |
|
input = (real_T *)rtmGetU(S); |
|
while(1) { |
|
/* Get CAB message */ |
p = (real_T *)cab_getmes(input_cid[in]); |
|
/* Set INPUT port */ |
input[in] = *p; |
|
/* Release CAB message */ |
cab_unget(input_cid[in], p); |
|
task_endcycle(); |
|
} |
|
return NULL; |
|
} |
|
TASK OUTPUT_body(void *output_port) { |
|
real_T *output, *p; |
real_T th; |
int out = (int)(output_port); |
char cname[20]; |
|
POINT pos; |
PATH_ELEM *e; |
real_T distance = 0; |
real_T angle = 0; |
real_T curve = 0; |
if (out >= NOUTPUTCAB) return NULL; |
|
output_cid[0] = cab_create("DISTANCE", sizeof(real_T), 2); |
output_cid[1] = cab_create("ANGLE", sizeof(real_T), 2); |
output_cid[2] = cab_create("CURVE", sizeof(real_T), 2); |
sprintf(cname,"OUTPUT%d",out); |
output_cid[out] = cab_create(cname, sizeof(real_T), 2); |
|
output = (real_T *)rtmGetY(S); |
|
while(1) { |
|
pos.x = output[0]; |
pos.y = output[1]; |
th = output[2]; |
/* Reserve a message */ |
p = (real_T *)cab_reserve(output_cid[out]); |
|
e = find_closest_elem(&pos, 1.0); |
if (e != 0) { |
distance = get_distance_from_elem(&pos, e); |
angle = th - get_angle_from_elem(&pos,e); |
curve = is_curve(e); |
} |
/* Save data */ |
*p = output[out]; |
|
/* Send Distance */ |
p = (real_T *)cab_reserve(output_cid[0]); |
*p = distance; |
cab_putmes(output_cid[0], p); |
/* Put CAB message */ |
cab_putmes(output_cid[out], p); |
|
/* Send Angle */ |
p = (real_T *)cab_reserve(output_cid[1]); |
*p = angle; |
cab_putmes(output_cid[1], p); |
task_endcycle(); |
|
/* Send Curve */ |
p = (real_T *)cab_reserve(output_cid[2]); |
*p = curve; |
cab_putmes(output_cid[2], p); |
} |
|
task_endcycle(); |
} |
return NULL; |
|
} |
|
void Init_Rtw() { |
int main() { |
|
HARD_TASK_MODEL RTW_task,OUTPUT_task; |
PID RTW_pid,OUTPUT_pid; |
HARD_TASK_MODEL RTW_task,INPUT_task,OUTPUT_task; |
PID RTW_pid,INPUT_pid,OUTPUT_pid; |
|
int i; |
|
474,11 → 465,13 |
int RTW_period; |
int RTW_wcet; |
|
int INPUT_period; |
int INPUT_wcet; |
|
int OUTPUT_period; |
int OUTPUT_wcet; |
|
Init_RealTime_Workshop(); |
Init_All_Path(); |
|
for (i=0;i<NINPUTCAB;i++) input_cid[i] = -1; |
for (i=0;i<NOUTPUTCAB;i++) output_cid[i] = -1; |
486,11 → 479,15 |
RTW_period =(int)(rtmGetStepSize(S) * 1000000.0); |
RTW_wcet = (int)(rtmGetStepSize(S) * 1000000.0 * 0.45); |
|
INPUT_period = (int)(10000.0); |
INPUT_wcet = (int)(10000.0 * 0.10); |
|
OUTPUT_period = (int)(10000.0); |
OUTPUT_wcet = (int)(10000.0 * 0.10); |
|
cprintf("Task Setup\n"); |
cprintf("RTW (P %8d W %8d)\n",RTW_period,RTW_wcet); |
cprintf("INPUT (P %8d W %8d)\n",INPUT_period,INPUT_wcet); |
cprintf("OUTPUT (P %8d W %8d)\n",OUTPUT_period,OUTPUT_wcet); |
|
hard_task_default_model(RTW_task); |
505,29 → 502,40 |
sys_end(); |
} |
|
hard_task_default_model(INPUT_task); |
hard_task_def_mit(INPUT_task,INPUT_period); |
hard_task_def_wcet(INPUT_task,INPUT_wcet); |
/* Set input port number */ |
hard_task_def_arg(INPUT_task,(void *)(0)); |
hard_task_def_usemath(INPUT_task); |
hard_task_def_ctrl_jet(INPUT_task); |
|
INPUT_pid = task_create("INPUT0",INPUT_body,&INPUT_task,NULL); |
if (INPUT_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [INPUT0] Task\n"); |
sys_end(); |
} |
|
hard_task_default_model(OUTPUT_task); |
hard_task_def_mit(OUTPUT_task,OUTPUT_period); |
hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet); |
/* Set output port number */ |
hard_task_def_arg(OUTPUT_task,(void *)(0)); |
hard_task_def_usemath(OUTPUT_task); |
hard_task_def_ctrl_jet(OUTPUT_task); |
|
OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL); |
OUTPUT_pid = task_create("OUTPUT0",OUTPUT_body,&OUTPUT_task,NULL); |
if (OUTPUT_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n"); |
cprintf("Error: Cannot create RealTime Workshop [OUTPUT0] Task\n"); |
sys_end(); |
} |
|
task_activate(INPUT_pid); |
task_activate(RTW_pid); |
task_activate(OUTPUT_pid); |
|
} |
|
int main() { |
|
Init_Rtw(); |
|
activate_sensors(); |
activate_control(); |
activate_actuators(); |
|
while(simulation_run); |
|