main.cc

Go to the documentation of this file.
00001 /*
00002  * Follow wall
00003  */
00004 
00005 #include <libplayerc++/playerc++.h>
00006 #include <iostream>
00007 #include <scorpion.h>
00008 
00009 using namespace PlayerCc;
00010 
00011 PlayerClient robot("localhost");
00012 Position2dProxy pp(&robot, 0);
00013 IrProxy ir(&robot, 0);
00014 
00015 enum direction {
00016              NORTH,
00017          NWEST,  NEAST,
00018        WEST,        EAST,
00019          SWEST,  SEAST,
00020              SOUTH,
00021 };
00022 
00023 enum { LEFT, MIDDLE, RIGHT };
00024 
00025 struct side {
00026    int direction;
00027    int sensors[3];
00028 };
00029 
00030 static struct side sides[] = {
00031    {  NORTH, { SCORPION_IR_TE_NNW,  SCORPION_IR_BN_N, SCORPION_IR_TW_NNE } },
00032    {  EAST,  { SCORPION_IR_BW_S, SCORPION_IR_BS_W, SCORPION_IR_BN_WNW } },
00033    {  WEST,  { SCORPION_IR_BE_S, SCORPION_IR_BS_E, SCORPION_IR_BN_ENE } },
00034    {  SOUTH, { SCORPION_IR_BW_S, SCORPION_IR_BS_E, SCORPION_IR_BN_ENE } },
00035 #if 0
00036 enum { RIGHT_DIRECTION = -1 , LEFT_DIRECTION = 1 };
00037 
00038    int back[4];
00039    int side[4];
00040    int diag[4];
00041    int front[4];
00042    {
00043       { SCORPION_IR_BE_S,  -1 },
00044       { SCORPION_IR_BS_E,  -1 },
00045       { SCORPION_IR_BN_ENE,   -1 },
00046       { SCORPION_IR_BN_NE, SCORPION_IR_TE_NNE, -1 },
00047       RIGHT_DIRECTION,
00048    },
00049    {
00050       { SCORPION_IR_BW_S,  -1 },
00051       { SCORPION_IR_BS_W,  -1 },
00052       { SCORPION_IR_BN_WNW,   -1 },
00053       { SCORPION_IR_BN_NW, SCORPION_IR_TW_NNW, -1 },
00054       LEFT_DIRECTION,
00055    },
00056    /* EAST */
00057    {  NEAST, { SCORPION_IR_BN_N, SCORPION_IR_TE_NNW, SCORPION_IR_TW_NNE } },
00058 #endif
00059 };
00060 
00061 enum behaviour {
00062    FIND_WALL,
00063    FACE_WALL,
00064    FOLLOW_WALL,
00065    FOLLOW_MIDDLE,
00066 };
00067 
00068 enum behaviour behaviour = FIND_WALL;
00069 player_pose_t pose;
00070 
00071 static double
00072 side_avg(enum direction direction)
00073 {
00074    int *sensors = sides[direction].sensors;
00075 
00076    return (ir[sensors[LEFT]] + ir[sensors[MIDDLE]] + ir[sensors[RIGHT]]) / 3;
00077 }
00078 
00079 static double
00080 has_obstacles(enum direction direction)
00081 {
00082    return side_avg(direction) < 0.75;
00083 }
00084 
00085 int main(int argc, char** argv)
00086 {
00087    /* Go into read-think-act loop */
00088    while (true) {
00089       bool turn = false;
00090       bool speed = false;
00091 
00092       robot.Read();
00093 
00094       printf("[ n=%0.2f e=%0.2f w=%0.2f s=%0.2f ] ",
00095          has_obstacles(NORTH),
00096          has_obstacles(EAST),
00097          has_obstacles(WEST),
00098          has_obstacles(SOUTH));
00099 
00100       continue;
00101 
00102 #if 0
00103       switch (behaviour) {
00104       case FIND_WALL:
00105          puts("find wall");
00106          if (has_obstacles_side(wall)) {
00107             behaviour = FOLLOW_WALL;
00108 
00109          } else if (has_obstacles_side(front) ||
00110                has_obstacles_side(other)) {
00111             behaviour = FACE_WALL;
00112 
00113          } else {
00114             speed = true;
00115          }
00116          break;
00117 
00118       case FACE_WALL:
00119          puts("face wall");
00120          if (has_obstacles_side(front)) {
00121             turn = true;
00122 
00123          } else if (has_obstacles(wall->side)) {
00124             behaviour = FOLLOW_WALL;
00125 
00126          } else if (has_obstacles_side(wall) ||
00127                has_obstacles_side(other)) {
00128             turn = true;
00129 
00130          } else {
00131             behaviour = FIND_WALL;
00132          }
00133          break;
00134 
00135       case FOLLOW_WALL:
00136          puts("follow wall");
00137 
00138          if (!has_obstacles_side(front) &&
00139              has_obstacles_side(wall)) {
00140 
00141             if (has_obstacles(wall->front)) {
00142                turn = true;
00143 
00144             } else if (has_obstacles(wall->diag)) {
00145                turn = true;
00146                speed = true;
00147 
00148             } else if (has_obstacles(wall->side)) {
00149                speed = true;
00150 
00151             } else {
00152                turn = true;
00153             }
00154          } else {
00155             behaviour = FACE_WALL;
00156          }
00157          break;
00158 
00159       case FOLLOW_MIDDLE:
00160          puts("follow middle");
00161       }
00162 #endif
00163 
00164       
00165       if (turn) {
00166          int direction;
00167 
00168 #if 0
00169          if (has_obstacles_side(front)) {
00170             direction = other->turn;
00171 
00172          } else if (has_obstacles(wall->side)) {
00173             if (has_obstacles(wall->front)) {
00174                pose.pa = DTOR(40) * 0.5 * other->turn;
00175 
00176             if (has_obstacles(wall->back)) {
00177             pose.pa = DTOR(40) * 0.5 * other->turn;
00178             behaviour = FOLLOW_WALL;
00179             direction = 0;
00180 
00181          } else if (has_obstacles_side(other)) {
00182          }
00183 #endif
00184          pose.pa = DTOR(40) * 0.5 * direction;
00185       } else {
00186          pose.pa = 0;
00187       }
00188 
00189       if (speed)
00190          pose.px = 0.20;
00191       else
00192          pose.px = 0;
00193 
00194       pp.SetSpeed(pose);
00195    }
00196 }

Generated on Sat Jan 13 03:11:09 2007 for ERSP Player driver by  doxygen 1.5.1