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,WR_pid,WL_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");
        exit(1);
    }

    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");
        exit(1);
    }

    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");
        exit(1);
    }

    hard_task_def_arg(SENSOR_task,(void *)(3));
    WR_pid = task_create("WR",SENSOR_body,&SENSOR_task,NULL);
    if (WR_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [WR] Task\n");
        exit(1);
    }

    hard_task_def_arg(SENSOR_task,(void *)(4));
    WL_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL);
    if (WR_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [WL] Task\n");
        exit(1);
    }
   
    task_activate(DISTANCE_pid);
    task_activate(ANGLE_pid);
    task_activate(CURVE_pid);
    task_activate(WR_pid);
    task_activate(WL_pid);

}