robot.h

Go to the documentation of this file.

#ifndef __robot__
#define __robot__

#include <stdint.h>
#include <stdio.h>
#include <mcl.h>
#include <RobotType.h>
#include "RobotTypeArr.h"
#include "roboevent.h"
#include <fsm.h>
#include "fsmloc.h"

#define LAS_CNT         12      /* dont forget to change CRA!! */
/* index of information in times array */
#define LAS_MEAS_CNTI   0       /* counter index of measured times */
#define LAS_MEAS_PERI   1       /* period index */
#define LAS_MEAS_DATAI  2       /* data index */

/* amount of FSM */
#define FSM_CNT         5

/* FSM ids */
enum robot_fsm_id {
    FSM_ID_MAIN = 0,
    FSM_ID_DET  = 1,
    FSM_ID_LOC  = 2,
    FSM_ID_MOTION = 3,
    FSM_ID_PICKUP = 4
};

#define FSM(id) (&robot.fsm[FSM_ID_##id])
#define FSM_GET_BY_ID(id) (&robot.fsm[FSM_ID_##id])

enum bottle_sens_state {
        NOTHING = 0,
        FAR,
        CLOSE
};

/* robot`s position configuration */
struct robo_pos {
        double x;       /* X position [m] */
        double y;       /* Y position [m] */
        double phi;     /* head angle [rad] */
};

struct bottle_sens {
        uint8_t sens1;
        uint8_t sens2;  
        uint8_t sens3;  
        uint8_t sens4;
};

/* Mapping of robot structure fields to locks */
//#define __robot_lock_ lock    /* ROBOT_LOCK() */
#define __robot_lock_act_pos            lock_act_pos
#define __robot_lock_est_pos            lock
#define __robot_lock_goal               lock
#define __robot_lock_IRsensors          lock
#define __robot_lock_joy                lock
#define __robot_lock_laser_recv         lock
#define __robot_lock_new_trajectory     lock
#define __robot_lock_sharpsOpponent     lock
#define __robot_lock_sharpsWaste        lock

/* robot`s main data structure */
struct robo_priv {
        long long cnt;
        unsigned char   mode;   
        pthread_mutex_t lock;
        pthread_mutex_t lock_act_pos;

        void *new_trajectory;
        
        unsigned char isTrajectory;
        unsigned char start;
        
        /* partial FSMs */
        struct robo_fsm fsm[FSM_CNT];

        /* mcl model */
        struct mcl_model mcl;
        /* actual position */
        struct position_type act_pos;

        /* estimated position */
        struct position_type est_pos;
        /* goal position */
        struct position_type goal;
        /* move */
        struct position_type move_dpos;

        /* orte */
        struct laserData_type laser_recv;
        struct motionSpeed_type orte_speed;

        struct stateServa_type serva;
        struct sharpShorts_type sharpsWaste;
        struct sharpLongs_type sharpsOpponent;
        struct stateInnerIR_type IRsensors;
        struct stateDigIn_type startBit;
        struct stateFrontDoor_type frontDoorAngle;
        
        struct bottle_sens bott_sens;
        
        struct motionPos_type odometry;
        
        struct joyData_type joy;

        ORTEPublication *publisherMotor;
        ORTEPublication *publisherServa;
        ORTEPublication *publisherPosition;

        int bottle_under_belt;
        int waste_cnt;
        
};      /* robo_priv */

/* robot`s mode */
enum {
        ROBO_COMPETING = 0,
        ROBO_TESTING
};

/* robot`s global data structure */
extern struct robo_priv robot;

/* locking manipulation */

#define ROBOT_LOCK(var)         (pthread_mutex_lock(&robot.__robot_lock_##var))

#define ROBOT_UNLOCK(var)       (pthread_mutex_unlock(&robot.__robot_lock_##var))



#ifdef __cplusplus
extern "C" {
#endif 

void initOrte();
void terminateOrte();
int robot_init();
int robot_start();
int robot_exit();
int robot_wait();
int robot_destroy();

FSM_STATE_DECL(main_init);
FSM_STATE_DECL(start_pickup);
FSM_STATE_DECL(move_init);
FSM_STATE_DECL(det_start);
FSM_STATE_DECL(loc_init);

#ifdef __cplusplus
}
#endif 


#define DEBUG
#ifdef DEBUG
#define PROG_NAME               "robot"
#define DBG(f, a...)            printf(f, ## a)
#define DBGPROG(f, a...)        printf(PROG_NAME ": " f, ## a)
#else
#define DBG(f, a...)            do { } while(0);
#define DBGPROG(f, a...)        do { } while(0);
#endif 

/* Constants */

#define ROBOT_WIDTH_MM 290
#define ROBOT_ROTATION_RADIUS_MM (254/2)
#define LASER_CENTER_OFFSET_MM 50 /*FIXME*/ /* Distance of the laser from rotation axis  */
#define ROBOT_WHEEL_RADIUS_MM 40
#define ROBOT_AXIS_TO_BACK_MM 75
#define ROBOT_AXIS_TO_FRONT_MM 183
#define ROBOT_AXIS_TO_BELT_MM 280

#define PLAYGROUND_WIDTH_MM     3000
#define PLAYGROUND_HEIGHT_MM    2100

/*Thread priorities*/
#define TRAJ_FOLLOWER_PRIO 255  /* As high as possible */
#define TRAJ_RECALC_PRIO 0      /* As low as possible */
#define SET_THREAD_PRIORITY(tid,prio) \
sched_param param; \
pthread_attr_t tattr; \
pthread_attr_init (&tattr); \
pthread_attr_getschedparam (&tattr, &param); \
param.sched_priority = prio;\
pthread_attr_setschedparam (&tattr, &param);

#endif  /* __robot__ */

---