roboorte.c

#include <orte.h>

#ifdef CONFIG_ORTE_RTL
#include <linux/module.h>
#include <posix/pthread.h>
#define printf rtl_printf
#elif defined CONFIG_ORTE_RTAI
#include <linux/module.h>
#include <rtai/compat.h>  
#define printf rt_printk
#else
#include <stdio.h>
#endif

#include "robot.h"
#include <RobotType.h>
#include <math.h>

ORTEDomain *d=NULL;
double k0 = 0;
double k1 = 0;
double rozvor = 0.255; // rozvor - zjistit
double aktUhel = 0;
int firstRun = 1;
motionPos initPos;

static void recvCallBackLaser(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct laserData_type *instance=(struct laserData_type*)vinstance;

        switch (info->status) {
                case NEW_DATA:
                        printf("laser data received\n");
                        ROBOT_LOCK(laser_recv);
                        robot.laser_recv = *instance;
                        ROBOT_UNLOCK(laser_recv);
                        FSM_SIGNAL(LOC, EV_LASER_RECEIVED);
                        break;
                case DEADLINE:
                        printf("laser data deadline\n");
                        //DBG("laser deadline occurred\n");
                        break;
        }
}

static void *subscriberLaserCreate(void *arg) {
        static struct laserData_type orte_laserData;

        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        laserData_type_register(d);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "laser", "laserData", 
                                 &robot.laser_recv, &deadline, &minimumSeparation, 
                                 recvCallBackLaser, NULL, IPADDRESS_INVALID);
        return arg;
}


static void recvSharp1CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct sharpLongs_type *instance=(struct sharpLongs_type*)vinstance;

        switch (info->status) {
                case NEW_DATA:
                        ROBOT_LOCK(sharpsOpponent);
                        robot.sharpsOpponent = *instance;
                        ROBOT_UNLOCK(sharpsOpponent);
                        //DBG("Callback\n");
                        //fsm_signal(FSM(LOC), EV_LASER_RECEIVED); 
                        break;
                case DEADLINE:
                        //DBG("long sharps deadline occurred\n");
                        break;
        }
}

static void recvSharp2CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct sharpShorts_type *instance=(struct sharpShorts_type*)vinstance;

        switch (info->status) {
                case NEW_DATA:
                        ROBOT_LOCK(sharpsWaste);
                        robot.sharpsWaste = *instance;
                        ROBOT_UNLOCK(sharpsWaste);
                        
                        //fsm_signal(FSM(LOC), EV_LASER_RECEIVED); 
                        break;
                case DEADLINE:
                        //DBG("short sharps deadline occurred\n");
                        break;
        }
}

static void recvIRCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct stateInnerIR_type *instance=(struct stateInnerIR_type*)vinstance;

        switch (info->status) {
                case NEW_DATA:
                        ROBOT_LOCK(IRsensors);
                        robot.IRsensors = *instance;
                        ROBOT_UNLOCK(IRsensors);
                        
                        //fsm_signal(FSM(LOC), EV_LASER_RECEIVED); 
                        break;
                case DEADLINE:
                        //DBG("IR deadline occurred\n");
                        break;
        }
}

static void recvOdoCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct motionPos_type *instance=(struct motionPos_type*)vinstance;
        static struct motionPos_type prevInstance;
        static int firstRun = 1;
        double n = (double)(28.0 / 1.0); // spocitat prevodovy pomer
        double c = (M_PI*2*ROBOT_WHEEL_RADIUS_MM/1000) / (n * 4*500.0); // vzdalenost na pulz - 4.442 je empiricka konstanta :)
        
        double aktk0 = 0;
        double aktk1 = 0;
        double deltaU = 0;
        double dk0 = 0;
        double dk1 = 0;
        double deltAlfa = 0;

        switch (info->status) {
                case NEW_DATA:
                        robot.odometry = *instance;
                        if(firstRun) {
                                prevInstance = *instance;
                                firstRun = 0;
                                break;
                        }
                        else {
                                robot.odometry.left -= initPos.left;
                                robot.odometry.right -= initPos.right;
                        }
                        
                        aktk0 = (prevInstance.left - instance->left) >> 8;
                        aktk1 = (instance->right - prevInstance.right) >> 8;                    
                        prevInstance = *instance;
                        dk0 = aktk0;
                        dk1 = aktk1;    

                        k0 = aktk0;
                        k1 = aktk1;

                        dk0 *= c;
                        dk1 *= c;
                        //printf("dk.left=%f dk.right=%f\n",dk0,dk1);
                        deltaU = (dk0 + dk1) / 2;

                        deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000);    // dk1 - dk0
                        ROBOT_LOCK(act_pos);
                        aktUhel = robot.act_pos.phi;
                        ROBOT_UNLOCK(act_pos);
                        
                        aktUhel += deltAlfa;            // aktualni uhel

                        ROBOT_LOCK(act_pos);
                        robot.act_pos.x += deltaU * cos(aktUhel);
                        robot.act_pos.y += deltaU * sin(aktUhel);
                        robot.act_pos.phi = aktUhel;
                        ROBOT_UNLOCK(act_pos);
                        break;
                case DEADLINE:
                        break;
        }
}

static void recvStartCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct stateDigIn_type *instance=(struct stateDigIn_type*)vinstance;
        static int lastState = 0;
        switch (info->status) {
                case NEW_DATA:
                        if((instance->state==0) && (lastState==0)) //ZAVOLAM STARTOVACI FUNKCI;
                                lastState=1;
                                robot.start = 0;
                        if((instance->state==1) && (lastState==1)) //ZAVOLAM STARTOVACI FUNKCI;
                        {
                                lastState = 2;
                                robot.start = 1;
                                
                        }
                        //fsm_signal(FSM(LOC), EV_LASER_RECEIVED); 
                        break;
                case DEADLINE:
                        //DBG("IR deadline occurred\n");
                        break;
        }
}

static void recvJoyCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
        struct joyData_type *instance=(struct joyData_type*)vinstance;

        switch (info->status) {
                case NEW_DATA:
                        ROBOT_LOCK(joy);
                        robot.joy = *instance;
                        ROBOT_UNLOCK(joy);
                        
                        //fsm_signal(FSM(LOC), EV_LASER_RECEIVED); 
                        break;
                case DEADLINE:
                        //DBG("IR deadline occurred\n");
                        break;
        }
}

void *subscriberJoyCreate(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        joyData_type_register(d);
//      ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv)); 
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "joy", "joyData",          &(robot.joy), &deadline, &minimumSeparation, recvJoyCallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberIRCreate(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        stateInnerIR_type_register(d);
//      ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv)); 
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "IR", "stateInnerIR",              &(robot.IRsensors), &deadline, &minimumSeparation, recvIRCallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberOdoCreate(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        motionPos_type_register(d);
//      ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv)); 
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "position", "motionPos",           &(robot.odometry), &deadline, &minimumSeparation, recvOdoCallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberDICreate(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        stateDigIn_type_register(d);
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "DI", "stateDigIn",                &(robot.startBit), &deadline, &minimumSeparation, recvStartCallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void *subscriberSharp1Create(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        sharpLongs_type_register(d);
//      ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv));
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "sharpSouper", "sharpLongs",
                                 &(robot.sharpsOpponent), &deadline, &minimumSeparation, recvSharp1CallBack, arg, IPADDRESS_INVALID);
                                 //FIXME: ^ Orte to prepisuje? Neni to lockovany!!!!
        return arg;
}

void *subscriberSharp2Create(void *arg) {
        ORTESubscription    *s;
        NtpTime             deadline,minimumSeparation;

        sharpShorts_type_register(d);
//      ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv)); 
        NTPTIME_BUILD(deadline,10);
        NTPTIME_BUILD(minimumSeparation,0);
        s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "sharpLahve", "sharpShorts",               &(robot.sharpsWaste), &deadline, &minimumSeparation, recvSharp2CallBack, arg, IPADDRESS_INVALID);
        return arg;
}

void sendCallBackMotor(const ORTESendInfo *info,void *instance, void *sendCallBackParam) {
        switch (info->status) {
                case NEED_DATA:
                        //DBG("need data\n");
                        break;
                case CQL:
                        //DBG("cql\n");
                        break;
        }
}
static void *publisherMotorCreate(void *arg) {
        NtpTime             persistence, delay;

        motionSpeed_type_register(d);

        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        robot.publisherMotor=ORTEPublicationCreate(d, "motor", "motionSpeed", 
                                                   &robot.orte_speed, &persistence, 1, 
                                                   sendCallBackMotor,  NULL, &delay);
        return arg;
}

void *publisherCreateServo(void *arg) {
        NtpTime             persistence, delay;

        stateServa_type_register(d);
        NTPTIME_BUILD(persistence,3);
        NTPTIME_BUILD(delay,1); 
        robot.publisherServa=ORTEPublicationCreate(d, "serva", "stateServa", &(robot.serva), &persistence, 1, NULL, arg, &delay);
        return arg;
}

void sendCallBackPosition(const ORTESendInfo *info,void *vinstance, void *sendCallBackParam) {
        position *instance=(position*)vinstance;

        switch (info->status) {
                case NEED_DATA:
                        instance = &(robot.act_pos);
                        break;
                case CQL:  //criticalQueueLevel
                        break;
        }
}

void *publisherCreatePosition(void *arg) {
        NtpTime             persistence, delay;

        position_type_register(d);
        NTPTIME_BUILD(persistence,3);
        NtpTimeAssembFromMs(delay,0,40); 
        robot.publisherPosition=ORTEPublicationCreate(d, "pos", "position", &(robot.act_pos), &persistence, 1, sendCallBackPosition, arg, &delay);
        return arg;
}

void initOrte() {
        ORTEInit();

        ORTEVerbositySetOptions("ALL.0");
        d=ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN,NULL,NULL,ORTE_FALSE);
        if (!d) {
                printf("ORTEDomainAppCreate failed!\n");
        }
        subscriberJoyCreate(NULL);
        subscriberLaserCreate(NULL);
        publisherMotorCreate(NULL);
        publisherCreateServo(NULL);
        subscriberSharp1Create(NULL);
        subscriberSharp2Create(NULL);
        subscriberIRCreate(NULL);
        subscriberDICreate(NULL);
        subscriberOdoCreate(NULL);
        publisherCreatePosition(NULL);
}

void terminateOrte() {
        ORTEDomainAppDestroy(d);
}

---