| 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,94 → 395,124 |
|---|
| |
| } |
| |
| TASK OUTPUT_body(void *arg) { |
| TASK INPUT_body(void *input_port) { |
| |
| real_T *output, *p; |
| real_T th; |
| real_T *input, *p; |
| int in = (int)(input_port); |
| |
| POINT pos; |
| PATH_ELEM *e; |
| real_T distance = 0; |
| real_T angle = 0; |
| real_T curve = 0; |
| if (in >= NINPUTCAB) 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); |
| input = (real_T *)rtmGetU(S); |
| |
| output = (real_T *)rtmGetY(S); |
| while(1) { |
| |
| while(1) { |
| /* Get CAB message */ |
| p = (real_T *)cab_getmes(input_cid[in]); |
| |
| pos.x = output[0]; |
| pos.y = output[1]; |
| th = output[2]; |
| |
| 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); |
| /* Set INPUT port */ |
| input[in] = *p; |
| |
| /* Release CAB message */ |
| cab_unget(input_cid[in], p); |
| |
| task_endcycle(); |
| |
| } |
| |
| /* Send Distance */ |
| p = (real_T *)cab_reserve(output_cid[0]); |
| *p = distance; |
| cab_putmes(output_cid[0], p); |
| |
| /* Send Angle */ |
| p = (real_T *)cab_reserve(output_cid[1]); |
| *p = angle; |
| cab_putmes(output_cid[1], p); |
| return NULL; |
| |
| /* Send Curve */ |
| p = (real_T *)cab_reserve(output_cid[2]); |
| *p = curve; |
| cab_putmes(output_cid[2], p); |
| } |
| |
| task_endcycle(); |
| } |
| return NULL; |
| TASK OUTPUT_body(void *output_port) { |
| |
| real_T *output, *p; |
| int out = (int)(output_port); |
| char cname[20]; |
| |
| if (out >= NOUTPUTCAB) return NULL; |
| |
| sprintf(cname,"OUTPUT%d",out); |
| output_cid[out] = cab_create(cname, sizeof(real_T), 2); |
| |
| output = (real_T *)rtmGetY(S); |
| |
| while(1) { |
| |
| /* Reserve a message */ |
| p = (real_T *)cab_reserve(output_cid[out]); |
| |
| /* Save data */ |
| *p = output[out]; |
| |
| /* Put CAB message */ |
| cab_putmes(output_cid[out], 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; |
| |
| |
| 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; |
| |
| |
| 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); |
| hard_task_def_mit(RTW_task,RTW_period); |
| hard_task_def_wcet(RTW_task,RTW_wcet); |
| hard_task_def_usemath(RTW_task); |
| hard_task_def_ctrl_jet(RTW_task); |
| |
| |
| RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL); |
| if (RTW_pid == NIL) { |
| cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n"); |
| cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n"); |
| 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(); |
| } |
| |
| 508,26 → 519,23 |
|---|
| 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); |
| task_activate(OUTPUT_pid); |
| |
| } |
| |
| int main() { |
| |
| Init_Rtw(); |
| |
| activate_sensors(); |
| activate_control(); |
| activate_actuators(); |
| |
| while(simulation_run); |
| |