laser-nav.h

/*
 * @(#)lasernav.h       07/03/21
 * 
 * License      : GNU GPL v.2
 * Description  : Robot navigation using 3 passive laser reflectors.
 * Author       : Tran Duy Khanh (www.tran.cz)
 */

/* verbose level 0-3 */
/*#ifndef DEBUG
#define DEBUG 1
#endif*/

/*define this macro to use red reflectors instead of default blue reflectors*/
/*#define USE_RED_REFLECTORS*/

#define FWIDTH                  3000
#define FHEIGHT                 2100
#define SET_POINT_A(p)          SET_POINT((p), 0.0, 0.0)
#define SET_POINT_B(p)          SET_POINT((p), FWIDTH, 0.0)
#define SET_POINT_C(p)          SET_POINT((p), FWIDTH, FHEIGHT)
#define SET_POINT_D(p)          SET_POINT((p), 0.0, FHEIGHT)
#define SET_POINT_E(p)          SET_POINT((p), FWIDTH/2, 0)
#define SET_POINT_F(p)          SET_POINT((p), FWIDTH/2, FHEIGHT)
#define SET_POINT_G(p)          SET_POINT((p), FWIDTH/2, FHEIGHT/4)
#define SET_POINT_H(p)          SET_POINT((p), FWIDTH/2, FHEIGHT/4*3)

/* use red reflectors */
#ifdef USE_RED_REFLECTORS
  #define SET_POINT_K(p)        SET_POINT((p), FWIDTH, FHEIGHT/2)
  #define SET_POINT_L(p)        SET_POINT((p), 0, FHEIGHT/2)
  #define SET_POINT_R1(p)       SET_POINT_K(p)
  #define SET_POINT_R2(p)       SET_POINT_A(p)
  #define SET_POINT_R3(p)       SET_POINT_D(p)
/* use blue reflectors */
#else
  #define SET_POINT_K(p)        SET_POINT((p), 0, FHEIGHT/2)
  #define SET_POINT_L(p)        SET_POINT((p), FWIDTH, FHEIGHT/2)
  #define SET_POINT_R1(p)       SET_POINT_K(p)
  #define SET_POINT_R2(p)       SET_POINT_B(p)
  #define SET_POINT_R3(p)       SET_POINT_C(p)
#endif

#define TIME2ANGLE(p,t)         (((double)t)/((double)p)*2*M_PI)
#define SET_POINT(p,a,b)        ({(p).x=(a); (p).y=(b);})

/* 2D point */
struct pln_point {
        double x;
        double y;
};

/* parameters of line */
struct pln_line {
        double k;       /* slope */
        double q;       /* y-intercept */
};

/* complex variable */
struct pln_compl {
        double real;
        double imag;
};

/* parameters of circle */
struct pln_circle {
        double x0;      /* center */
        double y0;      /* center */
        double radius;  /* radius */
};

/* parameters of the quadratic equation with 2 variables */
/* ax*X^2 + bx*X + cx + ay*Y^2 + by*Y + cy = r */
struct pln_fquad_2var {
        double ax;
        double cx;
        double bx;
        double ay;
        double by;
        double cy;
        double rs;
};

/* parameters of the quadratic equation */
struct pln_fquad {
        double a;
        double b;
        double c;
};

/* used to count the circle */
struct pln_circ {
        double angle;
        /* 3 already known points */
        struct pln_point pl;
        struct pln_point pc;
        struct pln_point pr;
};

/* position state */
struct pln_pos_state {
        double x;
        double y;
        double head;
};

/* time periods and enumerated angles from the laser */
struct pln_time_state {
        /* reflection time (absolute to the refrence point) */
        double t_theta[3];
        double period;
};

/* absolute angles between head and reflectors */
struct pln_aangle_state {
        double theta[3];
        double head;
};

/* relative angles between reflectors */
struct pln_angle_state {
        double alfa;
        double beta;
        double gama;
};

/* angle and it`s evaluation */
struct pln_angle_eval {
        double angle;
        double eval;
};

/* fixed points */
struct pln_point A, B, C, D, E, F, G, H, K, L, R1, R2, R3;

double evaluate_gaussian(double val, double sigma);
void pln_get_position(struct pln_angle_state as, struct pln_pos_state *pos);
void pln_set_points();
void pln_pos2ang(struct pln_pos_state ps, struct pln_aangle_state *aas);
void pln_coordinate(struct pln_pos_state estpos, struct pln_aangle_state aas, 
                        struct pln_angle_state *as);
void pln_sel_angles(struct pln_pos_state estpos, double *angles, 
                        int angcount, struct pln_aangle_state *maas);
void pln_cal_position(unsigned int *times, unsigned int timecnt, 
                        struct pln_pos_state act_pos, 
                        struct pln_pos_state *est_pos);

---