main.cc

Go to the documentation of this file.
00001 #include <libplayerc++/playerc++.h>
00002 #include <iostream>
00003 #include <args.h>
00004 #include <scorpion.h>
00005 using namespace PlayerCc;
00006 
00007 int main(int argc, char** argv)
00008 {
00009   // Parse input arguments
00010   parse_args(argc,argv);
00011 
00012    /* Setting values for driving and avoiding */
00013    // Allowed minimum distances to obstacles (meters)
00014    double mindistance = 0.70;
00015    
00016    // Speed and turn settings
00017    double speed = 0.200;
00018    double avoidspeed = 0;
00019    double turnrate = DTOR(40);
00020    
00021    // Changing speed and turn when avoiding
00022    double newturnrate=0.0f, newspeed=0.0f;
00023    
00024    // avoidcount counts for how many iteration the avoid face takes
00025    // start at 0, but when avoiding is set to AVOIDCOUNT.
00026    #define AVOIDCOUNT 15
00027    int avoidcount = 0;
00028    // True if obstacles closer than mindistance in the front
00029    bool obs = false;
00030    
00031    // Randomization integers
00032    int randint, randint2;
00033    int randcount = 0;
00034    // This set how for how many iteration robot drives with a certain turnrate
00035    // when there is no obstacles
00036    #define RANDCOUNT 20
00037    
00038   // libplayerc++ throws a PlayerError exception when
00039   // it runs into trouble
00040    // The PlayerError is often more readable...
00041    try
00042    {
00043      PlayerClient    robot(gHostname, gPort);
00044      Position2dProxy pp(&robot, gIndex);
00045      IrProxy ir(&robot, gIndex);
00046      
00047       /* Go into read-think-act loop */
00048      for(;;)
00049      { 
00050          // read from proxies
00051        robot.Read();
00052       
00053          /* See if there is an obstacle in front */
00054        obs = (
00055                      // Front ?
00056                      (ir.GetRange(SCORPION_IR_BN_N) < mindistance) ||
00057                      // On top of wheels looking forward 
00058                      // ... from left to the middle
00059                      (ir.GetRange(SCORPION_IR_TW_NNE) < mindistance) ||
00060                      // ... from right to the middle
00061                      (ir.GetRange(SCORPION_IR_TE_NNE) < mindistance) ||
00062                      // In the bumpers left side looking forward to the right
00063                      (ir.GetRange(SCORPION_IR_BN_NE) < mindistance) ||
00064                      // In the bumpers right side looking forward to the left
00065                      (ir.GetRange(SCORPION_IR_BN_NW) < mindistance) ||
00066                // Behind the wheels looking forward ...
00067                      // ... left side looking little left to outside of the robot
00068                      (ir.GetRange(SCORPION_IR_TW_NNW) < mindistance) ||
00069                      // ... right side looking little right to outside of the robot
00070                      (ir.GetRange(SCORPION_IR_TE_NNE) < mindistance));
00071          
00072          // Print sensor data detecting the obstacles
00073          if(obs && gDebug)
00074          {
00075             printf("=== FRONT === one of the %i sensors measure less than %f meters\n", ir.GetCount(), mindistance);
00076             for (int i = 0; i < ir.GetCount(); i++) {
00077                double measure = ir[i];
00078                if (measure < mindistance)
00079                   printf("%-10s -> %f\n", scorpion_ir_range_name(i), measure);
00080             }
00081          }
00082          
00083          /* Start or continue avoiding */
00084        if(obs || avoidcount ) // obstacles found phase...
00085          {
00086          newspeed = avoidspeed;
00087    
00088          // Once we start avoiding, continue avoiding for AVOIDCOUNT iterations
00089          if(!avoidcount)
00090          {
00091            avoidcount = AVOIDCOUNT;
00092            randcount = 0;
00093                
00094                // If obstacles - turn the way in which there is the "most space"
00095                if(obs) 
00096                {
00097             // Less space left than right?
00098                if(   ir.GetRange(SCORPION_IR_BS_W) + ir.GetRange(SCORPION_IR_BN_WNW) 
00099                         < 
00100                         ir.GetRange(SCORPION_IR_BS_E) + ir.GetRange(SCORPION_IR_BN_ENE))
00101                   {
00102                newturnrate = -turnrate; // more space right, then turn right
00103                      if(gDebug) printf("=== LEFT === sensors measure less space on the left:\n");
00104                      if(gDebug) printf("%-10s -> %f\n", scorpion_ir_range_name(SCORPION_IR_BS_W), ir.GetRange(SCORPION_IR_BS_W));
00105                      if(gDebug) printf("%-10s -> %f\n", scorpion_ir_range_name(SCORPION_IR_BN_WNW), ir.GetRange(SCORPION_IR_BN_WNW));
00106                   }
00107                   else
00108                   {
00109                newturnrate = turnrate;  // else turn left
00110                      if(gDebug) printf("=== RIGHT === sensors measure less space on the right:\n");
00111                      if(gDebug) printf("%-10s -> %f\n", scorpion_ir_range_name(SCORPION_IR_BS_E), ir.GetRange(SCORPION_IR_BS_E));
00112                      if(gDebug) printf("%-10s -> %f\n", scorpion_ir_range_name(SCORPION_IR_BN_ENE), ir.GetRange(SCORPION_IR_BN_ENE));
00113                   }
00114                }
00115             }
00116          avoidcount--;
00117        } // end obstacle found phase
00118        else //no obstacles phase
00119        {
00120          avoidcount = 0;
00121          newspeed = speed;
00122    
00123          // Random walk -  update turnrate every RANDCOUNT iteration seconds
00124          if(!randcount)
00125          {
00126            // make random int tween -20 and 20
00127            randint = rand() % 41 - 20;
00128    
00129            newturnrate = dtor(randint);
00130            randcount = RANDCOUNT;
00131           }
00132           randcount--;
00133        } // end no obstacles phase
00134          
00135          // End thinking about what to do...
00136        // write commands to robot
00137        pp.SetSpeed(newspeed, newturnrate);
00138      }
00139    }
00140    catch (PlayerCc::PlayerError e)
00141   {
00142     std::cerr << e << std::endl;
00143     return -1;
00144   }
00145 }

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