Subversion Repositories shark

#include "kernel/kern.h"

#include "modules/cabs.h"

#include "tmwtypes.h"

#include "rtw.h"

extern CAB output_cid[NOUTPUTCAB];

TASK SENSOR_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 %d = %lf\n",out,value);

        task_endcycle();

    }

    return NULL;

}

void activate_sensors() {

    HARD_TASK_MODEL     SENSOR_task;

    PID                 DISTANCE_pid,ANGLE_pid,CURVE_pid;

    hard_task_default_model(SENSOR_task);

    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);

    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();

    }

    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);

}